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-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:branch-rc-chassis-14493-subsystem
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

5 Commits
branch-rc- ... branch-rc-

Author SHA1 Message Date
robotics1
2133941dbe Commit 2024-11-12 16:05:10 -08:00
carlos
66a831fa59 Added Hoverstate 2024-11-07 16:23:47 -08:00
robotics1
3f8f6a41f0 Merge remote-tracking branch 'origin/branch-rc-chassis-14493-subsystem' into branch-rc-chassis-14493-subsystem 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java
 2024-11-07 15:54:28 -08:00
robotics1
8d5be574c5 Updated robot speed to 55%. Made High basket go higher 2024-11-07 15:53:15 -08:00
carlos
e5a429c6ae Changing arm controls to be more intuitive 2024-11-07 15:38:33 -08:00
68 changed files with 2750 additions and 2798 deletions
Expand all files Collapse all files

93
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherOrientBlue.java Normal file
View File

@ -0,0 +1,93 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
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;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AsherOrientBlue", group = "Autonomous Pathing Tuning")
public class AsherOrientBlue extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(9.757, 84.983, 90);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.4);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
/*
* Only update this path
*/
.addPath(
// Line 1
new BezierLine(
new Point(20.500, 7.800, Point.CARTESIAN),
new Point(20.500, 87.500, Point.CARTESIAN)
)
)
.addPath(
// Line 2
new BezierLine(
new Point(20.500, 87.500, Point.CARTESIAN),
new Point(7.800, 87.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
/*
* End of only update this path
*/
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

133
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathBlueV1.java Normal file
View File

@ -0,0 +1,133 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
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;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AsherPathBlueV1", group = "Autonomous Pathing Tuning")
public class AsherPathBlueV1 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(9.757, 84.983, 90);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.4);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
/*
* Only update this path
*/
.addPath(
// Line 1
new BezierCurve(
new Point(7.800, 87.5, Point.CARTESIAN),
new Point(19.000, 116.000, Point.CARTESIAN),
new Point(93.000, 118.000, Point.CARTESIAN),
new Point(45.000, 115.000, Point.CARTESIAN)
)
)
.addPath(
// Line 2
new BezierLine(
new Point(45.000, 115.000, Point.CARTESIAN),
new Point(14.000, 126.000, Point.CARTESIAN)
)
)
.addPath(
// Line 3
new BezierCurve(
new Point(14.000, 126.000, Point.CARTESIAN),
new Point(43.000, 112.500, Point.CARTESIAN),
new Point(64.000, 92.000, Point.CARTESIAN),
new Point(77.000, 117.000, Point.CARTESIAN)
)
)
.addPath(
// Line 4
new BezierLine(
new Point(77.000, 117.000, Point.CARTESIAN),
new Point(20.000, 135.000, Point.CARTESIAN)
)
)
.addPath(
// Line 5
new BezierCurve(
new Point(20.000, 135.000, Point.CARTESIAN),
new Point(113.000, 95.000, Point.CARTESIAN),
new Point(69.000, 135.000, Point.CARTESIAN)
)
)
.addPath(
// Line 6
new BezierLine(
new Point(69.000, 135.000, Point.CARTESIAN),
new Point(20.500, 135.000, Point.CARTESIAN)
)
)
.addPath(
// Line 7
new BezierCurve(
new Point(20.500, 135.000, Point.CARTESIAN),
new Point(101.500, 95.500, Point.CARTESIAN),
new Point(72.500, 95.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
/*
* End of only update this path
*/
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

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

@ -1,115 +0,0 @@
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;
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;
import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
@Autonomous(name = "Auto Test Competition", group = "Dev")
public class BlueBasketAuto extends OpMode {
private Follower follower;
private int state;
private HighBasketAutoPath1 path1;
private HighBasketAutoPath2 path2;
private AutoPark pathPark;
private SkyHookSubsystem hook;
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();
comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
comp.initCloseClaw();
runtime = new ElapsedTime();
hook = new SkyHookSubsystem(hardwareMap);
state = 0;
}
@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 SleepAsction(.5);
//path2.moveToPath1(follower);
//For next time, add encoder control to skyhook and extend here
//comp.moveSkyHook();
//pathPark.moveToPark(follower);
state = 3;
}
case 3:
if (runtime.seconds() > 15) {
telemetry.addData("case3", "case3");
hook.toLevel1Position();
state = 4;
}
case 4:
if (runtime.seconds() > 15) {
telemetry.addData("case3", "case3");
hook.toLevel1Position();
state = 4;
}
//System.out.println("default");
//telemetry.addData("default", "default");
//telemetry.update();
}
telemetry.update();
follower.update();
//follower.telemetryDebug(telemetry);
}
}

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

@ -1,56 +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.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
View File

@ -1,57 +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.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);
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CONTROLS.md
View File

@ -1,12 +0,0 @@
# Controller 1
## Motor Controls
- Left Joystick
- Forward & Backwards
- Right Joystick
- Strafe & Turning
## Arm Controls
-

290
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java
View File

@ -1,290 +0,0 @@
package org.firstinspires.ftc.teamcode;
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.pedroPathing.follower.Follower;
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.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 ComeBotDriveDevV2 extends OpMode {
private Follower follower;
private int state;
private HighBasketAutoPath1 path1;
private HighBasketAutoPath2 path2;
private HighBasketAutoPath3 path3;
private HighBasketAutoPath4 path4;
private HighBasketAutoPath5 path5;
private HighBasketAutoPath6 path6;
private AutoPark pathPark;
private SkyHookSubsystem hook;
private CometBotTeleopCompetition comp;
private static ElapsedTime runtime;
private static boolean initalized = false;
private static boolean followingPath = false;
private LiftActionsSubsystem liftActionsSubsystem;
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.75);
path1 = new HighBasketAutoPath1();
path2 = new HighBasketAutoPath2();
path3 = new HighBasketAutoPath3();
path4 = new HighBasketAutoPath4();
path5 = new HighBasketAutoPath5();
path6 = new HighBasketAutoPath6();
pathPark = new AutoPark();
comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
comp.initCloseClaw();
hook = new SkyHookSubsystem(hardwareMap);
state = 0;
}
public void loop() {
telemetry.addData("state", state);
telemetry.addData("followingPath", followingPath);
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
}
switch (state) {
case 0:
moveToPathOneAndHighBucket();
break;
case 1:
doArmThing();
break;
case 2:
moveToPathTwoAndPickSampleUp();
break;
case 3:
doPickUpThing();
break;
case 4:
moveToBasketPath3();
break;
case 5:
theArmThing();
break;
case 6:
moveToPickupAgainPath4();
break;
case 7:
doPickUpThingAgain();
break;
case 8:
moveToPickupAgainPath5();
break;
case 9:
theArmThingAgain();
break;
case 10:
//moveToParkPath6();
break;
case 11:
}
telemetry.update();
follower.update();
}
private void moveToPathOneAndHighBucket() {
if (!followingPath) {
runtime = new ElapsedTime();
path1.moveToPath1(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 4) {
state = 1;
followingPath = false;
}
}
}
public class SetStateAction implements Action {
private int value;
public SetStateAction(int value) {
this.value = value;
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
state = value;
return false;
}
}
private Action setStateValue(int value) {
return new SetStateAction(value);
}
private void doArmThing() {
comp.highBucketDropAuto();
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 theArmThingAgain() {
follower.breakFollowing();
comp.highBucketDropAuto();
state = 10;
}
private void moveToPathTwoAndPickSampleUp() {
if (!followingPath) {
path2.moveToPath2(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 22.0) {
state = 3;
followingPath = false;
}
}
}
private void moveToPickupAgainPath4() {
if (!followingPath) {
path4.moveToPickupAgainPath4(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 27.0) {
state = 7;
followingPath = false;
}
}
}
private void moveToPickupAgainPath5() {
if (!followingPath) {
path5.moveToPickupAgainPath5(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 36.0) {
state = 9;
followingPath = false;
}
}
}
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 moveToBasketPath3() {
if (!followingPath) {
path3.moveToBasketPath3(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 10.0) {
state = 5;
followingPath = false;
}
}
}
private void thePickUpAuto() {
Actions.runBlocking(new SequentialAction(
new SleepAction(.1),
comp.claw.openClaw(),
new SleepAction(.2),
comp.wrist.toPickupPosition(),
new SleepAction(.2),
comp.arm.toSubmarinePosition(),
new SleepAction(.5),
comp.claw.closeClaw(),
new SleepAction(.3),
comp.wrist.toFloorPosition(),
new SleepAction(.2),
comp.arm.toParkPosition(),
new SleepAction(.2)
));
}
private void thePickUp() {
Actions.runBlocking(new SequentialAction(
new SleepAction(.5),
comp.wrist.toPickupPosition(),
new SleepAction(.5),
comp.arm.toSubmarinePosition(),
new SleepAction(.7),
comp.claw.closeClaw(),
new SleepAction(.7),
comp.wrist.toFloorPosition(),
new SleepAction(.5),
comp.arm.toParkPosition(),
new SleepAction(.5)
));
}
private void doPickUpThing() {
follower.breakFollowing();
thePickUpAuto();
state = 4;
}
private void doPickUpThingAgain() {
follower.breakFollowing();
thePickUpAuto();
state = 8;
}
}

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java Normal file
View File

@ -0,0 +1,35 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.runmodes.Auto;
@Autonomous(name = "CometBot Auto", group = "Debug")
public class CometBotAuto extends OpMode {
public Auto auto;
@Override
public void init() {
auto = new Auto(hardwareMap, telemetry, new Follower(hardwareMap));
}
@Override
public void start() {
auto.start();
}
@Override
public void loop() {
auto.update();
telemetry.addData("Arm State", auto.arm.getState());
telemetry.addData("Arm Position", auto.arm.getPosition());
telemetry.addData("Claw State", auto.claw.getState());
telemetry.addData("Wrist State", auto.wrist.getState());
telemetry.addData("Wrist Position", auto.wrist.getPosition());
telemetry.update();
}
}

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java
View File

@ -1,24 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment;
@TeleOp(name = "CometBot Auto", group = "Development")
public class CometBotDevAuto extends OpMode {
public CometBotAutoDevelopment runMode;
@Override
public void init() {
this.runMode = new CometBotAutoDevelopment(hardwareMap, telemetry, gamepad1, gamepad2);
this.runMode.init();
}
@Override
public void loop() {
this.runMode.update();
telemetry.update();
}
}

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDrive.java
View File

@ -1,24 +1,31 @@
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.blueBucketStartPose;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.runmodes.Teleop;
@TeleOp(name = "ComeBot Drive", group = "Competition")
@TeleOp(name="ComeBot Drive", group="Debug")
@Disabled
public class CometBotDrive extends OpMode {
public CometBotTeleopCompetition runMode;
private Teleop teleop;
@Override
public void init() {
this.runMode = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
this.runMode.init();
teleop = new Teleop(hardwareMap,
telemetry,
new Follower(hardwareMap),
gamepad1);
teleop.start();
}
@Override
public void loop() {
this.runMode.update();
telemetry.update();
teleop.update();
}
}
}

97
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java Normal file
View File

@ -0,0 +1,97 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@TeleOp(name = "Dev Teleop Remix", group = "Debug")
@Disabled
public class DevTeleOpRemix extends OpMode {
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public LiftSubsystem lift;
public MotorsSubsystem motors;
public Gamepad currentGamepad1;
public Gamepad previousGamepad1;
public Gamepad currentGamepad2;
public Gamepad previousGamepad2;
public double power = .6;
@Override
public void init() {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
lift = new LiftSubsystem(hardwareMap);
motors = new MotorsSubsystem(hardwareMap, telemetry, power);
claw.init();
arm.init();
wrist.init();
lift.init();
motors.init();
currentGamepad1 = new Gamepad();
previousGamepad1 = new Gamepad();
currentGamepad2 = new Gamepad();
previousGamepad2 = new Gamepad();
}
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.a && !previousGamepad1.a) {
wrist.floorWrist();
arm.engageArm();
}
}
public void thePickup(ClawSubsystem claw) {
if (currentGamepad1.x && !previousGamepad1.x) {
claw.switchState();
}
}
public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.b && !previousGamepad1.b) {
arm.parkArm();
wrist.bucketWrist();
}
}
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
if (currentGamepad1.y && !previousGamepad1.y) {
lift.toLowBucket();
wrist.bucketWrist();
}
}
@Override
public void loop() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
theDrop(arm, wrist);
thePickup(claw);
theLift(arm, wrist);
theLowBucketScore(lift, wrist, arm);
motors.calculateTrajectory(gamepad1);
}
}

