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

Compare commits

base: SCDS:branch-rc-chassis-14493-new-subsystem-actions
Branches Tags
SCDS:master SCDS:branch-swiss-cheese SCDS:branch-rc-chassis-14493-subsystem-actions SCDS:branch-silver-14493-v2 SCDS:branch-rc-chassis-14493-new-subsystem-actions SCDS:branch-silver-14493 SCDS:branch-rc-chassis-14493-subsystem SCDS:branch-rc-chassis-14493 SCDS:branch-bobito-14493 SCDS:branch-black-14493 SCDS:branch-pedro-led-light SCDS:branch-asher SCDS:branch-pedro-into-the-deep SCDS:branch-alex SCDS:branch-carlos SCDS:branch-into-the-deep
...
compare: SCDS:85060159d898f7d30931f56f24f688bd221f84bc
Branches Tags
SCDS:branch-swiss-cheese SCDS:branch-rc-chassis-14493-subsystem-actions SCDS:branch-silver-14493-v2 SCDS:branch-rc-chassis-14493-new-subsystem-actions SCDS:branch-silver-14493 SCDS:branch-rc-chassis-14493-subsystem SCDS:branch-rc-chassis-14493 SCDS:branch-bobito-14493 SCDS:branch-black-14493 SCDS:branch-pedro-led-light SCDS:branch-asher SCDS:branch-pedro-into-the-deep SCDS:branch-alex SCDS:branch-carlos SCDS:branch-into-the-deep SCDS:master

11 Commits
branch-rc- ... 85060159d8

Author SHA1 Message Date
carlos
85060159d8 Fixed floor position issue 2024-12-29 11:06:15 -08:00
carlos
e70e853ac7 Merge remote-tracking branch 'origin/branch-rc-chassis-14493-subsystem-actions' into branch-rc-chassis-14493-subsystem-actions 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java
 2024-12-29 10:42:00 -08:00
