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

Changes that covered the hang and wrist touching bar during auto

Browse Source
This commit is contained in:
carlos
2025-02-06 16:50:13 -08:00
parent 47facdde5e
commit 2974904109
4 changed files with 33 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java
View File

@ -43,6 +43,7 @@ public class SpecimenAuto extends OpMode {
wrist.InitAuto(); wrist.InitAuto();
arm.initAuto(); arm.initAuto();
follower.setMaxPower(.45); follower.setMaxPower(.45);
@ -50,42 +51,49 @@ public class SpecimenAuto extends OpMode {
@Override @Override
public void loop() { public void loop() {
if(runtime != null){
telemetry.addData("Runtime (seconds)", runtime.seconds());
}
telemetry.addData("state", state); telemetry.addData("state", state);
switch(state) { switch(state) {
case 0: case 0:
runtime.reset();
wrist.toSpecimenPrep(); wrist.toSpecimenPrep();
arm.toSpecimenPrep(); arm.toSpecimenPrep();
state = 1; if(runtime.seconds() > 0.25){state = 1;}
new SleepAction(5);
break; break;
case 1: case 1:
state = 2; //line 1
if(runtime.seconds() > 3){state = 2;}
break; break;
case 2: case 2:
new SleepAction(5); lift.toSpecimanHangHeight();
state = 3; if(runtime.seconds() > 3.75){state = 3;}
break; break;
case 3: case 3:
wrist.toSpecimenHang();
new SleepAction(5); if(runtime.seconds() > 4){state = 4;}
state = 4;
break; break;
case 4: case 4:
lift.toSpecimanReleaseHeight();
new SleepAction(5); if(runtime.seconds() > 4.5){state = 5;}
state = 5;
break; break;
case 5: case 5:
//specimen drop claw.switchState();
if(runtime.seconds() > 4.65){state = 6;}
break; break;
case 6: case 6:
//path 4 lift.toFloorPosition();
if(runtime.seconds() > 4.75){state = 7;}
break; break;
case 7: case 7:
//specimen pick up arm.toParkPosition();
wrist.toTravelPosition();
//line 2
if(runtime.seconds() > 0.){state = 8;}
break; break;
case 8: case 8:
//path 5 if(runtime.seconds() > 0.25){state = 9;}
break; break;
case 9: case 9:
//specimen drop //specimen drop

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java
View File

@ -13,7 +13,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "Specimen Test", group = "Competition") @Autonomous(name = "Specimen Line Test", group = "Competition")
public class SpecimenAutoLineTest extends OpMode { public class SpecimenAutoLineTest extends OpMode {
private Telemetry telemetryA; private Telemetry telemetryA;
@ -40,15 +40,15 @@ public class SpecimenAutoLineTest extends OpMode {
new Point(39.500, 60.000, Point.CARTESIAN) new Point(39.500, 60.000, Point.CARTESIAN)
) )
) )
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0))
/* .addPath( .addPath(
// Line 2 // Line 2
new BezierLine( new BezierLine(
new Point(39.500, 60.000, Point.CARTESIAN), new Point(39.500, 60.000, Point.CARTESIAN),
new Point(37.000, 60.000, Point.CARTESIAN) new Point(37.000, 60.000, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(180)) .setConstantHeadingInterpolation(Math.toRadians(0))
.addPath( .addPath(
// Line 3 // Line 3
new BezierCurve( new BezierCurve(
@ -58,7 +58,7 @@ public class SpecimenAutoLineTest extends OpMode {
new Point(58.000, 23.500, Point.CARTESIAN) new Point(58.000, 23.500, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(180)) .setConstantHeadingInterpolation(Math.toRadians(0))
.addPath( .addPath(
// Line 4 // Line 4
new BezierLine( new BezierLine(
@ -66,7 +66,7 @@ public class SpecimenAutoLineTest extends OpMode {
new Point(14.000, 23.500, Point.CARTESIAN) new Point(14.000, 23.500, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(180)) .setConstantHeadingInterpolation(Math.toRadians(0))
.addPath( .addPath(
// Line 5 // Line 5
new BezierLine( new BezierLine(
@ -155,7 +155,7 @@ public class SpecimenAutoLineTest extends OpMode {
new Point(37.000, 68.000, Point.CARTESIAN) new Point(37.000, 68.000, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(180))*/.build(); .setConstantHeadingInterpolation(Math.toRadians(180)).build();
follower.followPath(path); follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());

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

@ -118,7 +118,7 @@ public class CometBotDriveV2 extends OpMode {
moveToPark(); moveToPark();
break; break;
case 99: case 99:
wrist.toTravelPosition(); wrist.toFloorPosition();
break; break;
default: default:
telemetry.addLine("default"); telemetry.addLine("default");

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HangMotorSubsystem.java
View File

@ -22,10 +22,10 @@ public class HangMotorSubsystem {
} }
public void init(){ public void init(){
//init hook, set runmodes //init hook, set runmodes
hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); //hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
hang.setDirection(DcMotorSimple.Direction.FORWARD); hang.setDirection(DcMotorSimple.Direction.FORWARD);
hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
hang.setMode(DcMotor.RunMode.RUN_USING_ENCODER); hang.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
} }
public void hangRobot(){ public void hangRobot(){