107
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java Normal file
View File

@ -0,0 +1,107 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@TeleOp(name = "Dev Teleop Remix Deux", group = "Debug")
@Disabled
public class DevTeleOpRemixDeux extends OpMode {
private Follower follower;
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public LiftSubsystem lift;
public MotorsSubsystem motors;
public Gamepad currentGamepad1;
public Gamepad previousGamepad1;
public Gamepad currentGamepad2;
public Gamepad previousGamepad2;
public double power = .6;
@Override
public void init() {
follower = new Follower(hardwareMap);
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
motors = new MotorsSubsystem(hardwareMap, telemetry);
lift = new LiftSubsystem(hardwareMap);
claw.init();
arm.init();
wrist.init();
lift.init();
motors.init();
currentGamepad1 = new Gamepad();
previousGamepad1 = new Gamepad();
currentGamepad2 = new Gamepad();
previousGamepad2 = new Gamepad();
follower.setMaxPower(this.power);
follower.startTeleopDrive();
}
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.a && !previousGamepad1.a) {
wrist.floorWrist();
arm.engageArm();
}
}
public void thePickup(ClawSubsystem claw) {
if (currentGamepad1.x && !previousGamepad1.x) {
claw.switchState();
}
}
public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.b && !previousGamepad1.b) {
arm.parkArm();
wrist.bucketWrist();
}
}
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
if (currentGamepad1.y && !previousGamepad1.y) {
lift.toLowBucket();
wrist.bucketWrist();
}
}
@Override
public void loop() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
theDrop(arm, wrist);
thePickup(claw);
theLift(arm, wrist);
theLowBucketScore(lift, wrist, arm);
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
}
}

196
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java Normal file
View File

@ -0,0 +1,196 @@
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
import android.graphics.Point;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@TeleOp(name = "Dev Teleop", group = "Debug")
public class DevTeleop extends OpMode {
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public LiftSubsystem lift;
public Gamepad currentGamepad1;
public Gamepad previousGamepad1;
public Gamepad currentGamepad2;
public Gamepad previousGamepad2;
public DcMotor frontLeftMotor;
public DcMotor backLeftMotor;
public DcMotor frontRightMotor;
public DcMotor backRightMotor;
private double MAX_POWER = .45;
@Override
public void init() {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
lift = new LiftSubsystem(hardwareMap);
claw.init();
arm.init();
wrist.init();
lift.init();
frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION);
frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
currentGamepad1 = new Gamepad();
previousGamepad1 = new Gamepad();
currentGamepad2 = new Gamepad();
previousGamepad2 = new Gamepad();
}
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
//pick up
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
wrist.floorWrist();
arm.engageArm();
}
}
public void thePickup(ClawSubsystem claw) {
//claw open close
if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
claw.switchState();
}
}
/* public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.b && !previousGamepad1.b) {
arm.parkArm();
wrist.bucketWrist();
}
}
*/
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
//low bucket
if (currentGamepad2.a && !previousGamepad2.a) {
lift.toLowBucket();
arm.bucketArm();
wrist.bucketWrist();
}
}
public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
//high basket
if (currentGamepad2.b && !previousGamepad2.b) {
lift.toHighBucket();
arm.bucketArm();
wrist.bucketWrist();
}
}
public void theTravel(LiftSubsystem lift, ArmSubsystem arm, WristSubsystem wrist){
//
if (currentGamepad2.dpad_right && !previousGamepad2.dpad_right){
lift.toFloor();
arm.bucketArm();
wrist.floorWrist();
}
}
public void hoverState(ArmSubsystem arm, WristSubsystem wrist, LiftSubsystem lift){
if (currentGamepad1.dpad_left && !previousGamepad2.dpad_left){
lift.toHover();
wrist.floorWrist();
arm.engageArm();
}
}
@Override
public void loop() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
theDrop(arm, wrist);
thePickup(claw);
// theLift(arm, wrist);
theLowBucketScore(lift, wrist, arm);
theHighBucketScore(lift, wrist, arm);
theTravel(lift, arm, wrist);
double max;
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
double lateral = gamepad1.left_stick_x;
double yaw = gamepad1.right_stick_x;
// Combine the joystick requests for each axis-motion to determine each wheel's power.
// Set up a variable for each drive wheel to save the power level for telemetry.
double leftFrontPower = axial + lateral + yaw;
double rightFrontPower = axial - lateral - yaw;
double leftBackPower = axial - lateral + yaw;
double rightBackPower = axial + lateral - yaw;
// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
max = Math.max(max, Math.abs(leftBackPower));
max = Math.max(max, Math.abs(rightBackPower));
if (max > 1.0) {
leftFrontPower /= max;
rightFrontPower /= max;
leftBackPower /= max;
rightBackPower /= max;
}
// Send calculated power to wheels
frontLeftMotor.setPower(leftFrontPower * MAX_POWER);
frontRightMotor.setPower(rightFrontPower * MAX_POWER);
backLeftMotor.setPower(leftBackPower * MAX_POWER);
backRightMotor.setPower(rightBackPower * MAX_POWER);
// Show the elapsed game time and wheel power.
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
telemetry.addData("Current Lift Position", lift.getPosition());
telemetry.update();
}
}

27
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java
View File

@ -1,27 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
@Autonomous(name = "BlueNetAuto", group = "Dev")
public class NetAuto extends OpMode {
public Follower follower;
@Override
public void init() {
follower = new Follower(hardwareMap);
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetry);
}
}

74
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java
View File