carlos
dbc53bdcd0 Blue basket auto comp 2 updates 2024-12-19 15:56:45 -08:00
robotics1
9adcba68c9 competition code updates for blue basket auto. Move to blue basket, put block in basket, park. 2024-12-13 20:39:49 -08:00
carlos
2720d596b5 Blue basket auto comp 2 updates 2024-12-13 16:42:29 -08:00
robotics1
2efe4229e9 Hanging code! 2024-12-12 18:19:35 -08:00
robotics1
f80026465b Basket Auto code 2024-12-12 17:30:06 -08:00
robotics1
1bf475772c True False skyhook code 2024-12-12 17:15:16 -08:00
robotics1
46dac51c28 Auto code 2024-12-12 13:47:16 -08:00
robotics1
fe39ffda11 Auto code 2024-12-11 10:41:04 -08:00
carlos
9320f0b286 Merge pull request 'Fixed naming convention' (#4) from branch-rc-chassis-14493-new-subsystem-actions into branch-rc-chassis-14493-subsystem-actions 
Reviewed-on: #4
 2024-12-10 16:58:47 -08:00
14 changed files with 399 additions and 426 deletions
Expand all files Collapse all files

224
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java
View File

@ -2,10 +2,15 @@ package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.SleepAction;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
import org.firstinspires.ftc.teamcode.configs.RobotConstants;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
@ -13,184 +18,81 @@ 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;
import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
@Autonomous(name = "Auto Test", group = "Dev")
@Autonomous(name = "Auto Test Competition", group = "Dev")
public class BlueBasketAuto extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private int state;
private PathChain path;
private HighBasketAutoPath1 path1;
private HighBasketAutoPath2 path2;
private AutoPark pathPark;
private final Pose startPose = new Pose(8, 65);
private CometBotTeleopCompetition comp;
private ElapsedTime runtime;
private LiftActionsSubsystem liftActionsSubsystem;
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
path1 = new HighBasketAutoPath1();
path2 = new HighBasketAutoPath2();
pathPark = new AutoPark();
follower.setStartingPose(startPose);
comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
comp.initCloseClaw();
runtime = new ElapsedTime();
state = 0;
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
new Point(8.000, 65.000, Point.CARTESIAN),
new Point(33.000, 65.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierLine(
new Point(33.000, 65.000, Point.CARTESIAN),
new Point(31.000, 65.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierCurve(
new Point(31.000, 65.000, Point.CARTESIAN),
new Point(26.000, 32.000, Point.CARTESIAN),
new Point(60.000, 34.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
// Line 4
new BezierLine(
new Point(60.000, 34.000, Point.CARTESIAN),
new Point(60.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 5
new BezierLine(
new Point(60.000, 24.000, Point.CARTESIAN),
new Point(14.500, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 6
new BezierLine(
new Point(14.500, 24.000, Point.CARTESIAN),
new Point(18.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 1
new BezierCurve(
new Point(18.000, 24.000, Point.CARTESIAN),
new Point(18.000, 67.000, Point.CARTESIAN),
new Point(31.000, 67.000, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 8
new BezierLine(
new Point(31.000, 67.500, Point.CARTESIAN),
new Point(33.000, 67.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierLine(
new Point(33.000, 67.500, Point.CARTESIAN),
new Point(31.000, 67.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 10
new BezierCurve(
new Point(31.000, 67.500, Point.CARTESIAN),
new Point(26.000, 32.000, Point.CARTESIAN),
new Point(60.000, 34.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
// Line 11
new BezierLine(
new Point(60.000, 34.000, Point.CARTESIAN),
new Point(60.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 12
new BezierLine(
new Point(60.000, 12.000, Point.CARTESIAN),
new Point(12.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 13
new BezierLine(
new Point(12.000, 12.000, Point.CARTESIAN),
new Point(20.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 14
new BezierLine(
new Point(20.000, 12.000, Point.CARTESIAN),
new Point(20.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 15
new BezierLine(
new Point(20.000, 24.000, Point.CARTESIAN),
new Point(14.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 16
new BezierCurve(
new Point(14.000, 24.000, Point.CARTESIAN),
new Point(14.000, 70.000, Point.CARTESIAN),
new Point(31.000, 70.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0))
.addPath(
// Line 17
new BezierLine(
new Point(31.000, 70.000, Point.CARTESIAN),
new Point(33.000, 70.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 18
new BezierLine(
new Point(33.000, 70.000, Point.CARTESIAN),
new Point(31.000, 70.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
switch(state) {
case 0:
telemetry.addData("case0", "case0");
path1.moveToPath1(follower);
state = 1;
runtime.reset();
case 1:
if (runtime.seconds() > 5) {
telemetry.addData("case1", "case1");
new SleepAction(.5);
comp.highBucketDropAuto();
state = 2;
}
case 2:
if (runtime.seconds() > 15) {
telemetry.addData("case2", "case2");
new SleepAction(.5);
path2.moveToPath1(follower);
//For next time, add encoder control to skyhook and extend here
//comp.moveSkyHook();
//pathPark.moveToPark(follower);
state = 5;
}
default:
//System.out.println("default");
//telemetry.addData("default", "default");
//telemetry.update();
}
telemetry.update();
follower.update();
follower.telemetryDebug(telemetryA);
//follower.telemetryDebug(telemetry);
}
}

195
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop.java
View File

@ -1,195 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "Auto Test", group = "Dev")
public class BlueBasketAutoWithDrop extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(8, 65);
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
new Point(8.000, 65.000, Point.CARTESIAN),
new Point(33.000, 65.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierLine(
new Point(33.000, 65.000, Point.CARTESIAN),
new Point(31.000, 65.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierCurve(
new Point(31.000, 65.000, Point.CARTESIAN),
new Point(26.000, 32.000, Point.CARTESIAN),
new Point(60.000, 34.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
// Line 4
new BezierLine(
new Point(60.000, 34.000, Point.CARTESIAN),
new Point(60.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 5
new BezierLine(
new Point(60.000, 24.000, Point.CARTESIAN),
new Point(14.500, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 6
new BezierLine(
new Point(14.500, 24.000, Point.CARTESIAN),
new Point(18.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 1
new BezierCurve(
new Point(18.000, 24.000, Point.CARTESIAN),
new Point(18.000, 67.000, Point.CARTESIAN),
new Point(31.000, 67.000, Point.CARTESIAN)
)
)
.setTangentHeadingInterpolation()
.addPath(
// Line 8
new BezierLine(
new Point(31.000, 67.500, Point.CARTESIAN),
new Point(33.000, 67.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierLine(
new Point(33.000, 67.500, Point.CARTESIAN),
new Point(31.000, 67.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 10
new BezierCurve(
new Point(31.000, 67.500, Point.CARTESIAN),
new Point(26.000, 32.000, Point.CARTESIAN),
new Point(60.000, 34.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
// Line 11
new BezierLine(
new Point(60.000, 34.000, Point.CARTESIAN),
new Point(60.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 12
new BezierLine(
new Point(60.000, 12.000, Point.CARTESIAN),
new Point(12.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 13
new BezierLine(
new Point(12.000, 12.000, Point.CARTESIAN),
new Point(20.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 14
new BezierLine(
new Point(20.000, 12.000, Point.CARTESIAN),
new Point(20.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 15
new BezierLine(
new Point(20.000, 24.000, Point.CARTESIAN),
new Point(14.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 16
new BezierCurve(
new Point(14.000, 24.000, Point.CARTESIAN),
new Point(14.000, 70.000, Point.CARTESIAN),
new Point(31.000, 70.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0))
.addPath(
// Line 17
new BezierLine(
new Point(31.000, 70.000, Point.CARTESIAN),
new Point(33.000, 70.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 18
new BezierLine(
new Point(33.000, 70.000, Point.CARTESIAN),
new Point(31.000, 70.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetryA);
}
}

56
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop1.java Normal file
View File

@ -0,0 +1,56 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "Auto Test1", group = "Dev")
public class BlueBasketAutoWithDrop1 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(8, 89);
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierCurve(
new Point(8.000, 89.000, Point.CARTESIAN),
new Point(24.000, 96.000, Point.CARTESIAN),
new Point(16.000, 128.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetryA);
}
}

57
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop2.java Normal file
View File

@ -0,0 +1,57 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "Auto Test2", group = "Dev")
public class BlueBasketAutoWithDrop2 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(16, 128);
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierCurve(
new Point(16.000, 128.000, Point.CARTESIAN),
new Point(88.000, 140.000, Point.CARTESIAN),
new Point(83.250, 99.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(270)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetryA);
}
}

78
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java
View File

@ -88,6 +88,18 @@ public class CometBotTeleopCompetition {
follower.startTeleopDrive();
}
public void initCloseClaw(){
this.motors.init();
this.hook.init();
this.claw.init();
this.arm.init();
this.wrist.init();
this.lift.init();
this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
follower.setMaxPower(MAX_POWER);
follower.startTeleopDrive();
}
public void update() {
this.previousGP1.copy(currentGP1);
this.currentGP1.copy(this.GP1);
@ -101,8 +113,8 @@ public class CometBotTeleopCompetition {
this.clawControl();
this.decreaseMaxPower();
this.increaseMaxPower();
this.raiseSkyHook();
this.lowerSkyHook();
this.moveSkyHook();
Actions.runBlocking(this.lift.toFloorPosition());
@ -162,6 +174,14 @@ public class CometBotTeleopCompetition {
if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
this.follower.breakFollowing();
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
highBucketDrop();
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
this.follower.startTeleopDrive();
}
}
}
public void highBucketDrop() {
Actions.runBlocking(new SequentialAction(
new SleepAction(.5),
this.lift.toHighBucketPosition(),
@ -178,12 +198,25 @@ public class CometBotTeleopCompetition {
this.lift.toFloorPosition(),
new SleepAction(.5)
));
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
this.follower.startTeleopDrive();
}
public void highBucketDropAuto() {
Actions.runBlocking(new SequentialAction(
new SleepAction(.5),
this.lift.toHighBucketPosition(),
new SleepAction(.5),
this.arm.toBucketPosition(),
new SleepAction(.5),
this.wrist.toBucketPosition(),
new SleepAction(.5),
this.claw.openClaw(),
new SleepAction(.5),
this.wrist.toFloorPosition(),
new SleepAction(.5),
this.arm.toParkPosition(),
this.lift.toZeroPosition(),
new SleepAction(.5)
));
}
}
/*
Type: PS4 / Logitech
Controller: 2
@ -252,6 +285,23 @@ public class CometBotTeleopCompetition {
}
}
public void moveSkyHook() {
if (this.currentGP2.dpad_down) {
hook.moveSkyHook(-1.00);
}
else if (this.currentGP2.dpad_up) {
hook.moveSkyHook(1.00);
}
else{
hook.moveSkyHook(0.00);
}
}
/*
Type: PS4
Controller: 2
@ -294,18 +344,16 @@ public class CometBotTeleopCompetition {
}
}
public void lowerSkyHook() {
hook.raiseHook(currentGP2.left_trigger);
}
public void raiseSkyHook() {
hook.lowerHook(currentGP2.right_trigger * 2.0);
}
}

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

@ -4,8 +4,8 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class RobotConstants {
public final static double clawOpen = 0.5;
public final static double clawClose = 0.05;
public final static double clawClose = 0.8;
public final static double clawOpen = 0.05;
public final static double armFloor = 0.7;
public final static double armSubmarine = 0.55;
@ -20,8 +20,7 @@ public class RobotConstants {
public final static double wristSpeciemen = 0.1;
public final static int liftToFloorPos = 350;
public final static int liftToSubmarinePos = 350;
public final static int liftToFloorPos = 550;
public final static int liftToLowBucketPos = 2650;
public final static int liftToHighRung = 2100;
public final static int dropToHighRung = 1675;

50
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java
View File

@ -929,31 +929,31 @@ public class Follower {
* method will use to output the debug data.
*/
public void telemetryDebug(MultipleTelemetry telemetry) {
telemetry.addData("follower busy", isBusy());
telemetry.addData("heading error", headingError);
telemetry.addData("heading vector magnitude", headingVector.getMagnitude());
telemetry.addData("corrective vector magnitude", correctiveVector.getMagnitude());
telemetry.addData("corrective vector heading", correctiveVector.getTheta());
telemetry.addData("translational error magnitude", getTranslationalError().getMagnitude());
telemetry.addData("translational error direction", getTranslationalError().getTheta());
telemetry.addData("translational vector magnitude", translationalVector.getMagnitude());
telemetry.addData("translational vector heading", translationalVector.getMagnitude());
telemetry.addData("centripetal vector magnitude", centripetalVector.getMagnitude());
telemetry.addData("centripetal vector heading", centripetalVector.getTheta());
telemetry.addData("drive error", driveError);
telemetry.addData("drive vector magnitude", driveVector.getMagnitude());
telemetry.addData("drive vector heading", driveVector.getTheta());
telemetry.addData("x", getPose().getX());
telemetry.addData("y", getPose().getY());
telemetry.addData("heading", getPose().getHeading());
telemetry.addData("total heading", poseUpdater.getTotalHeading());
telemetry.addData("velocity magnitude", getVelocity().getMagnitude());
telemetry.addData("velocity heading", getVelocity().getTheta());
driveKalmanFilter.debug(telemetry);
telemetry.update();
if (drawOnDashboard) {
Drawing.drawDebug(this);
}
// telemetry.addData("follower busy", isBusy());
// telemetry.addData("heading error", headingError);
// telemetry.addData("heading vector magnitude", headingVector.getMagnitude());
// telemetry.addData("corrective vector magnitude", correctiveVector.getMagnitude());
// telemetry.addData("corrective vector heading", correctiveVector.getTheta());
// telemetry.addData("translational error magnitude", getTranslationalError().getMagnitude());
// telemetry.addData("translational error direction", getTranslationalError().getTheta());
// telemetry.addData("translational vector magnitude", translationalVector.getMagnitude());
// telemetry.addData("translational vector heading", translationalVector.getMagnitude());
// telemetry.addData("centripetal vector magnitude", centripetalVector.getMagnitude());
// telemetry.addData("centripetal vector heading", centripetalVector.getTheta());
// telemetry.addData("drive error", driveError);
// telemetry.addData("drive vector magnitude", driveVector.getMagnitude());
// telemetry.addData("drive vector heading", driveVector.getTheta());
// telemetry.addData("x", getPose().getX());
// telemetry.addData("y", getPose().getY());
// telemetry.addData("heading", getPose().getHeading());
// telemetry.addData("total heading", poseUpdater.getTotalHeading());
// telemetry.addData("velocity magnitude", getVelocity().getMagnitude());
// telemetry.addData("velocity heading", getVelocity().getTheta());
// driveKalmanFilter.debug(telemetry);
// telemetry.update();
// if (drawOnDashboard) {
// Drawing.drawDebug(this);
// }
}
/**

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java
View File

@ -14,6 +14,7 @@ import java.util.ArrayList;
* @version 1.0, 3/11/2024
*/
public class PathBuilder {
public PathBuilder setTangentHeadingInterpolation;
private ArrayList<Path> paths = new ArrayList<>();
private ArrayList<PathCallback> callbacks = new ArrayList<>();

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoPark.java Normal file
View File

@ -0,0 +1,36 @@
package org.firstinspires.ftc.teamcode.subsystem;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
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 AutoPark {
public void moveToPark(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
new BezierCurve(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(85.000, 132.750, Point.CARTESIAN),
new Point(84.000, 97.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawActionsSubsystem.java
View File

@ -68,7 +68,7 @@ public class ClawActionsSubsystem {
}
public void init() {
Actions.runBlocking(openClaw());
Actions.runBlocking(closeClaw());
}
public void start() {

38
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java Normal file
View File

@ -0,0 +1,38 @@
package org.firstinspires.ftc.teamcode.subsystem;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
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 HighBasketAutoPath1 {
private final Pose startPose = new Pose(8, 89);
public void moveToPath1(Follower robot) {
PathChain pathChain;
robot.setStartingPose(startPose);
PathBuilder builder = new PathBuilder();
builder
.addPath(
new BezierCurve(
new Point(8.000, 89.000, Point.CARTESIAN),
new Point(24.000, 96.000, Point.CARTESIAN),
new Point(18.000, 126.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
pathChain = builder.build();
robot.followPath(pathChain);
}
}

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java Normal file
View File

@ -0,0 +1,36 @@
package org.firstinspires.ftc.teamcode.subsystem;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
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 HighBasketAutoPath2 {
public void moveToPath1(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 2
new BezierCurve(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(85.000, 132.750, Point.CARTESIAN),
new Point(84.000, 97.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java
View File

@ -6,12 +6,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToSubmarinePos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRung;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRungAttach;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.dropToHighRung;
import androidx.annotation.NonNull;
@ -60,7 +56,7 @@ public class LiftActionsSubsystem {
}
public Action toFloorPosition() {
return new MoveToPosition(liftToSubmarinePos, LiftState.FLOOR);
return new MoveToPosition(liftToFloorPos, LiftState.FLOOR);
}
public Action toHighRung() {
@ -72,12 +68,15 @@ public class LiftActionsSubsystem {
}
public Action toZeroPosition() {
return new MoveToPosition(0, LiftState.FLOOR);
}
public Action toHighRungAttach() {
return new MoveToPosition(liftToHighRungAttach, LiftState.HIGH_RUNG);
}
public Action toLowBucketPosition() {
return new MoveToPosition(liftToLowBucketPos, LiftState.LOW_BUCKET);

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java
View File

@ -25,15 +25,11 @@ public class SkyHookSubsystem {
public void raiseHook(double power){
this.hook.setPower(power);
this.hook.setDirection(DcMotorSimple.Direction.FORWARD);
this.setState(SkyHookState.UP);
}
public void lowerHook(double power){
public void moveSkyHook(double power){
this.hook.setPower(power);
this.hook.setDirection(DcMotorSimple.Direction.REVERSE);
this.setState(SkyHookState.DOWN);
//this.setState(SkyHookState.DOWN);
}
public void init() {