SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Added new paths for autonomous

Browse Source
This commit is contained in:
robotics1
2025-01-26 12:32:43 -08:00
parent fe44a38cdd
commit a36f40be45
7 changed files with 202 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

201
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java
View File

@ -1,35 +1,19 @@
package org.firstinspires.ftc.teamcode.cometbots.paths;
import static com.qualcomm.hardware.lynx.commands.core.LynxSetMotorPIDFControlLoopCoefficientsCommand.InternalMotorControlAlgorithm.Max;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.util.ElapsedTime;
//import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
//import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
//import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3;
//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4;
//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath5;
//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath6;
//import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
//import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
@Autonomous(name = "Auto Test Competition V2", group = "Dev")
public class CometBotDriveV2 extends OpMode {
@ -40,6 +24,8 @@ public class CometBotDriveV2 extends OpMode {
private HighBasketPath2 path2;
private HighBasketPath3 path3;
private HighBasketPath4 path4;
private HighBasketPath5 path5;
private HighBasketPath6 path6;
//private CometBotTeleopCompetition comp;
@ -60,6 +46,8 @@ public class CometBotDriveV2 extends OpMode {
path2 = new HighBasketPath2();
path3 = new HighBasketPath3();
path4 = new HighBasketPath4();
path5 = new HighBasketPath5();
path6 = new HighBasketPath6();
lift = new DualMotorSliderSubsystem(hardwareMap);
arm = new ArmSubsystem(hardwareMap);
wrist = new WristSubsystem(hardwareMap);
@ -91,61 +79,77 @@ public class CometBotDriveV2 extends OpMode {
}
switch (state) {
case 0:
// claw.thumbOutOfTheWay();
runtime.reset();
moveToPathOneAndHighBucket();
break;
case 1:
//doArmThing();
//lift.update();
doArmThing();
state = 2;
break;
case 2:
moveToPathTwoAndPickSampleUp();
//lift.update();
break;
case 3:
// doPickUpThing();
// runtime.reset();
state = 4;
doPickUpThing();
break;
case 4:
moveToBasketPath3();
break;
case 5:
// theArmThing();
state = 6;
theArmThing();
break;
case 6:
moveToPickupAgainPath4();
break;
case 7:
// doPickUpThingAgain();
case 7:
doPickUpThingAgain();
break;
case 8:
moveToPickupAgainPath5();
moveToBasketPath5();
break;
case 9:
theArmThingAgain();
break;
case 10:
moveToParkPath6();
break;
case 11:
moveToPark();
break;
// case 9:
// //theArmThingAgain();
// break;
// case 10:
// //moveToParkPath6();
// break;
// case 11:
default:
telemetry.addLine("default");
}
telemetry.update();
lift.update();
follower.update();
}
public void moveToPark() {
if (runtime.seconds() > 26) {
path6.moveToBasketPath6(follower);
wrist.toTravelPosition();
state = 99;
}
}
private void moveToPathOneAndHighBucket() {
path1.moveToPath1(follower);
state = 1;
}
private void moveToBasketPath5() {
if(runtime.seconds() > 19){
path5.moveToBasketPath5(follower);
lift.toHighBucketReverseDrop();
wrist.toTravelPosition();
state = 9;
}
}
public class SetStateAction implements Action {
@ -168,35 +172,54 @@ public class CometBotDriveV2 extends OpMode {
private void doArmThing() {
claw.thumbOutOfTheWay();
wrist.toTravelPosition();
lift.toHighBucketReverseDrop();
arm.toReverseBucket();
wrist.toReverseBucket();
claw.openClaw();
wrist.toFloorPosition();
//arm.toReverseBucket();
//wrist.toReverseBucket();
//claw.openClaw();
//wrist.toFloorPosition();
state = 2;
}
// private void theArmThing() {
// telemetry.addData("busy?", follower.isBusy());
// telemetry.addData("end?", follower.atParametricEnd());
// if (follower.atParametricEnd()){
// follower.breakFollowing();
// comp.highBucketDropAuto();
// state = 6;
// }
// follower.breakFollowing();
// }
private void theArmThing() {
if(lift.getFixedPosition() >= 4280){
arm.toReverseBucket();
if(runtime.seconds() > 12) {
wrist.toReverseBucket();
}
if(runtime.seconds() > 12.75) {
claw.openClaw();
state = 6;
}
}
}
private void theArmThingAgain() {
if(lift.getFixedPosition() >= 4280){
arm.toReverseBucket();
if(runtime.seconds() > 22) {
wrist.toReverseBucket();
}
if(runtime.seconds() > 23.00) {
claw.openClaw();
state = 10;
}
}
}
// private void theArmThingAgain() {
// follower.breakFollowing();
// comp.highBucketDropAuto();
// state = 10;
// }
private void moveToPathTwoAndPickSampleUp() {
if (runtime.seconds() > 6) {
if (runtime.seconds() > 3.25 && runtime.seconds() < 4) {
wrist.toReverseBucket();
}
if (runtime.seconds() > 3.5) {
claw.openClaw();
}
if (runtime.seconds() > 4) {
wrist.toTravelPosition();
lift.toFloorPosition();
path2.moveToPath2(follower);
state = 3;
}
@ -204,15 +227,19 @@ public class CometBotDriveV2 extends OpMode {
//
private void moveToBasketPath3() {
if (runtime.seconds() > 10)
{
if (runtime.seconds() > 7.25) {
lift.toHighBucketReverseDrop();
path3.moveToBasketPath3(follower);
state = 5;
}
}
private void moveToPickupAgainPath4() {
if (runtime.seconds() > 13) {
wrist.toTravelPosition();
}
if (runtime.seconds() > 14) {
lift.toFloorPosition();
path4.moveToPickupPath4(follower);
state = 7;
}
@ -222,19 +249,17 @@ public class CometBotDriveV2 extends OpMode {
}
// private void moveToParkPath6() {
// if (!followingPath) {
// path6.moveToParkPath6(follower);
// followingPath = true;
// }
// if (runtime != null) {
// telemetry.addData("Runtime (seconds)", runtime.seconds());
// if (follower.atParametricEnd() || runtime.seconds() > 48.0) {
// state = 11;
// followingPath = false;
// }
// }
// }
private void moveToParkPath6() {
if (runtime.seconds() > 24.5) {
arm.toBucketPosition();
wrist.toTravelPosition();
}
if (runtime.seconds() > 25.) {
lift.toFloorPosition();
claw.closeClaw();
state = 11;
}
}
//
// private void moveToBasketPath3() {
// if (!followingPath) {
@ -283,18 +308,24 @@ public class CometBotDriveV2 extends OpMode {
// ));
// }
//
// private void doPickUpThing() {
// follower.breakFollowing();
// thePickUpAuto();
// state = 4;
// }
//
// private void doPickUpThingAgain() {
// follower.breakFollowing();
// thePickUpAuto();
// state = 8;
// }
//
//
private void doPickUpThing() {
wrist.toPickupPosition();
arm.toFloorPosition();
if (runtime.seconds() > 7){
claw.closeClaw();
state = 4;
}
}
private void doPickUpThingAgain() {
wrist.toPickupPosition();
arm.toFloorPosition();
if (runtime.seconds() > 18){
claw.closeClaw();
state = 8;
}
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath2.java
View File

@ -24,7 +24,7 @@ public class HighBasketPath2 {
// Line 1
new BezierLine(
new Point(20.000, 130.000, Point.CARTESIAN),
new Point(26.000, 117.000, Point.CARTESIAN)
new Point(27.000, 117.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java
View File

@ -23,7 +23,7 @@ public class HighBasketPath4 {
// Line 1
new BezierLine(
new Point(22.000, 132.000, Point.CARTESIAN),
new Point(26.000, 127.500, Point.CARTESIAN)
new Point(27.000, 128.500, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath5.java Normal file
View File

@ -0,0 +1,35 @@
package org.firstinspires.ftc.teamcode.cometbots.paths;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class HighBasketPath5 {
public void moveToBasketPath5(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(27.000, 127.000, Point.CARTESIAN),
new Point(22.000, 132.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6.java Normal file
View File

@ -0,0 +1,36 @@
package org.firstinspires.ftc.teamcode.cometbots.paths;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class HighBasketPath6 {
public void moveToBasketPath6(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(27.000, 128.500, Point.CARTESIAN),
new Point(84.00, 100.00, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
View File

@ -21,18 +21,20 @@ public class RobotConstants {
public final static int slideBelowFloor = -150;
public final static int backwardBucketDrop = 4670;
public final static double armBucket = 0.45;
public final static double armInit = 0.135;
public final static double wristInit = 0.25;
public final static double armInit = 0.13;
public final static double wristInit = 0.125;
public final static double wristPickup = 0.475;
public final static double wristBucket = 0.56;
public final static double wristFloor = 0.75;
public final static double wristBackwardBucket = 0.775;
public final static double wristBackwardBucket = 0.725;
public final static double wristRung = 0.55;
public final static double wristSpeciemen = 0.1;
public final static int toBar = 500;
public final static double wristtravel = 0.525;
public final static int toFloor = 0;
public final static int liftToFloorPos = 350;
public final static int liftToSubmarinePos = 350;
@ -43,6 +45,6 @@ public class RobotConstants {
public final static int liftToHighRungAttach = 1050;
public final static int liftToHighBucketPos = 4000;
public final static int liftToHighBucketPos = 4500;
public final static double liftPower = 1;
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java
View File

@ -9,6 +9,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlu
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristInit;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristtravel;
import androidx.annotation.NonNull;
@ -21,7 +22,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
public class WristSubsystem {
public enum WristState {
FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERSE_BUCKET
FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERSE_BUCKET, TRAVEL
}
public ServoImplEx wrist;
@ -97,6 +98,11 @@ public class WristSubsystem {
wrist.setPosition(wristBackwardBucket);
setState(WristState.REVERSE_BUCKET);
}
public void toTravelPosition(){
wrist.setPosition(wristtravel);
setState(WristState.TRAVEL);
}
public WristState getState() {
return this.state;
}