@ -8,79 +8,43 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
public class PedroConstants {
/*
Motor configuration names
Robot parameters
*/
public static final String FRONT_LEFT_MOTOR = "front-left";
public static final String BACK_LEFT_MOTOR = "back-left";
public static final String FRONT_RIGHT_MOTOR = "front-right";
public static final String BACK_RIGHT_MOTOR = "back-right";
// Turn localizer - -0.003
/*
Motor directions
*/
// Robot motor configurations
public static final String BRAIN_ROT = "Sikidi rizz 360 no teleop tf2 mama mia 2cool 4skool yasyasy yasyasyasyasyasyasyaysy ohio yes heh me is moar skeebeedee than u walked and got tripped on by your aunt my very educaded mother just served us nine what? just kydinfoiwfowefwofwioefoiejfeoiwjfomdsklfnslefknesfklnkfenfenkfeknfenkfeknfenkefnk";
public static final String FRONT_LEFT_MOTOR = "Drive front lt";
public static final String BACK_LEFT_MOTOR = "Drive back lt";
public static final String FRONT_RIGHT_MOTOR = "Drive front rt";
public static final String BACK_RIGHT_MOTOR = "Drive back rt";
// Robot motor direction
public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
public static final Direction BACK_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
/*
Centricity : true is robot-centric movement; false if field-centric movement
*/
public static final boolean CENTRICITY = true;
/*
Motor Max Power
*/
public static final double MAX_POWER = .8;
/*
IMU
*/
// Robot IMU configuration
public static final String IMU = "imu";
// Robot IMU placement
public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION
= RevHubOrientationOnRobot.LogoFacingDirection.DOWN;
public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION
= RevHubOrientationOnRobot.UsbFacingDirection.LEFT;
/*
Dead wheels
*/
public static final String RIGHT_ENCODER = "back-right";
public static final String BACK_ENCODER = "front-right";
public static final String LEFT_ENCODER = "front-left";
// Robot encoders
public static final String LEFT_ENCODER = "encoder left";
public static final String RIGHT_ENCODER = "encoder right";
public static final String BACK_ENCODER = "encoder back";
/*
Dead wheel directions
*/
// Robot encoder direction
public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
/*
Arm configuration name
*/
public static final String ARM_NAME = "arm-servo";
/*
Claw configuration name
*/
public static final String CLAW_NAME = "claw-servo";
/*
Lift configuration name
*/
public static final String LIFT_NAME = "lift-motor";
/*
Wrist configuration name
*/
public static final String WRIST_NAME = "wrist-servo";
/*
Skyhook configuration name
*/
public static final String SKYHOOK_NAME = "skyhook";
/*
Pedro's parameters
*/

135
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AsherPathV1.java Normal file
View File

@ -0,0 +1,135 @@
package org.firstinspires.ftc.teamcode.cometbots;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
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;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AsherPathV1", group = "Autonomous Pathing Tuning")
public class AsherPathV1 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(10.0, 40, 90);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.4);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
/*
* Only update this path
*/
.addPath(
// Line 1
new BezierCurve(
new Point(9.757, 84.983, Point.CARTESIAN),
new Point(33.000, 105.000, Point.CARTESIAN),
new Point(80.000, 118.000, Point.CARTESIAN),
new Point(55.000, 120.000, Point.CARTESIAN)
)
)
.addPath(
// Line 2
new BezierCurve(
new Point(55.000, 120.000, Point.CARTESIAN),
new Point(22.000, 106.000, Point.CARTESIAN),
new Point(11.000, 131.000, Point.CARTESIAN)
)
)
.addPath(
// Line 3
new BezierCurve(
new Point(11.000, 131.000, Point.CARTESIAN),
new Point(75.000, 95.000, Point.CARTESIAN),
new Point(112.000, 132.000, Point.CARTESIAN),
new Point(61.000, 131.000, Point.CARTESIAN)
)
)
.addPath(
// Line 4
new BezierLine(
new Point(61.000, 131.000, Point.CARTESIAN),
new Point(11.000, 131.000, Point.CARTESIAN)
)
)
.addPath(
// Line 5
new BezierCurve(
new Point(11.000, 131.000, Point.CARTESIAN),
new Point(100.000, 118.000, Point.CARTESIAN),
new Point(103.000, 135.000, Point.CARTESIAN),
new Point(61.000, 135.000, Point.CARTESIAN)
)
)
.addPath(
// Line 6
new BezierLine(
new Point(61.000, 135.000, Point.CARTESIAN),
new Point(11.000, 131.000, Point.CARTESIAN)
)
)
.addPath(
// Line 7
new BezierCurve(
new Point(11.000, 131.000, Point.CARTESIAN),
new Point(113.000, 95.000, Point.CARTESIAN),
new Point(67.000, 95.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
/*
* End of only update this path
*/
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

79
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExample.java Normal file
View File

@ -0,0 +1,79 @@
package org.firstinspires.ftc.teamcode.cometbots;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
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.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AutoExample - Straight Path", group = "Autonomous Pathing Tuning")
public class AutoExample extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(0.0, 20.0, 0);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.6);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
new Point(0.000, 20.000, Point.CARTESIAN),
new Point(50.000, 20.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

106
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleFour.java Normal file
View File

@ -0,0 +1,106 @@
package org.firstinspires.ftc.teamcode.cometbots;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
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;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AutoExample - 2 Curves/2 Lines", group = "Autonomous Pathing Tuning")
public class AutoExampleFour extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(12,60, 0);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.45);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierCurve(
new Point(12.000, 60.000, Point.CARTESIAN),
new Point(60.000, 60.000, Point.CARTESIAN),
new Point(60.000, 12.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
.addPath(
// Line 2
new BezierLine(
new Point(60.000, 12.000, Point.CARTESIAN),
new Point(40.000, 12.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
.addPath(
// Line 3
new BezierCurve(
new Point(40.000, 12.000, Point.CARTESIAN),
new Point(35.000, 35.000, Point.CARTESIAN),
new Point(12.000, 35.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
.addPath(
// Line 4
new BezierLine(
new Point(12.000, 35.000, Point.CARTESIAN),
new Point(12.000, 60.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(0))
.build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

104
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleSeason2025V1.java
View File

@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.cometbots;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
@ -8,27 +9,41 @@ 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 = "Pre Loaded Blue Basket Auto", group = "Competition")
public class PreLoadedBlueBasketAuto extends OpMode {
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AutoExampleSeason2025V1", group = "Autonomous Pathing Tuning")
public class AutoExampleSeason2025V1 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(7.875, 89.357);
private final Pose startPose = new Pose(15.0, 35, 90);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.45);
follower.setMaxPower(.375);
follower.setStartingPose(startPose);
@ -36,89 +51,92 @@ public class PreLoadedBlueBasketAuto extends OpMode {
.addPath(
// Line 1
new BezierLine(
new Point(8, 89, Point.CARTESIAN),
new Point(10.125, 126.804, Point.CARTESIAN)
new Point(15.000, 35.000, Point.CARTESIAN),
new Point(60.000, 35.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 2
new BezierCurve(
new Point(10.125, 126.804, Point.CARTESIAN),
new Point(37.607, 90.000, Point.CARTESIAN),
new Point(62.357, 119.893, Point.CARTESIAN)
new BezierLine(
new Point(60.000, 35.000, Point.CARTESIAN),
new Point(60.000, 25.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 3
new BezierCurve(
new Point(62.357, 119.893, Point.CARTESIAN),
new Point(33.750, 112.500, Point.CARTESIAN),
new Point(15.107, 130.661, Point.CARTESIAN)
new BezierLine(
new Point(60.000, 25.000, Point.CARTESIAN),
new Point(15.000, 25.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 4
new BezierCurve(
new Point(15.107, 130.661, Point.CARTESIAN),
new Point(58.821, 103.018, Point.CARTESIAN),
new Point(59.625, 126.964, Point.CARTESIAN)
new BezierLine(
new Point(15.000, 25.000, Point.CARTESIAN),
new Point(60.000, 25.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 5
new BezierLine(
new Point(59.625, 126.964, Point.CARTESIAN),
new Point(15.107, 130.339, Point.CARTESIAN)
new Point(60.000, 25.000, Point.CARTESIAN),
new Point(60.000, 15.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 6
new BezierLine(
new Point(15.107, 130.339, Point.CARTESIAN),
new Point(59.625, 126.964, Point.CARTESIAN)
new Point(60.000, 15.000, Point.CARTESIAN),
new Point(15.000, 15.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 7
new BezierLine(
new Point(59.625, 126.964, Point.CARTESIAN),
new Point(57.857, 131.071, Point.CARTESIAN)
new Point(15.000, 15.000, Point.CARTESIAN),
new Point(60.000, 15.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 8
new BezierLine(
new Point(57.857, 131.071, Point.CARTESIAN),
new Point(18.964, 131.679, Point.CARTESIAN)
new Point(60.000, 15.000, Point.CARTESIAN),
new Point(60.000, 8.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 9
new BezierCurve(
new Point(18.964, 134.679, Point.CARTESIAN),
new Point(84.536, 131.786, Point.CARTESIAN),
new Point(80.036, 96.429, Point.CARTESIAN)
new BezierLine(
new Point(60.000, 8.000, Point.CARTESIAN),
new Point(15.000, 8.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleThree.java Normal file
View File

@ -0,0 +1,89 @@
package org.firstinspires.ftc.teamcode.cometbots;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
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;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AutoExample - Curve and Line", group = "Autonomous Pathing Tuning")
public class AutoExampleThree extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(10,45, 0);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.4);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierCurve(
new Point(10.000, 45.000, Point.CARTESIAN),
new Point(45.000, 45.000, Point.CARTESIAN),
new Point(50.000, 20.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
.addPath(
// Line 2
new BezierLine(
new Point(50.000, 20.000, Point.CARTESIAN),
new Point(10.000, 20.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
.build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

80
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleTwo.java Normal file
View File

@ -0,0 +1,80 @@
package org.firstinspires.ftc.teamcode.cometbots;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
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;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "AutoExample - Simple Curve", group = "Autonomous Pathing Tuning")
public class AutoExampleTwo extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(10.0, 45, 0);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.4);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierCurve(
new Point(10.000, 45.000, Point.CARTESIAN),
new Point(45.000, 45.000, Point.CARTESIAN),
new Point(50.000, 20.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
.build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

197
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BasicOmniOpMode_Linear.java Normal file
View File

@ -0,0 +1,197 @@
/* Copyright (c) 2021 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
/*
* This file contains an example of a Linear "OpMode".
* An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
* The names of OpModes appear on the menu of the FTC Driver Station.
* When a selection is made from the menu, the corresponding OpMode is executed.
*
* This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot.
* This code will work with either a Mecanum-Drive or an X-Drive train.
* Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html
* Note that a Mecanum drive must display an X roller-pattern when viewed from above.
*
* Also note that it is critical to set the correct rotation direction for each motor. See details below.
*
* Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously.
* Each motion axis is controlled by one Joystick axis.
*
* 1) Axial: Driving forward and backward Left-joystick Forward/Backward
* 2) Lateral: Strafing right and left Left-joystick Right and Left
* 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left
*
* This code is written assuming that the right-side motors need to be reversed for the robot to drive forward.
* When you first test your robot, if it moves backward when you push the left stick forward, then you must flip
* the direction of all 4 motors (see code below).
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
*/
@TeleOp(name="Basic: Omni Linear OpMode", group="Linear OpMode")
public class BasicOmniOpMode_Linear extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private final ElapsedTime runtime = new ElapsedTime();
@Override
public void runOpMode() {
// Initialize the hardware variables. Note that the strings used here must correspond
// to the names assigned during the robot configuration step on the DS or RC devices.
DcMotor leftFrontDrive = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
DcMotor leftBackDrive = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
// TODO: replace these with your encoder ports
Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
Encoder rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
Encoder strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
// ########################################################################################
// !!! IMPORTANT Drive Information. Test your motor directions. !!!!!
// ########################################################################################
// Most robots need the motors on one side to be reversed to drive forward.
// The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft)
// If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure
// that your motors are turning in the correct direction. So, start out with the reversals here, BUT
// when you first test your robot, push the left joystick forward and observe the direction the wheels turn.
// Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward
// Keep testing until ALL the wheels move the robot forward when you push the left joystick forward.
leftFrontDrive.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
leftBackDrive.setDirection(BACK_LEFT_MOTOR_DIRECTION);
rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
// Wait for the game to start (driver presses START)
telemetry.addData("Status", "Initialized");
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
telemetry.update();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
double max;
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
double lateral = gamepad1.left_stick_x;
double yaw = gamepad1.right_stick_x;
// Combine the joystick requests for each axis-motion to determine each wheel's power.
// Set up a variable for each drive wheel to save the power level for telemetry.
double leftFrontPower = axial + lateral + yaw;
double rightFrontPower = axial - lateral - yaw;
double leftBackPower = axial - lateral + yaw;
double rightBackPower = axial + lateral - yaw;
// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
max = Math.max(max, Math.abs(leftBackPower));
max = Math.max(max, Math.abs(rightBackPower));
if (max > 1.0) {
leftFrontPower /= max;
rightFrontPower /= max;
leftBackPower /= max;
rightBackPower /= max;
}
// This is test code:
//
// Uncomment the following code to test your motor directions.
// Each button should make the corresponding motor run FORWARD.
// 1) First get all the motors to take to correct positions on the robot
// by adjusting your Robot Configuration if necessary.
// 2) Then make sure they run in the correct direction by modifying the
// the setDirection() calls above.
// Once the correct motors move in the correct direction re-comment this code.
/*
leftFrontPower = gamepad1.x ? 1.0 : 0.0; // X gamepad
leftBackPower = gamepad1.a ? 1.0 : 0.0; // A gamepad
rightFrontPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad
rightBackPower = gamepad1.b ? 1.0 : 0.0; // B gamepad
*/
// Send calculated power to wheels
leftFrontDrive.setPower(leftFrontPower);
rightFrontDrive.setPower(rightFrontPower);
leftBackDrive.setPower(leftBackPower);
rightBackDrive.setPower(rightBackPower);
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
telemetry.update();
}
}}

130
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueNonBasketAuto.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueBasketAuto.java
View File

@ -1,9 +1,11 @@
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.cometbots;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
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;
@ -13,149 +15,147 @@ 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 = "Blue Non Basket Auto", group = "Competition")
public class BlueNonBasketAuto extends OpMode {
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "BlueBasketAuto", group = "Autonomous Pathing Tuning")
public class BlueBasketAuto extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private PathChain path2;
private final Pose startPose = new Pose(8.000, 55.000);
private final Pose startPose = new Pose(11.25, 95.75);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.6);
follower.setMaxPower(.45);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierCurve(
new Point(8.000, 55.000, Point.CARTESIAN),
new Point(27.482, 33.750, Point.CARTESIAN),
new Point(62.357, 33.107, Point.CARTESIAN)
new BezierLine(
new Point(11.250, 95.750, Point.CARTESIAN),
new Point(37.000, 108.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0)).build();
follower.followPath(path);
path2 = follower.pathBuilder()
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierLine(
new Point(62.357, 33.107, Point.CARTESIAN),
new Point(62.000, 27.000, Point.CARTESIAN)
new BezierCurve(
new Point(37.000, 108.000, Point.CARTESIAN),
new Point(73.286, 111.536, Point.CARTESIAN),
new Point(67.821, 120.536, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierLine(
new Point(62.000, 27.000, Point.CARTESIAN),
new Point(10.000, 27.000, Point.CARTESIAN)
new Point(67.821, 120.536, Point.CARTESIAN),
new Point(28.000, 121.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 4
new BezierLine(
new Point(10.000, 27.000, Point.CARTESIAN),
new Point(61.875, 27.000, Point.CARTESIAN)
new Point(28.000, 121.500, Point.CARTESIAN),
new Point(18.000, 130.179, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 5
new BezierLine(
new Point(61.875, 27.000, Point.CARTESIAN),
new Point(61.714, 17.357, Point.CARTESIAN)
new BezierCurve(
new Point(18.000, 130.179, Point.CARTESIAN),
new Point(59.000, 102.500, Point.CARTESIAN),
new Point(68.700, 130.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 6
new BezierLine(
new Point(61.714, 17.357, Point.CARTESIAN),
new Point(14.464, 17.357, Point.CARTESIAN)
new Point(68.700, 130.500, Point.CARTESIAN),
new Point(18.000, 130.339, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 7
new BezierLine(
new Point(14.464, 17.357, Point.CARTESIAN),
new Point(61.714, 17.357, Point.CARTESIAN)
new BezierCurve(
new Point(18.000, 130.339, Point.CARTESIAN),
new Point(49.018, 121.179, Point.CARTESIAN),
new Point(63.804, 135.321, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 8
new BezierLine(
new Point(61.714, 17.357, Point.CARTESIAN),
new Point(61.554, 8.000, Point.CARTESIAN)
new Point(63.804, 135.321, Point.CARTESIAN),
new Point(53.036, 135.161, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierLine(
new Point(61.554, 8.000, Point.CARTESIAN),
new Point(12.536, 8.196, Point.CARTESIAN)
new Point(53.036, 135.161, Point.CARTESIAN),
new Point(18.643, 135.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 10
new BezierCurve(
new Point(12.536, 8.196, Point.CARTESIAN),
new Point(52.071, 19.929, Point.CARTESIAN),
new Point(50.786, 33.750, Point.CARTESIAN)
new BezierLine(
new Point(18.643, 135.000, Point.CARTESIAN),
new Point(72.300, 97.400, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 11
new BezierCurve(
new Point(50.786, 33.750, Point.CARTESIAN),
new Point(2.571, 39.375, Point.CARTESIAN),
new Point(20.732, 78.911, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 12
new BezierCurve(
new Point(20.732, 78.911, Point.CARTESIAN),
new Point(24.429, 111.054, Point.CARTESIAN),
new Point(46.929, 121.018, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 13
new BezierCurve(
new Point(46.929, 121.018, Point.CARTESIAN),
new Point(68.143, 116.357, Point.CARTESIAN),
new Point(63.000, 97.714, Point.CARTESIAN)
// Line 9
new BezierLine(
new Point(18.643, 135.000, Point.CARTESIAN),
new Point(83.250, 95.464, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
follower.followPath(path2);
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

246
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java Normal file
View File

@ -0,0 +1,246 @@
package org.firstinspires.ftc.teamcode.cometbots;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
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.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/**
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
* a circle, but some Bezier curves that have control points set essentially in a square. However,
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
* heading is to be expected.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Aaron Yang - 10158 Scott's Bots
* @author Harrison Womack - 10158 Scott's Bots
* @version 1.0, 3/12/2024
*/
@Config
@Autonomous(name = "BluebAutoV1", group = "Autonomous Pathing Tuning")
public class BluebAutoV1 extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(7.5, 72, 90);
/**
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
* initializes the FTC Dashboard telemetry.
*/
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.4);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
new Point(7.5, 72, Point.CARTESIAN),
new Point(29.893, 38.250, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 2
new BezierLine(
new Point(29.893, 38.250, Point.CARTESIAN),
new Point(65.250, 32.143, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 3
new BezierLine(
new Point(65.250, 32.143, Point.CARTESIAN),
new Point(61.714, 24.429, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 4
new BezierLine(
new Point(61.714, 24.429, Point.CARTESIAN),
new Point(13.821, 22.821, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 5
new BezierLine(
new Point(13.821, 22.821, Point.CARTESIAN),
new Point(61.714, 24.429, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 6
new BezierLine(
new Point(61.714, 24.429, Point.CARTESIAN),
new Point(60.750, 12.696, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 7
new BezierLine(
new Point(60.750, 12.696, Point.CARTESIAN),
new Point(12.375, 13.179, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 8
new BezierLine(
new Point(12.375, 13.179, Point.CARTESIAN),
new Point(60.750, 12.536, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 9
new BezierLine(
new Point(60.750, 12.536, Point.CARTESIAN),
new Point(60.589, 9.321, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 10
new BezierLine(
new Point(60.589, 9.321, Point.CARTESIAN),
new Point(12.536, 8.357, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 11
new BezierLine(
new Point(12.536, 8.357, Point.CARTESIAN),
new Point(26.679, 8.679, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 12
new BezierLine(
new Point(26.679, 8.679, Point.CARTESIAN),
new Point(22.821, 109.446, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 13
new BezierLine(
new Point(22.821, 109.446, Point.CARTESIAN),
new Point(70.714, 109.446, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 14
new BezierLine(
new Point(70.714, 109.446, Point.CARTESIAN),
new Point(71.036, 120.214, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 15
new BezierLine(
new Point(71.036, 120.214, Point.CARTESIAN),
new Point(22.179, 120.214, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 16
new BezierLine(
new Point(22.179, 120.214, Point.CARTESIAN),
new Point(11.089, 130.821, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 17
new BezierLine(
new Point(11.089, 130.821, Point.CARTESIAN),
new Point(70.714, 112.018, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 18
new BezierLine(
new Point(70.714, 112.018, Point.CARTESIAN),
new Point(70.714, 128.250, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 19
new BezierLine(
new Point(70.714, 128.250, Point.CARTESIAN),
new Point(9.964, 130.018, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 20
new BezierLine(
new Point(9.964, 130.018, Point.CARTESIAN),
new Point(70.554, 130.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 21
new BezierLine(
new Point(70.554, 130.500, Point.CARTESIAN),
new Point(70.393, 135.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90))
.addPath(
// Line 22
new BezierLine(
new Point(70.393, 135.000, Point.CARTESIAN),
new Point(13.821, 134.839, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
}
follower.telemetryDebug(telemetryA);
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluenbAutov1.java Normal file
View File

@ -0,0 +1,4 @@
package org.firstinspires.ftc.teamcode.cometbots;
public class BluenbAutov1 {
}

108
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoCompetition.java
View File

@ -1,108 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots;
public class CometBotAutoCompetition {
}
//import com.qualcomm.robotcore.hardware.HardwareMap;
//
//import org.firstinspires.ftc.robotcore.external.Telemetry;
//import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
//import org.firstinspires.ftc.teamcode.pedroPathing.util.Timer;
//import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
//import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
//import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
//
//public class Auto {
//
// public ClawSubsystem claw;
// public ArmSubsystem arm;
// public WristSubsystem wrist;
//
// public Timer clawTimer = new Timer();
// public Timer armTimer = new Timer();
// public Timer wristTimer = new Timer();
//
// public Follower follower;
// public Telemetry telemetry;
//
// public int caseState = 1;
//
// public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) {
// claw = new ClawSubsystem(hardwareMap, telemetry);
// arm = new ArmSubsystem(hardwareMap, telemetry);
// wrist = new WristSubsystem(hardwareMap, telemetry);
//
// this.follower = follower;
// this.telemetry = telemetry;
//
// init();
// }
//
// public void init() {
// claw.init();
// arm.init();
// wrist.init();
// }
//
// public void start() {
// clawTimer.resetTimer();
// armTimer.resetTimer();
// wristTimer.resetTimer();
//
// claw.start();
// arm.start();
// wrist.start();
// }
//
// public void update() {
//
// this.telemetry.addData("Current State", caseState);
// this.telemetry.addData("Claw Timer", clawTimer.getElapsedTimeSeconds());
// this.telemetry.addData("Arm Timer", armTimer.getElapsedTimeSeconds());
// this.telemetry.addData("Wrist Timer", wristTimer.getElapsedTimeSeconds());
// this.telemetry.update();
//
// switch (caseState) {
// case 1:
// claw.openClaw();
// caseState = 2;
// break;
// case 2:
// if (clawTimer.getElapsedTimeSeconds() > 2) {
// arm.toFloorPosition();
// caseState = 3;
// }
// break;
// case 3:
// if (armTimer.getElapsedTimeSeconds() > 4) {
// wrist.toFloorPosition();
// caseState = 4;
// }
// break;
// case 4:
// if (clawTimer.getElapsedTimeSeconds() > 6) {
// claw.closeClaw();
// caseState = 5;
// }
// break;
// case 5:
// if (armTimer.getElapsedTimeSeconds() > 8) {
// arm.toBucketPosition();
// wrist.toBucketPosition();
// caseState = 6;
// }
// break;
// case 6:
// if (clawTimer.getElapsedTimeSeconds() > 10) {
// claw.openClaw();
// caseState = 7;
// }
// break;
// case 7:
// this.init();
// break;
// }
// }
//}

193
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java
View File

@ -1,193 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots;
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.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.states.FieldStates;
import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
public class CometBotAutoDevelopment {
/*
Subsystems
*/
private MotorsSubsystem motors;
public ClawActionsSubsystem claw;
public ArmActionsSubsystem arm;
public WristActionsSubsystem wrist;
public LiftActionsSubsystem lift;
/*
Controllers
*/
public Gamepad GP1;
public Gamepad GP2;
public Gamepad currentGP1;
public Gamepad previousGP1;
public Gamepad currentGP2;
public Gamepad previousGP2;
private Telemetry telemetry;
public FieldStates fieldStates;
private Follower follower;
/*
Actions - Path
*/
private AutoLine1 myFirstPath;
public CometBotAutoDevelopment(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
this.claw = new ClawActionsSubsystem(hardwareMap);
this.arm = new ArmActionsSubsystem(hardwareMap);
this.wrist = new WristActionsSubsystem(hardwareMap);
this.lift = new LiftActionsSubsystem(hardwareMap);
this.GP1 = gp1;
this.GP2 = gp2;
this.telemetry = telemetry;
this.currentGP1 = new Gamepad();
this.currentGP2 = new Gamepad();
this.previousGP1 = new Gamepad();
this.previousGP2 = new Gamepad();
this.fieldStates = new FieldStates();
this.follower = new Follower(hardwareMap);
}
public void init() {
this.motors.init();
this.claw.init();
this.arm.init();
this.wrist.init();
this.lift.init();
this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
follower.setMaxPower(.75);
follower.startTeleopDrive();
}
public void update() {
this.previousGP1.copy(currentGP1);
this.currentGP1.copy(this.GP1);
this.previousGP2.copy(currentGP2);
this.currentGP2.copy(this.GP2);
this.toHighBucketScore();
this.toLowBucketScore();
this.toArmParkPosition();
this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition();
this.clawControl();
// this.motors.calculateTrajectory(this.GP1);
follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x);
follower.update();
this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
this.telemetry.addData("Claw State", this.claw.getState());
this.telemetry.addData("Claw Position", this.claw.getPosition());
this.telemetry.addData("Wrist State", this.wrist.getState());
this.telemetry.addData("Arm State", this.arm.getState());
this.telemetry.addData("Lift State", this.lift.getState());
this.telemetry.addData("Lift Position", this.lift.getPosition());
}
/*
Controller: 1
Button: A
Action: On button press, Arm hovers the floor with wrist parallel to arm
*/
public void toHighBucketScore() {
if (this.currentGP1.triangle && !this.previousGP1.triangle) {
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
this.wrist.toFloorPosition(),
this.arm.toParkPosition(),
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.toFloorPosition()
));
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
}
}
public void toLowBucketScore() {
if (this.currentGP1.circle && !this.previousGP1.circle) {
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
this.wrist.toFloorPosition(),
this.arm.toParkPosition(),
this.lift.toLowBucketPosition(),
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.toFloorPosition()
));
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
}
}
public void clawControl() {
if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) {
this.claw.switchState();
}
}
public void toArmParkPosition() {
if (this.currentGP1.square && !this.previousGP1.square) {
Actions.runBlocking(this.arm.toParkPosition());
}
}
public void toArmParkThenSwitchBetweenSubmarineAndFloorPosition() {
if (this.currentGP1.cross && !previousGP1.cross) {
if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
Actions.runBlocking(
new SequentialAction(
this.arm.toSubmarinePosition(),
this.wrist.toFloorPosition()
)
);
} else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE) {
Actions.runBlocking(
new SequentialAction(
this.arm.toFloorPosition(),
this.wrist.toFloorPosition()
)
);
} else if (this.arm.getState() == ArmActionsSubsystem.ArmState.FLOOR) {
Actions.runBlocking(
new SequentialAction(
this.arm.toSubmarinePosition(),
this.wrist.toFloorPosition()
)
);
}
}
}
}

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

@ -1,356 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots;
import static org.firstinspires.ftc.teamcode.PedroConstants.CENTRICITY;
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.states.FieldStates;
import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
public class CometBotTeleopCompetition {
/*
Subsystems
*/
private MotorsSubsystem motors;
public ClawActionsSubsystem claw;
public ArmActionsSubsystem arm;
public WristActionsSubsystem wrist;
public LiftActionsSubsystem lift;
public SkyHookSubsystem hook;
/*
Controllers
*/
public Gamepad GP1;
public Gamepad GP2;
public Gamepad currentGP1;
public Gamepad previousGP1;
public Gamepad currentGP2;
public Gamepad previousGP2;
/*
Pedro/FTC Components
*/
private Follower follower;
private Telemetry telemetry;
/*
States
*/
public FieldStates fieldStates;
/*
Configurations
*/
public double currentPower = MAX_POWER;
public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
this.claw = new ClawActionsSubsystem(hardwareMap);
this.arm = new ArmActionsSubsystem(hardwareMap);
this.wrist = new WristActionsSubsystem(hardwareMap);
this.lift = new LiftActionsSubsystem(hardwareMap);
this.hook = new SkyHookSubsystem(hardwareMap);
this.GP1 = gp1;
this.GP2 = gp2;
this.telemetry = telemetry;
this.currentGP1 = new Gamepad();
this.currentGP2 = new Gamepad();
this.previousGP1 = new Gamepad();
this.previousGP2 = new Gamepad();
this.fieldStates = new FieldStates();
this.follower = new Follower(hardwareMap);
}
public void init() {
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 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);
this.previousGP2.copy(currentGP2);
this.currentGP2.copy(this.GP2);
this.moveSkyHook();
this.toHighBucketScore();
this.toLowBucketScore();
this.toArmParkPosition();
this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition();
this.clawControl();
this.decreaseMaxPower();
this.increaseMaxPower();
Actions.runBlocking(this.lift.toFloorPosition());
follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, CENTRICITY);
follower.update();
this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
this.telemetry.addData("Claw State", this.claw.getState());
this.telemetry.addData("Claw Position", this.claw.getPosition());
this.telemetry.addData("Wrist State", this.wrist.getState());
this.telemetry.addData("Arm State", this.arm.getState());
this.telemetry.addData("Lift State", this.lift.getState());
this.telemetry.addData("Lift Position", this.lift.getPosition());
this.telemetry.addData("MaxPower", MAX_POWER);
}
/*
Type: PS4 / Logitech
Controller: 1
Button: Left Bumper
Assumption: Working motor mechanism
Action: Decreases maximum speed by -.05
*/
public void decreaseMaxPower() {
if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
this.currentPower = this.currentPower - .05;
this.follower.setMaxPower(this.currentPower);
}
}
/*
Type: PS4 / Logitech
Controller: 1
Button: Left Bumper
Assumption: Working motor mechanism
Action: Increases maximum speed by +.05
*/
public void increaseMaxPower() {
if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) {
this.currentPower = this.currentPower + .05;
this.follower.setMaxPower(this.currentPower);
}
}
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 / Logitech
Controller: 2
Button: TRIANGLE / Y
Assumption: Claw is holding specimen, robot is facing buckets ready to score
Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift
raises to high bucket. Once at high bucket position, move arm forward, wrist forward
and open claw to drop specimen into bucket. Finally, put arm back up and wrist back up,
retract lift all the way down to floor position and back to TRAVELING state.
*/
public void toHighBucketScore() {
if (this.currentGP2.triangle && !this.previousGP2.triangle) {
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(),
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.toFloorPosition(),
new SleepAction(.5)
));
}
public void highBucketDropAuto() {
Actions.runBlocking(new SequentialAction(
new SleepAction(.1),
this.lift.toHighBucketPosition(),
new SleepAction(.25),
this.arm.toBucketPosition(),
new SleepAction(.25),
this.wrist.toBucketPosition(),
new SleepAction(.25),
this.claw.openClaw(),
new SleepAction(.25),
this.wrist.toFloorPosition(),
new SleepAction(.25),
this.arm.toParkPosition(),
this.lift.toZeroPosition(),
new SleepAction(.25)
));
}
/*
Type: PS4 / Logitech
Controller: 2
Button: CIRCLE / B
Assumption: Claw is holding specimen, robot is facing buckets ready to score
Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift
raises to low bucket. Once at low bucket position, move arm forward, wrist forward
and open claw to drop specimen into bucket. Finally, put arm back up and wrist back up,
retract lift all the way down to floor position and back to TRAVELING state.
*/
public void toLowBucketScore() {
if (this.currentGP2.circle && !this.previousGP2.circle) {
if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
this.follower.breakFollowing();
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
new SleepAction(.5),
this.lift.toLowBucketPosition(),
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.toFloorPosition(),
new SleepAction(.5)
));
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
this.follower.startTeleopDrive();
}
}
}
/*
Type: PS4 / Logitech
Controller: 2
Button: RIGHT BUMPER
Assumption: Working claw mechanism
Action: On button press, claw switches state from OPEN to CLOSE
*/
public void clawControl() {
if (this.currentGP2.right_bumper && !this.previousGP2.right_bumper) {
this.claw.switchState();
}
}
/*
Type: PS4 / Logitech
Controller: 2
Button: SQUARE / X
Assumption: Working arm mechanism
Action: On button press, pulls arm up and wrist up, ideal for traveling the field when
holding a specimen in claws
*/
public void toArmParkPosition() {
if (this.currentGP2.square && !this.previousGP2.square) {
Actions.runBlocking(new SequentialAction(
this.wrist.toFloorPosition(),
this.arm.toParkPosition(),
this.lift.toFloorPosition()
));
}
}
/*
Type: PS4
Controller: 2
Button: CROSS / A
Assumption: Working claw, arm and wrist mechanisms
Action: On button press, if arm is in PARK (toArmParkPosition), drop the arm to SUBMARINE
position. SUBMARINE position means the arm and wrist are parallel to the floor, raised
3 INCHES off the ground. This state is ideal for moving the arm into the SUBMARINE
area of the field.
When arm is in SUBMARINE position, pressing the button again puts the arm and wrist into
FLOOR state. This angles the arm and wrist down so that it is able to pick specimens
from within the SUBMARINE floor.
*/
public void toArmParkThenSwitchBetweenSubmarineAndFloorPosition() {
if (this.currentGP2.cross && !previousGP2.cross) {
if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
Actions.runBlocking(
new SequentialAction(
this.arm.toSubmarinePosition(),
this.wrist.toFloorPosition()
)
);
} else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE && this.wrist.getState() == WristActionsSubsystem.WristState.FLOOR) {
Actions.runBlocking(
new SequentialAction(
// this.lift.toFloorPosition(),
this.arm.toSubmarinePosition(),
this.wrist.toPickupPosition()
)
);
} else if (this.wrist.getState() == WristActionsSubsystem.WristState.PICKUP) {
Actions.runBlocking(
new SequentialAction(
// this.arm.toSubmarinePosition(),
this.wrist.toFloorPosition()
)
);
}
}
}
}

186
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopDevelopment.java
View File

@ -1,186 +0,0 @@
package org.firstinspires.ftc.teamcode.cometbots;
public class CometBotTeleopDevelopment {
}
//import com.qualcomm.robotcore.hardware.Gamepad;
//import com.qualcomm.robotcore.hardware.HardwareMap;
//
//import org.firstinspires.ftc.robotcore.external.Telemetry;
//import org.firstinspires.ftc.teamcode.states.FieldStates;
//import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
//import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
//import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
//import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
//import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
//import org.firstinspires.ftc.teamcode.util.action.Actions;
//import org.firstinspires.ftc.teamcode.util.action.SequentialAction;
//import org.firstinspires.ftc.teamcode.util.action.SleepAction;
//
//public class DevTeleopRunMode {
//
// /*
// Subsystems
// */
// private MotorsSubsystem motors;
// public ClawSubsystem claw;
// public ArmSubsystem arm;
// public WristSubsystem wrist;
// public LiftSubsystem lift;
//
// /*
// Controllers
// */
// public Gamepad GP1;
// public Gamepad GP2;
// public Gamepad currentGP1;
// public Gamepad previousGP1;
// public Gamepad currentGP2;
// public Gamepad previousGP2;
// private Telemetry telemetry;
// public FieldStates fieldStates;
//
// public DevTeleopRunMode(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
// this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
// this.claw = new ClawSubsystem(hardwareMap, telemetry);
// this.arm = new ArmSubsystem(hardwareMap, telemetry);
// this.wrist = new WristSubsystem(hardwareMap, telemetry);
// this.lift = new LiftSubsystem(hardwareMap, telemetry);
// this.GP1 = gp1;
// this.GP2 = gp2;
// this.telemetry = telemetry;
// this.currentGP1 = new Gamepad();
// this.currentGP2 = new Gamepad();
// this.previousGP1 = new Gamepad();
// this.previousGP2 = new Gamepad();
// this.fieldStates = new FieldStates();
// }
//
// public void init() {
// this.motors.init();
// this.claw.init();
// this.arm.init();
// this.wrist.init();
// this.lift.init();
// }
//
// public void update() {
// this.previousGP1.copy(currentGP1);
// this.currentGP1.copy(this.GP1);
// this.previousGP2.copy(currentGP2);
// this.currentGP2.copy(this.GP2);
// this.toTravelfromField();
// this.thePickup();
// this.toFieldFromBucketScore();
// this.toLowBucketScore();
// this.toHighBucketScore();
// this.toHold();
// this.motors.calculateTrajectory(this.GP1);
// this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
// this.telemetry.addData("Claw State", this.claw.getState());
// this.telemetry.addData("Claw Position", this.claw.getPosition());
// this.telemetry.addData("Wrist State", this.wrist.getState());
// this.telemetry.addData("Arm State", this.arm.getState());
// this.telemetry.addData("Lift State", this.lift.getState());
// this.telemetry.addData("Lift Position", this.lift.getPosition());
// }
//
// /*
// Controller: 1
// Button: A
// Action: On button press, Arm hovers the floor with wrist parallel to arm
// */
// public void toTravelfromField() {
// if (this.currentGP1.dpad_down && !this.previousGP1.dpad_down) {
// if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.TRAVELING &&
// this.lift.getPosition() < 40) {
// Actions.runBlocking(new SequentialAction(
// this.wrist.toFloorPosition,
// new SleepAction(.75),
// this.arm.toFloorPosition
// ));
// fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
// }
// }
// }
//
// /*
// Controller: 1
// Button: Right Bumper
// Action: On button press, open and closes claw
// */
// public void thePickup() {
// if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) {
// this.claw.switchState();
// }
// }
//
// /*
// Controller: 1
// Button: Right Bumper
// Action: On button press, open and closes claw
// */
// public void toHold() {
// if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
// Actions.runBlocking(new SequentialAction(
// arm.toParkPosition,
// wrist.toFloorPosition
// ));
// }
// }
//
// /*
// Controller: 2
// Button: Y
// Action: On button press, lift to low bucket height,
// arm to bucket position, wrist to bucket position
// */
// public void toLowBucketScore() {
// if (this.currentGP1.a && !this.previousGP1.a) {
// fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
// Actions.runBlocking(new SequentialAction(
// lift.toLowBucket,
// arm.toBucketPosition,
// wrist.toBucketPosition
// ));
// }
// }
//
// /*
// Controller: 2
// Button: A
// Action: On button press, lift to low bucket height,
// arm to bucket position, wrist to bucket position
// */
// public void toHighBucketScore() {
// if (this.currentGP1.b && !this.previousGP1.b) {
// fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
// Actions.runBlocking(new SequentialAction(
// lift.toHighBucket,
// arm.toBucketPosition,
// wrist.toBucketPosition
// ));
// }
// }
//
// /*
// Controller: 2
// Button: Direction Pad DOWN
// Action: On directional press, lift to floor height,
// arm to bucket position, wrist to floor position
// */
// public void toFieldFromBucketScore() {
// if (this.currentGP1.dpad_right && !this.previousGP1.dpad_right) {
// if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.BUCKET) {
// Actions.runBlocking(new SequentialAction(
// lift.toFloor,
// arm.toBucketPosition,
// wrist.toFloorPosition
// ));
//// fieldStates.setFieldLocation(FieldStates.FieldLocation.FIELD);
// }
// }
// }
//
//}

172
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/SensorIMUOrthogonal.java Normal file
View File

@ -0,0 +1,172 @@
/* Copyright (c) 2022 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_LOGO_FACING_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_USB_FACING_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.firstinspires.ftc.teamcode.PedroConstants;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
/*
* This OpMode shows how to use the new universal IMU interface. This
* interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
* on the robot with the name "imu".
*
* The sample will display the current Yaw, Pitch and Roll of the robot.<br>
* With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
* Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br>
* Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br>
* Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br>
*
* The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
*
* This specific sample assumes that the Hub is mounted on one of the three orthogonal planes
* (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments.
*
* Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at
* the alternative SensorIMUNonOrthogonal sample in this folder.
*
* This "Orthogonal" requirement means that:
*
* 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions:
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
*
* 2) The USB ports can only be pointing in one of the same six directions:<br>
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
*
* So, To fully define how your Hub is mounted to the robot, you must simply specify:<br>
* logoFacingDirection<br>
* usbFacingDirection
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
*
* Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode
* to use those parameters.
*/
@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor")
@Disabled // Comment this out to add to the OpMode list
public class SensorIMUOrthogonal extends LinearOpMode {
// The IMU sensor object
IMU imu;
private Encoder leftEncoder;
private Encoder rightEncoder;
private Encoder strafeEncoder;
//----------------------------------------------------------------------------------------------
// Main logic
//----------------------------------------------------------------------------------------------
@Override
public void runOpMode() throws InterruptedException {
// Retrieve and initialize the IMU.
// This sample expects the IMU to be in a REV Hub and named "imu".
imu = hardwareMap.get(IMU.class, PedroConstants.IMU);
// TODO: replace these with your encoder ports
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
// TODO: reverse any encoders necessary
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
/* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values.
*
* Two input parameters are required to fully specify the Orientation.
* The first parameter specifies the direction the printed logo on the Hub is pointing.
* The second parameter specifies the direction the USB connector on the Hub is pointing.
* All directions are relative to the robot, and left/right is as-viewed from behind the robot.
*
* If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the
* RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection.
*/
/* The next two lines define Hub orientation.
* The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
*
* To Do: EDIT these two lines to match YOUR mounting configuration.
*/
RevHubOrientationOnRobot.LogoFacingDirection logoDirection = IMU_LOGO_FACING_DIRECTION;
RevHubOrientationOnRobot.UsbFacingDirection usbDirection = IMU_USB_FACING_DIRECTION;
RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
// Now initialize the IMU with this mounting orientation
// Note: if you choose two conflicting directions, this initialization will cause a code exception.
imu.initialize(new IMU.Parameters(orientationOnRobot));
// Loop and update the dashboard
while (!isStopRequested()) {
telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection);
// Check to see if heading reset is requested
if (gamepad1.y) {
telemetry.addData("Yaw", "Resetting\n");
imu.resetYaw();
} else {
telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
}
// Retrieve Rotational Angles and Velocities
YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
telemetry.update();
}
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java
View File

@ -34,7 +34,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
@TeleOp(name = "Arm Test", group = "Debug")
public class ArmTest extends LinearOpMode {
@ -48,7 +48,7 @@ public class ArmTest extends LinearOpMode {
/*
* Instantiate Arm
*/
ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
ArmSubsystem arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
/*
* Instantiate gamepad state holders
@ -67,11 +67,15 @@ public class ArmTest extends LinearOpMode {
currentGamepad1.copy(gamepad1);
if (currentGamepad1.circle && !previousGamepad1.circle) {
arm.toParkPosition();
arm.parkArm();
}
if (currentGamepad1.square && !previousGamepad1.square) {
arm.toBucketPosition();
arm.engageArm();
}
if (currentGamepad1.cross && !previousGamepad1.cross) {
arm.switchState();
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java
View File

@ -34,8 +34,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
@TeleOp(name = "Claw Test", group = "Debug")
public class ClawTest extends LinearOpMode {
@ -49,7 +48,7 @@ public class ClawTest extends LinearOpMode {
/*
* Instantiate Claw
*/
ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
ClawSubsystem claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.OPEN);
/*
* Instantiate gamepad state holders
@ -58,7 +57,6 @@ public class ClawTest extends LinearOpMode {
Gamepad previousGamepad1 = new Gamepad();
waitForStart();
claw.init();
runtime.reset();
// run until the end of the match (driver presses STOP)

95
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftWristArmTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftRawTest.java
View File

@ -29,36 +29,31 @@
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
@TeleOp(name = "Lift Wrist Arm Test", group = "Debug")
public class LiftWristArmTest extends LinearOpMode {
@TeleOp(name = "Lift Raw Test", group = "Debug")
public class LiftRawTest extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private final ElapsedTime runtime = new ElapsedTime();
private final int MIN_POINT = 0;
private final int MAX_POINT = 3700;
@Override
public void runOpMode() {
/*
* Instantiate Lift
*/
LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap);
WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap);
ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
DcMotor liftDrive = hardwareMap.get(DcMotor.class, "lift-motor");
liftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
/*
* Instantiate gamepad state holders
@ -66,10 +61,6 @@ public class LiftWristArmTest extends LinearOpMode {
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
lift.init();
wrist.init();
arm.init();
claw.init();
waitForStart();
runtime.reset();
@ -80,57 +71,53 @@ public class LiftWristArmTest extends LinearOpMode {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
arm.setPosition(arm.getPosition() + .05);
}
liftDrive.setPower(.5);
if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
arm.setPosition(arm.getPosition() - .05);
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
claw.switchState();
}
if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
Actions.runBlocking(
new SequentialAction(
lift.toHighRung(),
arm.toBucketPosition(),
wrist.toRungPosition(),
new SleepAction(0.5),
lift.toHighRungAttach(),
new SleepAction(0.5),
claw.openClaw())
);
// Max position is 6800, safely setting to 6500
//Delete open claw
if (currentGamepad1.square && !previousGamepad1.square) {
liftDrive.setTargetPosition(MIN_POINT);
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if (currentGamepad1.triangle && !previousGamepad1.triangle) {
wrist.setPosition(wrist.getPosition() + .05);
liftDrive.setTargetPosition(1500);
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
liftDrive.setTargetPosition(2750);
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if (currentGamepad1.cross && !previousGamepad1.cross) {
wrist.setPosition(wrist.getPosition() - .05);
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
lift.setPosition(lift.getPosition() + 100);
liftDrive.setTargetPosition(MAX_POINT);
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
if (lift.getPosition() > 1000) {
lift.setPosition(lift.getPosition()-1000);
int newPosition = liftDrive.getCurrentPosition() - 125;
if (newPosition < MIN_POINT) {
liftDrive.setTargetPosition(MIN_POINT);
} else {
liftDrive.setTargetPosition(newPosition);
}
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
int newPosition = liftDrive.getCurrentPosition() + 125;
if (newPosition > MAX_POINT) {
liftDrive.setTargetPosition(MAX_POINT);
} else {
liftDrive.setTargetPosition(newPosition);
}
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Lift Drive Position", lift.getPosition());
telemetry.addData("Wrist Position", wrist.getPosition());
telemetry.addData("Arm Position", arm.getPosition());
telemetry.addData("Lift Drive Position", liftDrive.getCurrentPosition());
telemetry.update();
}
}

26
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java
View File

@ -29,26 +29,32 @@
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
@TeleOp(name = "Lift Test", group = "Debug")
public class LiftTest extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
private final ElapsedTime runtime = new ElapsedTime();
private final int MIN_POINT = 0;
// 2000 ~ 2500
// 3750 max
private final int MAX_POINT = 6500;
@Override
public void runOpMode() {
/*
* Instantiate Lift
*/
LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap);
LiftSubsystem lift = new LiftSubsystem(hardwareMap);
/*
* Instantiate gamepad state holders
@ -68,29 +74,21 @@ public class LiftTest extends LinearOpMode {
currentGamepad1.copy(gamepad1);
if (currentGamepad1.square && !previousGamepad1.square) {
Actions.runBlocking(lift.toFloorPosition());
lift.toFloor();
}
if (currentGamepad1.triangle && !previousGamepad1.triangle) {
Actions.runBlocking(lift.toHighBucketPosition());
lift.toHighBucket();
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
Actions.runBlocking(lift.toLowBucketPosition());
lift.toLowBucket();
}
if (currentGamepad1.cross && !previousGamepad1.cross) {
lift.switchState();
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
lift.setPosition(lift.getPosition() + 25);
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
lift.setPosition(lift.getPosition() - 25);
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Lift Drive Position", lift.getPosition());

88
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SkyhookTest.java
View File

@ -1,88 +0,0 @@
/* Copyright (c) 2021 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
@TeleOp(name = "Skyhook Test", group = "Debug")
public class SkyhookTest extends LinearOpMode {
private final ElapsedTime runtime = new ElapsedTime();
@Override
public void runOpMode() {
/*
* Instantiate Lift
*/
SkyHookSubsystem hook = new SkyHookSubsystem(hardwareMap);
/*
* Instantiate gamepad state holders
*/
Gamepad currentGamepad2 = new Gamepad();
Gamepad previousGamepad2 = new Gamepad();
hook.init();
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
if (currentGamepad2.dpad_up) {
hook.moveSkyHook(-1.00);;
} else if (currentGamepad2.dpad_down) {
hook.moveSkyHook(1.00);
} else {
hook.moveSkyHook(0.00);
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Skyhook Position", hook.getHookPosition());
telemetry.update();
}
}
}

196
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java
View File

@ -1,196 +0,0 @@
/* Copyright (c) 2021 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode.cometbots.tests;
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.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
@TeleOp(name = "SpecimenTest", group = "Debug")
public class SpecimenTest extends LinearOpMode {
private final ElapsedTime runtime = new ElapsedTime();
private PathChain path;
private Follower robot;
private final Pose startPose = new Pose(36, 72);
public AutoLine1 firstPath = new AutoLine1();
@Override
public void runOpMode() {
/*
* Instantiate Lift
*/
LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap);
WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap);
ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
Follower robot = new Follower(hardwareMap);
firstPath.moveToPath1(robot);
/*
Robot stuff
*/
robot.setStartingPose(startPose);
path = robot.pathBuilder().addPath(
// Line 1
new BezierLine(
new Point(37.500, 72.000, Point.CARTESIAN),
new Point(36.000, 72.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0)).build();
/*
* Instantiate gamepad state holders
*/
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
Gamepad currentGamepad2 = new Gamepad();
Gamepad previousGamepad2 = new Gamepad();
lift.init();
wrist.init();
arm.init();
claw.init();
robot.setMaxPower(.40);
robot.followPath(path);
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
robot.setTeleOpMovementVectors(-currentGamepad1.left_stick_y, -currentGamepad1.left_stick_x, -currentGamepad1.right_stick_x);
//robot.update();
if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
arm.setPosition(arm.getPosition() + .05);
}
if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
arm.setPosition(arm.getPosition() - .05);
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
claw.switchState();
}
if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
robot.update();
Actions.runBlocking(
new SequentialAction(
wrist.toFloorPosition(),
lift.toHighRung(),
wrist.toSpeciemenBar(),
lift.dropToHighRung()
// reverseMoveToPath(robot),
// claw.openClaw(),
// wrist.toFloorPosition(),
// lift.toFloorPosition()
)
);
}
if (currentGamepad1.triangle && !previousGamepad1.triangle) {
wrist.setPosition(wrist.getPosition() + .05);
}
if (currentGamepad1.cross && !previousGamepad1.cross) {
wrist.setPosition(wrist.getPosition() - .05);
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
lift.setPosition(lift.getPosition() + 175);
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
lift.setPosition(lift.getPosition() - 25);
}
if (currentGamepad2.x && !previousGamepad2.x) {
while (true) {
robot.update();
if (!robot.isBusy()) {
robot.breakFollowing();
robot.startTeleopDrive();
break;
}
}
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Lift Drive Position", lift.getPosition());
telemetry.addData("Wrist Position", wrist.getPosition());
telemetry.addData("Arm Position", arm.getPosition());
telemetry.update();
}
}
}

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java
View File

@ -29,13 +29,12 @@
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@TeleOp(name = "Wrist Test", group = "Debug")
public class WristTest extends LinearOpMode {
@ -49,7 +48,7 @@ public class WristTest extends LinearOpMode {
/*
* Instantiate Wrist
*/
WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap);
WristSubsystem wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
/*
* Instantiate gamepad state holders
@ -68,11 +67,11 @@ public class WristTest extends LinearOpMode {
currentGamepad1.copy(gamepad1);
if (currentGamepad1.square && !previousGamepad1.square) {
Actions.runBlocking(wrist.toBucketPosition());
wrist.bucketWrist();
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
Actions.runBlocking(wrist.toFloorPosition());
wrist.floorWrist();
}
if (currentGamepad1.cross && !previousGamepad1.cross) {

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

@ -4,30 +4,19 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class RobotConstants {
public final static double clawClose = 0.8;
public final static double clawOpen = 0.15;
public static double clawClose = 1.00;
public static double clawOpen = 0.05;
public final static double armFloor = 0.7;
public final static double armSubmarine = 0.55;
public final static double armPark = 0.0;
public final static double armBucket = 0.2;
public static double armEngage = 0.5;
public static double armPark = 0.125;
public static double armBucket = 0.175;
public final static double wristFloor = 0.55;
public final static double wristBucket = 0.25;
public final static double wristRung = 0.55;
public final static double wristPickup = 0.1;
public final static double wristSpeciemen = 0.1;
public final static int liftToFloorPos = 500;
public final static int liftToLowBucketPos = 2650;
public final static int liftToHighRung = 2100;
public final static int dropToHighRung = 1675;
public final static int liftToHighRungAttach = 1050;
public final static int liftToHighBucketPos = 4900;
public final static double liftPower = 1;
public static double wristFloor = 0.625;
public static double wristBucket = 0.215;
public static int liftToFloorPos = 20;
public static int liftToFloatPos = 150;
public static int liftToLowBucketPos = 2250;
public static int liftToHighBucketPos = 3900;
public static double liftPower = .45;
public static int liftToHoverState = 60;
}

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,7 +14,6 @@ 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<>();

114
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java Normal file
View File

@ -0,0 +1,114 @@
package org.firstinspires.ftc.teamcode.runmodes;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
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.Path;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
import org.firstinspires.ftc.teamcode.pedroPathing.util.Timer;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.*;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.util.action.Action;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
import org.firstinspires.ftc.teamcode.util.action.SequentialAction;
import org.firstinspires.ftc.teamcode.util.action.SleepAction;
public class Auto {
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public Timer clawTimer = new Timer();
public Timer armTimer = new Timer();
public Timer wristTimer = new Timer();
public Follower follower;
public Telemetry telemetry;
public int caseState = 1;
public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
this.follower = follower;
this.telemetry = telemetry;
init();
}
public void init() {
claw.init();
arm.init();
wrist.init();
}
public void start() {
clawTimer.resetTimer();
armTimer.resetTimer();
wristTimer.resetTimer();
claw.start();
arm.start();
wrist.start();
}
public void update() {
this.telemetry.addData("Current State", caseState);
this.telemetry.addData("Claw Timer", clawTimer.getElapsedTimeSeconds());
this.telemetry.addData("Arm Timer", armTimer.getElapsedTimeSeconds());
this.telemetry.addData("Wrist Timer", wristTimer.getElapsedTimeSeconds());
this.telemetry.update();
switch(caseState) {
case 1:
claw.openClaw();
caseState = 2;
break;
case 2:
if (clawTimer.getElapsedTimeSeconds() > 2) {
arm.engageArm();
caseState = 3;
}
break;
case 3:
if (armTimer.getElapsedTimeSeconds() > 4) {
wrist.floorWrist();
caseState = 4;
}
break;
case 4:
if (clawTimer.getElapsedTimeSeconds() > 6) {
claw.closeClaw();
caseState = 5;
}
break;
case 5:
if (armTimer.getElapsedTimeSeconds() > 8) {
arm.bucketArm();
wrist.bucketWrist();
caseState = 6;
}
break;
case 6:
if (clawTimer.getElapsedTimeSeconds() > 10) {
claw.openClaw();
caseState = 7;
}
break;
case 7:
this.init();
break;
}
}
}

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java Normal file
View File

@ -0,0 +1,89 @@
package org.firstinspires.ftc.teamcode.runmodes;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
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.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class Teleop {
private ClawSubsystem claw;
private Follower follower;
private DcMotorEx leftFront;
private DcMotorEx leftRear;
private DcMotorEx rightFront;
private DcMotorEx rightRear;
private Telemetry telemetry;
private Gamepad gamepad1;
private Gamepad currentGamepad1;
private Gamepad previousGamepad1;
public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Gamepad gamepad1) {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
initMotors(hardwareMap);
this.follower = follower;
this.telemetry = telemetry;
this.gamepad1 = gamepad1;
this.currentGamepad1 = new Gamepad();
this.previousGamepad1 = new Gamepad();
}
public void start() {
claw.start();
follower.startTeleopDrive();
}
public void update() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
if (currentGamepad1.a && !previousGamepad1.a)
claw.switchState();
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
telemetry.addData("X", follower.getPose().getX());
telemetry.addData("Y", follower.getPose().getY());
telemetry.addData("Heading", Math.toDegrees(follower.getPose().getHeading()));
telemetry.addData("Claw State", claw.getState());
telemetry.update();
}
private void initMotors(HardwareMap hardwareMap) {
rightFront = hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR);
rightRear = hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR);
leftFront = hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR);
leftRear = hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR);
rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION);
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
}

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java
View File

@ -1,18 +0,0 @@
package org.firstinspires.ftc.teamcode.states;
public class FieldStates {
public enum FieldLocation {
BUCKET, SUBMARINE, FLOATING, TRAVELING
}
private FieldLocation fieldLocation;
public FieldLocation getFieldLocation() {
return fieldLocation;
}
public void setFieldLocation(FieldLocation fieldLocation) {
this.fieldLocation = fieldLocation;
}
}

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmActionsSubsystem.java
View File

@ -1,89 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.ARM_NAME;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armSubmarine;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.ServoImplEx;
public class ArmActionsSubsystem {
public enum ArmState {
PARK, FLOOR, BUCKET, SUBMARINE
}
private ServoImplEx arm;
private ArmState state;
public ArmActionsSubsystem(HardwareMap hardwareMap) {
this.arm = hardwareMap.get(ServoImplEx.class, ARM_NAME);
}
public class MoveToPosition implements Action {
private double positionValue;
private ArmState positionState;
public MoveToPosition(double positionValue, ArmState positionState) {
this.positionValue = positionValue;
this.positionState = positionState;
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
arm.setPosition(positionValue);
setState(positionState);
telemetryPacket.put("Arm State", positionState);
return false;
}
}
public Action toParkPosition() {
return new MoveToPosition(armPark, ArmState.PARK);
}
public Action toSubmarinePosition() {
return new MoveToPosition(armSubmarine, ArmState.SUBMARINE);
}
public Action toFloorPosition() {
return new MoveToPosition(armFloor, ArmState.FLOOR);
}
public Action toBucketPosition() {
return new MoveToPosition(armBucket, ArmState.BUCKET);
}
public void setState(ArmState armState) {
this.state = armState;
}
public ArmState getState() {
return this.state;
}
public void init() {
this.arm.resetDeviceConfigurationForOpMode();
Actions.runBlocking(this.toParkPosition());
}
public void start() {
Actions.runBlocking(this.toParkPosition());
}
public double getPosition() {
return this.arm.getPosition();
}
public void setPosition(double position) {
this.arm.setPosition(position);
}
}

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java Normal file
View File

@ -0,0 +1,89 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armEngage;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import org.firstinspires.ftc.teamcode.util.action.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class ArmSubsystem {
public enum ArmState {
PARK, ENGAGE, BUCKET
}
public ServoImplEx arm;
public ArmState state;
public RunAction engageArm, parkArm, bucketArm;
public ArmSubsystem(HardwareMap hardwareMap, ArmState armState) {
arm = hardwareMap.get(ServoImplEx.class, "arm-servo");
arm.resetDeviceConfigurationForOpMode();
this.state = armState;
parkArm = new RunAction(this::parkArm);
engageArm = new RunAction(this::engageArm);
bucketArm = new RunAction(this::bucketArm);
}
public void setState(ArmState armState) {
if (armState == ArmState.ENGAGE) {
arm.setPosition(armEngage);
this.state = ArmState.ENGAGE;
} else if (armState == ArmState.PARK) {
arm.setPosition(armPark);
this.state = ArmState.PARK;
} else if (armState == ArmState.BUCKET) {
arm.setPosition(armBucket);
this.state = ArmState.BUCKET;
}
}
public void engageArm() {
setState(ArmState.ENGAGE);
}
public void parkArm() {
setState(ArmState.PARK);
}
public void bucketArm() {
setState(ArmState.BUCKET);
}
public void switchState() {
if (state == ArmState.ENGAGE) {
setState(ArmState.PARK);
} else if (state == ArmState.PARK) {
setState(ArmState.BUCKET);
} else if (state == ArmState.BUCKET) {
setState(ArmState.ENGAGE);
}
}
public ArmState getState() {
return this.state;
}
public void init() {
Actions.runBlocking(parkArm);
}
public void start() {
Actions.runBlocking(parkArm);
}
public double getPosition() {
return this.arm.getPosition();
}
public void setPosition(double position) {
this.arm.setPosition(position);
}
}

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java
View File

@ -1,36 +0,0 @@
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 AutoLine1 {
private Pose startPose = new Pose(36, 72);
public void moveToPath1(Follower robot) {
PathChain pathChain;
robot.setStartingPose(startPose);
PathBuilder builder = new PathBuilder();
builder
.addPath(
new BezierCurve(
new Point(36.000, 72.000, Point.CARTESIAN),
new Point(37.500, 72.000, Point.CARTESIAN)
)
);
pathChain = builder.build();
robot.followPath(pathChain);
}
}

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

@ -1,36 +0,0 @@
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);
}
}

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

@ -1,82 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.CLAW_NAME;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
public class ClawActionsSubsystem {
public enum ClawState {
CLOSED, OPEN
}
private Servo claw;
private ClawState state;
public ClawActionsSubsystem(HardwareMap hardwareMap) {
this.claw = hardwareMap.get(Servo.class, CLAW_NAME);
}
public class MoveToPosition implements Action {
private double positionValue;
private ClawState positionState;
public MoveToPosition(double positionValue, ClawState positionState) {
this.positionValue = positionValue;
this.positionState = positionState;
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
setState(positionState);
claw.setPosition(positionValue);
telemetryPacket.put("Claw State", positionState);
return false;
}
}
public Action openClaw() {
return new MoveToPosition(clawOpen, ClawState.OPEN);
}
public Action closeClaw() {
return new MoveToPosition(clawClose, ClawState.CLOSED);
}
public void setState(ClawState clawState) {
this.state = clawState;
}
public ClawState getState() {
return this.state;
}
public void switchState() {
if (state == ClawState.CLOSED) {
Actions.runBlocking(openClaw());
} else if (state == ClawState.OPEN) {
Actions.runBlocking(closeClaw());
}
}
public void init() {
Actions.runBlocking(closeClaw());
}
public void start() {
Actions.runBlocking(openClaw());
}
public double getPosition() {
return this.claw.getPosition();
}
}

67
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java Normal file
View File

@ -0,0 +1,67 @@
package org.firstinspires.ftc.teamcode.subsystem;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.util.action.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose;
public class ClawSubsystem {
public enum ClawState {
CLOSED, OPEN
}
private Servo claw;
private ClawState state;
public RunAction openClaw, closeClaw;
public ClawSubsystem(HardwareMap hardwareMap, ClawState clawState) {
claw = hardwareMap.get(Servo.class, "claw-servo");
this.state = clawState;
openClaw = new RunAction(this::openClaw);
closeClaw = new RunAction(this::closeClaw);
}
public void setState(ClawState clawState) {
if (clawState == ClawState.CLOSED) {
claw.setPosition(clawClose);
this.state = ClawState.CLOSED;
} else if (clawState == ClawState.OPEN) {
claw.setPosition(clawOpen);
this.state = ClawState.OPEN;
}
}
public ClawState getState() {
return this.state;
}
public void switchState() {
if (state == ClawState.CLOSED) {
setState(ClawState.OPEN);
} else if (state == ClawState.OPEN) {
setState(ClawState.CLOSED);
}
}
public void openClaw() {
setState(ClawState.OPEN);
}
public void closeClaw() {
setState(ClawState.CLOSED);
}
public void init() {
Actions.runBlocking(closeClaw);
}
public void start() {
Actions.runBlocking(closeClaw);
}
}

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

@ -1,38 +0,0 @@
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));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

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

@ -1,36 +0,0 @@
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.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 HighBasketAutoPath2 {
public void moveToPath2(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(25.500, 120.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java
View File

@ -1,35 +0,0 @@
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.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 HighBasketAutoPath3 {
public void moveToBasketPath3(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(29.000, 120.000, Point.CARTESIAN),
new Point(18.000, 126.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath4.java
View File

@ -1,34 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
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 HighBasketAutoPath4 {
public void moveToPickupAgainPath4(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(25.000, 129.900, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath5.java
View File

@ -1,34 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
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 HighBasketAutoPath5 {
public void moveToPickupAgainPath5(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(24.000, 131.000, Point.CARTESIAN),
new Point(18.000, 126.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

50
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath6.java
View File

@ -1,50 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
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 HighBasketAutoPath6 {
public void moveToParkPath6(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(18.000, 126.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(270))
.addPath(
// Line 2
new BezierLine(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(81.000, 125.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270))
.addPath(
// Line 3
new BezierLine(
new Point(81.000, 125.000, Point.CARTESIAN),
new Point(81.000, 100.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

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

@ -1,126 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_NAME;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.dropToHighRung;
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.liftToHighRung;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRungAttach;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
public class LiftActionsSubsystem {
public DcMotor lift;
public enum LiftState {
FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE, HIGH_RUNG, HIGH_RUNG_DROP
}
private LiftState liftState;
public LiftActionsSubsystem(HardwareMap hardwareMap) {
lift = hardwareMap.get(DcMotor.class, LIFT_NAME);
}
public class MoveToPosition implements Action {
private int positionValue;
private LiftState positionState;
public MoveToPosition(int positionValue, LiftState positionState) {
this.positionValue = positionValue;
this.positionState = positionState;
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
setState(positionState);
lift.setTargetPosition(positionValue);
lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
telemetryPacket.put("Lift State", positionState);
telemetryPacket.put("Lift Position", lift.getCurrentPosition());
boolean result = lift.getTargetPosition() - 15 < lift.getCurrentPosition() &&
lift.getCurrentPosition() < lift.getTargetPosition() + 15;
return !result;
}
}
public Action toFloorPosition() {
return new MoveToPosition(liftToFloorPos, LiftState.FLOOR);
}
public Action toHighRung() {
return new MoveToPosition(liftToHighRung, LiftState.HIGH_RUNG);
}
public Action dropToHighRung() {
return new MoveToPosition(dropToHighRung, LiftState.HIGH_RUNG_DROP);
}
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);
}
public Action toHighBucketPosition() {
return new MoveToPosition(liftToHighBucketPos, LiftState.HIGH_BUCKET);
}
public void switchState() {
if (this.liftState == LiftState.FLOOR) {
Actions.runBlocking(toLowBucketPosition());
} else if (this.liftState == LiftState.LOW_BUCKET) {
Actions.runBlocking(toHighBucketPosition());
} else if (this.liftState == LiftState.HIGH_BUCKET) {
Actions.runBlocking(toFloorPosition());
}
}
public void init() {
lift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
lift.setDirection(DcMotorSimple.Direction.REVERSE);
lift.setPower(liftPower);
}
private void setState(LiftState liftState) {
this.liftState = liftState;
}
public LiftState getState() {
return this.liftState;
}
public int getPosition() {
return lift.getCurrentPosition();
}
public void setPosition(int position) {
lift.setTargetPosition(position);
lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
public void start() {
Actions.runBlocking(toFloorPosition());
}
}

94
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java Normal file
View File

@ -0,0 +1,94 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloatPos;
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.liftToHoverState;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class LiftSubsystem {
public DcMotor lift;
public RunAction toFloor, toLowBucket, toHighBucket;
public enum LiftState {
FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, HOVER
}
private LiftState liftState;
public LiftSubsystem(HardwareMap hardwareMap) {
lift = hardwareMap.get(DcMotor.class, "lift-motor");
toFloor = new RunAction(this::toFloor);
toLowBucket = new RunAction(this::toLowBucket);
toHighBucket = new RunAction(this::toHighBucket);
}
public void setTarget(int b) {
lift.setTargetPosition(b);
lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
public void switchState() {
if (this.liftState == LiftState.FLOOR) {
this.toFloor();
} else if (this.liftState == LiftState.FLOAT) {
this.toLowBucket();
} else if (this.liftState == LiftState.LOW_BUCKET) {
this.toHighBucket();
} else if (this.liftState == LiftState.HIGH_BUCKET) {
this.toFloor();
}
}
public void toHover() {
this.setTarget(liftToHoverState);
this.setState(LiftState.HOVER);
}
public void toFloor() {
this.setTarget(liftToFloorPos);
this.setState(LiftState.FLOOR);
}
public void toFloat() {
this.setTarget(liftToFloatPos);
this.setState(LiftState.FLOAT);
}
public void toLowBucket() {
this.setTarget(liftToLowBucketPos);
this.setState(LiftState.LOW_BUCKET);
}
public void toHighBucket() {
this.setTarget(liftToHighBucketPos);
this.setState(LiftState.HIGH_BUCKET);
}
public void init() {
lift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
lift.setDirection(DcMotorSimple.Direction.REVERSE);
lift.setPower(liftPower);
}
private void setState(LiftState liftState) {
this.liftState = liftState;
}
public int getPosition() {
return lift.getCurrentPosition();
}
public void start() {
this.toFloor();
}
}

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java
View File

@ -26,7 +26,7 @@ public class MotorsSubsystem {
public DcMotor backRightMotor;
public enum TravelState {
STOPPED, MOVING
PARKED, BUCKET, SUBMARINE
}
public TravelState travelState;
@ -60,8 +60,6 @@ public class MotorsSubsystem {
backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.setState(TravelState.STOPPED);
}
public void setFrontLeftMotorPower(double power) {
@ -117,8 +115,8 @@ public class MotorsSubsystem {
// Show the elapsed game time and wheel power.
this.telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
this.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
this.telemetry.addData("Current State", this.getState());
this.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
this.telemetry.update();
}
public void setState(TravelState travelState) {
@ -129,12 +127,4 @@ public class MotorsSubsystem {
return this.travelState;
}
public void setPower(DcMotor motor, double power) {
motor.setPower(power);
if (power < 0.05) {
this.setState(TravelState.MOVING);
} else {
this.setState(TravelState.STOPPED);
}
}
}

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

@ -1,52 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.SKYHOOK_NAME;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
import com.acmerobotics.roadrunner.Action;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
public class SkyHookSubsystem {
public DcMotorEx hook;
public SkyHookSubsystem(HardwareMap hardwareMap) {
hook = hardwareMap.get(DcMotorEx.class, SKYHOOK_NAME);
}
public void moveSkyHook(int position){
hook.setTargetPosition(position);
hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
public void moveSkyHook(double power) {
hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
hook.setPower(power);
}
public void init() {
hook.setPower(0);
hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
hook.setTargetPosition(0);
}
public void toParkPosition() {
moveSkyHook(0);
}
public void toLevel1Position() {
moveSkyHook(1500);
}
public int getHookPosition() {
return hook.getCurrentPosition();
}
}

104
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java
View File

@ -1,104 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_NAME;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristRung;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.ServoImplEx;
public class WristActionsSubsystem {
public enum WristState {
FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN
}
public ServoImplEx wrist;
public WristState state;
public WristActionsSubsystem(HardwareMap hardwareMap) {
this.wrist = hardwareMap.get(ServoImplEx.class, WRIST_NAME);
}
public class MoveToPosition implements Action {
private double positionValue;
private WristState positionState;
public MoveToPosition(double positionValue, WristState positionState) {
this.positionValue = positionValue;
this.positionState = positionState;
}
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
setState(positionState);
wrist.setPosition(positionValue);
telemetryPacket.put("Wrist State", positionState);
return false;
}
}
public Action toFloorPosition() {
return new MoveToPosition(wristFloor, WristState.FLOOR);
}
public Action toSpeciemenBar() {
return new MoveToPosition(wristSpeciemen, WristState.SPECIMEN);
}
public Action toRungPosition() {
return new MoveToPosition(wristFloor, WristState.FLOOR);
}
public Action toBucketPosition() {
return new MoveToPosition(wristBucket, WristState.BUCKET);
}
public Action toPickupPosition() {
return new MoveToPosition(wristPickup, WristState.PICKUP);
}
public void setState(WristState wristState) {
this.state = wristState;
}
public void switchState() {
if (state == WristState.FLOOR) {
Actions.runBlocking(this.toBucketPosition());
} else if (state == WristState.BUCKET) {
Actions.runBlocking(this.toFloorPosition());
}
}
public WristState getState() {
return this.state;
}
public void init() {
wrist.resetDeviceConfigurationForOpMode();
Actions.runBlocking(this.toFloorPosition());
}
public void start() {
Actions.runBlocking(this.toFloorPosition());
}
public void setPosition(double position) {
wrist.setPosition(position);
}
public double getPosition() {
return wrist.getPosition();
}
}

79
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java Normal file
View File

@ -0,0 +1,79 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import org.firstinspires.ftc.teamcode.util.action.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class WristSubsystem {
public enum WristState {
FLOOR, BUCKET
}
public ServoImplEx wrist;
public WristState state;
public RunAction floorWrist, bucketWrist;
public WristSubsystem(HardwareMap hardwareMap, WristState wristState) {
wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo");
wrist.resetDeviceConfigurationForOpMode();
this.state = wristState;
bucketWrist = new RunAction(this::bucketWrist);
floorWrist = new RunAction(this::floorWrist);
}
public void setState(WristState wristState) {
if (wristState == WristState.FLOOR) {
wrist.setPosition(wristFloor);
this.state = WristState.FLOOR;
} else if (wristState == WristState.BUCKET) {
wrist.setPosition(wristBucket);
this.state = WristState.BUCKET;
}
}
public void floorWrist() {
setState(WristState.FLOOR);
}
public void bucketWrist() {
setState(WristState.BUCKET);
}
public void switchState() {
if (state == WristState.FLOOR) {
setState(WristState.BUCKET);
} else if (state == WristState.BUCKET) {
setState(WristState.FLOOR);
}
}
public WristState getState() {
return this.state;
}
public void init() {
Actions.runBlocking(floorWrist);
}
public void start() {
Actions.runBlocking(floorWrist);
}
public double getPosition() {
return this.wrist.getPosition();
}
public void setPosition(double position) {
this.wrist.setPosition(position);
}
}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Action.java Normal file
View File

@ -0,0 +1,11 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
public interface Action {
boolean run(TelemetryPacket p);
default void preview(Canvas fieldOverlay) {
}
}

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java Normal file
View File

@ -0,0 +1,24 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
public class Actions {
public static void runBlocking(Action a) {
FtcDashboard dash = FtcDashboard.getInstance();
Canvas c = new Canvas();
a.preview(c);
boolean b = true;
while (b && !Thread.currentThread().isInterrupted()) {
TelemetryPacket p = new TelemetryPacket();
p.fieldOverlay().getOperations().addAll(c.getOperations());
b = a.run(p);
dash.sendTelemetryPacket(p);
}
}
}

39
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/FieldConstants.java Normal file
View File

@ -0,0 +1,39 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.config.Config;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
@Config
public class FieldConstants {
public enum RobotStart {
BLUE_BUCKET,
BLUE_OBSERVATION,
RED_BUCKET,
RED_OBSERVATION
}
public static final Pose blueBucketStartPose = new Pose(8, 79.5, Math.toRadians(180));
public static final Pose blueObservationStartPose = new Pose(8, 36, Math.toRadians(180));
public static final Pose redBucketStartPose = new Pose(144-8, 79.5, 0);
public static final Pose redObservationStartPose = new Pose(144-8, 36, 0);
// Blue Preload Poses
public static final Pose blueBucketPreloadPose = new Pose(34.5, 79.5, Math.toRadians(180));
// Blue Bucket Sample Poses
public static final Pose blueBucketLeftSamplePose = new Pose(34.75, 113.5, Math.toRadians(66));
public static final Pose blueBucketLeftSampleControlPose = new Pose(32, 108);
public static final Pose blueBucketMidSamplePose = new Pose(33, 125.5, Math.toRadians(73));
public static final Pose blueBucketMidSampleControlPose = new Pose(47.5, 110);
public static final Pose blueBucketRightSamplePose = new Pose(33, 133, Math.toRadians(74));
public static final Pose blueBucketRightSampleControlPose = new Pose(46, 101);
public static final Pose blueBucketScorePose = new Pose(16, 128, Math.toRadians(-45));
public static final Pose blueBucketParkPose = new Pose(65, 97.75, Math.toRadians(90));
public static final Pose blueBucketParkControlPose = new Pose(60.25, 123.5);
}

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/RunAction.java Normal file
View File

@ -0,0 +1,30 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
public class RunAction implements Action {
private final Runnable runnable;
private Runnable callback;
public RunAction(Runnable runnable) {
this.runnable = runnable;
}
public void runAction() {
runnable.run();
if (callback != null) {
callback.run();
}
}
public void setCallback(Runnable callback) {
this.callback = callback;
}
// Adapter to make Action compatible with the Action interface
public boolean run(TelemetryPacket p) {
runAction();
return false; // Regular actions complete after one execution
}
}

43
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SequentialAction.java Normal file
View File

@ -0,0 +1,43 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
public class SequentialAction implements Action {
private List<Action> actions;
public SequentialAction(List<Action> actions) {
this.actions = new ArrayList<>(actions);
}
public SequentialAction(Action... actions) {
this(Arrays.asList(actions));
}
@Override
public boolean run(TelemetryPacket p) {
if (actions.isEmpty()) {
return false;
}
if (actions.get(0).run(p)) {
return true;
} else {
actions.remove(0);
return run(p);
}
}
@Override
public void preview(Canvas fieldOverlay) {
for (Action a : actions) {
a.preview(fieldOverlay);
}
}
}

32
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java Normal file
View File

@ -0,0 +1,32 @@
package org.firstinspires.ftc.teamcode.util.action;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import java.util.Timer;
import java.util.TimerTask;
public class SleepAction implements Action {
private final double dt;
private boolean isFinished = false;
public SleepAction(double dt) {
this.dt = dt;
}
@Override
public boolean run(TelemetryPacket p) {
if (!isFinished) {
Timer timer = new Timer();
timer.schedule(new TimerTask() {
@Override
public void run() {
isFinished = true;
}
}, (long) (dt * 1000));
} else {
isFinished = false; // Reset the flag after sleep is complete
return false; // Indicate that the action is finished
}
return true; // Indicate that the action is still running
}
}

7
build.dependencies.gradle
View File

@ -16,9 +16,6 @@ dependencies {
implementation 'org.firstinspires.ftc:Vision:10.1.0'
implementation 'androidx.appcompat:appcompat:1.2.0'
implementation "com.acmerobotics.roadrunner:ftc:0.1.14"
implementation "com.acmerobotics.roadrunner:core:1.0.0"
implementation "com.acmerobotics.roadrunner:actions:1.0.0"
implementation "com.acmerobotics.dashboard:dashboard:0.4.16"
implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'
}