Compare commits
1 Commits
branch-rc-
...
branch-ped
| Author | SHA1 | Date | |
|---|---|---|---|
| c1023e831b |
@ -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);
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -1,161 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
@Autonomous(name = "Blue Non Basket Auto", group = "Competition")
|
||||
public class BlueNonBasketAuto extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private PathChain path2;
|
||||
|
||||
private final Pose startPose = new Pose(8.000, 55.000);
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.6);
|
||||
|
||||
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)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0)).build();
|
||||
follower.followPath(path);
|
||||
|
||||
path2 = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(62.357, 33.107, Point.CARTESIAN),
|
||||
new Point(62.000, 27.000, 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)
|
||||
)
|
||||
)
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
|
||||
|
||||
follower.followPath(path2);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -1,12 +0,0 @@
|
||||
# Controller 1
|
||||
|
||||
## Motor Controls
|
||||
|
||||
- Left Joystick
|
||||
- Forward & Backwards
|
||||
- Right Joystick
|
||||
- Strafe & Turning
|
||||
|
||||
## Arm Controls
|
||||
|
||||
-
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
@ -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.CometBotTeleopCompetition;
|
||||
|
||||
@TeleOp(name = "ComeBot Drive", group = "Competition")
|
||||
public class CometBotDrive extends OpMode {
|
||||
|
||||
public CometBotTeleopCompetition runMode;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
this.runMode = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
|
||||
this.runMode.init();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
this.runMode.update();
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,92 @@
|
||||
/*
|
||||
Copyright (c) 2024 Alan Smith
|
||||
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 Alan Smith 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 FITNESSFOR 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;
|
||||
|
||||
/*
|
||||
* This OpMode illustrates how to use the goBILDA Indicator Light
|
||||
*
|
||||
* To make this work, this Light uses a Servo motor and setPositions.
|
||||
* Valid values are from .277 to .722.
|
||||
*
|
||||
* If it's less than .277 then no light
|
||||
* If it's greater than .722 then white light
|
||||
*
|
||||
* Spec Sheet is here: https://cdn11.bigcommerce.com/s-x56mtydx1w/images/stencil/original/products/2275/13464/3118-0808-0002-Product-Insight-4__03940__01348.1728056113.png?c=1
|
||||
*/
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
@TeleOp(name = "Demo: goBILDA Indicator Light", group = "Concept")
|
||||
public class DemoGoBildaIndicatorLight extends LinearOpMode {
|
||||
Servo front_led;
|
||||
private final double buffer = .277;
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
front_led = hardwareMap.get(Servo.class, "test_servo");
|
||||
|
||||
waitForStart();
|
||||
|
||||
while (opModeIsActive()) {
|
||||
double left_x = Math.abs(gamepad1.left_stick_x) / 4;
|
||||
double left_y = Math.abs(gamepad1.left_stick_y) / 4;
|
||||
double right_x = Math.abs(gamepad1.right_stick_x) / 4;
|
||||
double right_y = Math.abs(gamepad1.right_stick_y) / 4;
|
||||
double total_position = Math.min(left_x + left_y + right_x + right_y + buffer, 1.0);
|
||||
|
||||
telemetry.addData("left_x", left_x);
|
||||
telemetry.addData("left_y", left_y);
|
||||
telemetry.addData("right_x", right_x);
|
||||
telemetry.addData("right_y", right_y);
|
||||
telemetry.addData("total (plus buffer)", total_position);
|
||||
telemetry.update();
|
||||
|
||||
front_led.setPosition(total_position);
|
||||
|
||||
if (gamepad1.cross) {
|
||||
front_led.setPosition(.611);
|
||||
sleep(2000);
|
||||
}
|
||||
if (gamepad1.triangle) {
|
||||
front_led.setPosition(.444);
|
||||
sleep(2000);
|
||||
}
|
||||
if (gamepad1.square) {
|
||||
front_led.setPosition(.7);
|
||||
sleep(2000);
|
||||
}
|
||||
if (gamepad1.circle) {
|
||||
front_led.setPosition(.3);
|
||||
sleep(2000);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -8,78 +8,39 @@ 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";
|
||||
|
||||
/*
|
||||
Motor directions
|
||||
*/
|
||||
// Robot motor configurations
|
||||
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";
|
||||
public static final double BACK_ENCODER_DIRECTION = Encoder.REVERSE;
|
||||
|
||||
/*
|
||||
Pedro's parameters
|
||||
@ -89,21 +50,21 @@ public class PedroConstants {
|
||||
public static final double ROBOT_WEIGHT_IN_KG = 10.5;
|
||||
|
||||
// Maximum velocity of the robot going forward
|
||||
public static final double ROBOT_SPEED_FORWARD = 51.4598;
|
||||
public static final double ROBOT_SPEED_FORWARD = 72.0693;
|
||||
|
||||
// Maximum velocity of the robot going right
|
||||
public static final double ROBOT_SPEED_LATERAL = 28.7119;
|
||||
public static final double ROBOT_SPEED_LATERAL = 24.1401;
|
||||
|
||||
// Rate of deceleration when power is cut-off when the robot is moving forward
|
||||
public static final double FORWARD_ZERO_POWER_ACCEL = -59.805;
|
||||
public static final double FORWARD_ZERO_POWER_ACCEL = -74.3779;
|
||||
|
||||
// Rate of deceleration when power is cut-off when the robot is moving to the right
|
||||
public static final double LATERAL_ZERO_POWER_ACCEL = -99.672;
|
||||
public static final double LATERAL_ZERO_POWER_ACCEL = -111.8409;
|
||||
|
||||
// Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
|
||||
public static final double ZERO_POWER_ACCEL_MULT = 3.5;
|
||||
public static final double ZERO_POWER_ACCEL_MULT = 4;
|
||||
|
||||
/* Centripetal force correction - increase if robot is correcting into the path
|
||||
- decrease if robot is correcting away from the path */
|
||||
public static final double CENTRIPETAL_SCALING = 0.0004;
|
||||
public static final double CENTRIPETAL_SCALING = 0.0005;
|
||||
}
|
||||
|
||||
@ -1,124 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
|
||||
@Autonomous(name = "Pre Loaded Blue Basket Auto", group = "Competition")
|
||||
public class PreLoadedBlueBasketAuto extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(7.875, 89.357);
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.45);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(8, 89, Point.CARTESIAN),
|
||||
new Point(10.125, 126.804, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 5
|
||||
new BezierLine(
|
||||
new Point(59.625, 126.964, Point.CARTESIAN),
|
||||
new Point(15.107, 130.339, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 6
|
||||
new BezierLine(
|
||||
new Point(15.107, 130.339, Point.CARTESIAN),
|
||||
new Point(59.625, 126.964, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 7
|
||||
new BezierLine(
|
||||
new Point(59.625, 126.964, Point.CARTESIAN),
|
||||
new Point(57.857, 131.071, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.addPath(
|
||||
// Line 8
|
||||
new BezierLine(
|
||||
new Point(57.857, 131.071, Point.CARTESIAN),
|
||||
new Point(18.964, 131.679, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.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)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(0), 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);
|
||||
|
||||
}
|
||||
}
|
||||
@ -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;
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
@ -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()
|
||||
)
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -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()
|
||||
)
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//
|
||||
//}
|
||||
@ -1,91 +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.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.ArmActionsSubsystem;
|
||||
|
||||
@TeleOp(name = "Arm Test", group = "Debug")
|
||||
public class ArmTest extends LinearOpMode {
|
||||
|
||||
// Declare OpMode members for each of the 4 motors.
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
|
||||
/*
|
||||
* Instantiate Arm
|
||||
*/
|
||||
ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
|
||||
|
||||
/*
|
||||
* Instantiate gamepad state holders
|
||||
*/
|
||||
Gamepad currentGamepad1 = new Gamepad();
|
||||
Gamepad previousGamepad1 = new Gamepad();
|
||||
|
||||
arm.init();
|
||||
waitForStart();
|
||||
runtime.reset();
|
||||
|
||||
// run until the end of the match (driver presses STOP)
|
||||
while (opModeIsActive()) {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
if (currentGamepad1.circle && !previousGamepad1.circle) {
|
||||
arm.toParkPosition();
|
||||
}
|
||||
|
||||
if (currentGamepad1.square && !previousGamepad1.square) {
|
||||
arm.toBucketPosition();
|
||||
}
|
||||
|
||||
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
|
||||
arm.setPosition(arm.getPosition() - .05);
|
||||
}
|
||||
|
||||
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
|
||||
arm.setPosition(arm.getPosition() + .05);
|
||||
}
|
||||
|
||||
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||
telemetry.addData("Arm State", arm.getState());
|
||||
telemetry.addData("Arm Position", arm.getPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,80 +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.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.ClawActionsSubsystem;
|
||||
|
||||
|
||||
@TeleOp(name = "Claw Test", group = "Debug")
|
||||
public class ClawTest extends LinearOpMode {
|
||||
|
||||
// Declare OpMode members for each of the 4 motors.
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
|
||||
/*
|
||||
* Instantiate Claw
|
||||
*/
|
||||
ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
|
||||
|
||||
/*
|
||||
* Instantiate gamepad state holders
|
||||
*/
|
||||
Gamepad currentGamepad1 = new Gamepad();
|
||||
Gamepad previousGamepad1 = new Gamepad();
|
||||
|
||||
waitForStart();
|
||||
claw.init();
|
||||
runtime.reset();
|
||||
|
||||
// run until the end of the match (driver presses STOP)
|
||||
while (opModeIsActive()) {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
if (currentGamepad1.cross && !previousGamepad1.cross) {
|
||||
claw.switchState();
|
||||
}
|
||||
|
||||
// Show the elapsed game time and wheel power.
|
||||
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||
telemetry.addData("Claw State", claw.getState());
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,100 +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;
|
||||
|
||||
@TeleOp(name = "Lift Test", group = "Debug")
|
||||
public class LiftTest extends LinearOpMode {
|
||||
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
|
||||
/*
|
||||
* Instantiate Lift
|
||||
*/
|
||||
LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap);
|
||||
|
||||
/*
|
||||
* Instantiate gamepad state holders
|
||||
*/
|
||||
Gamepad currentGamepad1 = new Gamepad();
|
||||
Gamepad previousGamepad1 = new Gamepad();
|
||||
|
||||
lift.init();
|
||||
waitForStart();
|
||||
runtime.reset();
|
||||
|
||||
|
||||
// run until the end of the match (driver presses STOP)
|
||||
while (opModeIsActive()) {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
if (currentGamepad1.square && !previousGamepad1.square) {
|
||||
Actions.runBlocking(lift.toFloorPosition());
|
||||
}
|
||||
|
||||
if (currentGamepad1.triangle && !previousGamepad1.triangle) {
|
||||
Actions.runBlocking(lift.toHighBucketPosition());
|
||||
}
|
||||
|
||||
if (currentGamepad1.circle && !previousGamepad1.circle) {
|
||||
Actions.runBlocking(lift.toLowBucketPosition());
|
||||
}
|
||||
|
||||
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());
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,137 +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.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.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 {
|
||||
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
@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);
|
||||
|
||||
|
||||
/*
|
||||
* Instantiate gamepad state holders
|
||||
*/
|
||||
Gamepad currentGamepad1 = new Gamepad();
|
||||
Gamepad previousGamepad1 = new Gamepad();
|
||||
|
||||
lift.init();
|
||||
wrist.init();
|
||||
arm.init();
|
||||
claw.init();
|
||||
waitForStart();
|
||||
runtime.reset();
|
||||
|
||||
|
||||
// run until the end of the match (driver presses STOP)
|
||||
while (opModeIsActive()) {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
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_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())
|
||||
);
|
||||
|
||||
//Delete open claw
|
||||
}
|
||||
|
||||
|
||||
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() + 100);
|
||||
}
|
||||
|
||||
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
|
||||
if (lift.getPosition() > 1000) {
|
||||
lift.setPosition(lift.getPosition()-1000);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,97 +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.WristActionsSubsystem;
|
||||
|
||||
@TeleOp(name = "Wrist Test", group = "Debug")
|
||||
public class WristTest extends LinearOpMode {
|
||||
|
||||
// Declare OpMode members for each of the 4 motors.
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
|
||||
/*
|
||||
* Instantiate Wrist
|
||||
*/
|
||||
WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap);
|
||||
|
||||
/*
|
||||
* Instantiate gamepad state holders
|
||||
*/
|
||||
Gamepad currentGamepad1 = new Gamepad();
|
||||
Gamepad previousGamepad1 = new Gamepad();
|
||||
|
||||
wrist.init();
|
||||
waitForStart();
|
||||
runtime.reset();
|
||||
|
||||
// run until the end of the match (driver presses STOP)
|
||||
while (opModeIsActive()) {
|
||||
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
currentGamepad1.copy(gamepad1);
|
||||
|
||||
if (currentGamepad1.square && !previousGamepad1.square) {
|
||||
Actions.runBlocking(wrist.toBucketPosition());
|
||||
}
|
||||
|
||||
if (currentGamepad1.circle && !previousGamepad1.circle) {
|
||||
Actions.runBlocking(wrist.toFloorPosition());
|
||||
}
|
||||
|
||||
if (currentGamepad1.cross && !previousGamepad1.cross) {
|
||||
wrist.switchState();
|
||||
}
|
||||
|
||||
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
|
||||
wrist.setPosition(wrist.getPosition() - .05);
|
||||
}
|
||||
|
||||
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
|
||||
wrist.setPosition(wrist.getPosition() + .05);
|
||||
}
|
||||
|
||||
// Show the elapsed game time and wheel power.
|
||||
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||
telemetry.addData("Wrist State", wrist.getState());
|
||||
telemetry.addData("Wrist Position", wrist.getPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,33 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.configs;
|
||||
|
||||
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 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 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;
|
||||
}
|
||||
@ -71,9 +71,7 @@ measurements will be in centimeters.
|
||||
of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but
|
||||
what works best for you is most important. Higher numbers will cause a faster brake, but increase
|
||||
oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in
|
||||
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`.
|
||||
|
||||
* The drive PID is much, much more sensitive than the others. For reference,
|
||||
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more sensitive than the others. For reference,
|
||||
my P values were in the hundredths and thousandths place values, and my D values were in the hundred
|
||||
thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and
|
||||
`useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth`
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -5,7 +5,6 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.DriveEncoderLocalizer;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelIMULocalizer;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.TwoWheelLocalizer;
|
||||
@ -70,8 +69,7 @@ public class PoseUpdater {
|
||||
*/
|
||||
public PoseUpdater(HardwareMap hardwareMap) {
|
||||
// TODO: replace the second argument with your preferred localizer
|
||||
this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
|
||||
// this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
|
||||
this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -38,9 +38,9 @@ public class DriveEncoderLocalizer extends Localizer {
|
||||
private Encoder leftRear;
|
||||
private Encoder rightRear;
|
||||
private double totalHeading;
|
||||
public static double FORWARD_TICKS_TO_INCHES = -0.6308;
|
||||
public static double STRAFE_TICKS_TO_INCHES = 46.4839;
|
||||
public static double TURN_TICKS_TO_RADIANS = -0.002;
|
||||
public static double FORWARD_TICKS_TO_INCHES = 1;
|
||||
public static double STRAFE_TICKS_TO_INCHES = 1;
|
||||
public static double TURN_TICKS_TO_RADIANS = 1;
|
||||
public static double ROBOT_WIDTH = 1;
|
||||
public static double ROBOT_LENGTH = 1;
|
||||
|
||||
|
||||
@ -57,12 +57,9 @@ public class ThreeWheelLocalizer extends Localizer {
|
||||
private Pose rightEncoderPose;
|
||||
private Pose strafeEncoderPose;
|
||||
private double totalHeading;
|
||||
// public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
||||
public static double FORWARD_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
||||
// public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
||||
public static double STRAFE_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
||||
// public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
||||
public static double TURN_TICKS_TO_RADIANS = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
||||
public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
||||
public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
||||
public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
||||
|
||||
/**
|
||||
* This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
|
||||
@ -83,9 +80,9 @@ public class ThreeWheelLocalizer extends Localizer {
|
||||
*/
|
||||
public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) {
|
||||
// TODO: replace these with your encoder positions
|
||||
leftEncoderPose = new Pose(0, 6.19375, 0);
|
||||
rightEncoderPose = new Pose(0, -6.19375, 0);
|
||||
strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
|
||||
leftEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0);
|
||||
rightEncoderPose = new Pose(-18.4/25.4 - 0.1, -159.6/25.4, 0);
|
||||
strafeEncoderPose = new Pose(0*(-107.9/25.4+8)+-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90));
|
||||
|
||||
hardwareMap = map;
|
||||
|
||||
@ -95,9 +92,9 @@ public class ThreeWheelLocalizer extends Localizer {
|
||||
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);
|
||||
//leftEncoder.setDirection(Encoder.REVERSE);
|
||||
// rightEncoder.setDirection(Encoder.REVERSE);
|
||||
//strafeEncoder.setDirection(Encoder.FORWARD);
|
||||
|
||||
setStartPose(setStartPose);
|
||||
timer = new NanoTimer();
|
||||
|
||||
@ -63,7 +63,6 @@ public class LateralTuner extends OpMode {
|
||||
telemetryA.addData("distance moved", poseUpdater.getPose().getY());
|
||||
telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
|
||||
telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getY() / poseUpdater.getLocalizer().getLateralMultiplier()));
|
||||
|
||||
telemetryA.update();
|
||||
|
||||
Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50");
|
||||
|
||||
@ -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<>();
|
||||
|
||||
@ -42,7 +42,7 @@ public class FollowerConstants {
|
||||
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
|
||||
0.1,
|
||||
0,
|
||||
0.01,
|
||||
0,
|
||||
0);
|
||||
|
||||
// Translational Integral
|
||||
@ -58,9 +58,9 @@ public class FollowerConstants {
|
||||
|
||||
// Heading error PIDF coefficients
|
||||
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
|
||||
2,
|
||||
1,
|
||||
0,
|
||||
0,
|
||||
.025,
|
||||
0);
|
||||
|
||||
// Feed forward constant added on to the heading PIDF
|
||||
@ -69,10 +69,10 @@ public class FollowerConstants {
|
||||
|
||||
// Drive PIDF coefficients
|
||||
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
|
||||
0.006,
|
||||
0.025,
|
||||
0,
|
||||
0.00001,
|
||||
0.8,
|
||||
0.6,
|
||||
0);
|
||||
|
||||
// Feed forward constant added on to the drive PIDF
|
||||
@ -81,7 +81,7 @@ public class FollowerConstants {
|
||||
// Kalman filter parameters for the drive error Kalman filter
|
||||
public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters(
|
||||
6,
|
||||
3);
|
||||
1);
|
||||
|
||||
|
||||
// Mass of robot in kilograms
|
||||
|
||||
@ -119,13 +119,6 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
|
||||
telemetry.addData("x",poseUpdater.getPose().getX());
|
||||
telemetry.addData("y",poseUpdater.getPose().getY());
|
||||
telemetry.addData("heading",poseUpdater.getPose().getHeading());
|
||||
telemetry.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
|
||||
telemetry.addData("velo theta", poseUpdater.getVelocity().getTheta());
|
||||
|
||||
if (gamepad1.cross || gamepad1.a) {
|
||||
requestOpModeStop();
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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());
|
||||
}
|
||||
|
||||
}
|
||||
@ -1,140 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystem;
|
||||
|
||||
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.Gamepad;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
|
||||
public class MotorsSubsystem {
|
||||
|
||||
public HardwareMap hardwareMap;
|
||||
public Telemetry telemetry;
|
||||
|
||||
public DcMotor frontLeftMotor;
|
||||
public DcMotor backLeftMotor;
|
||||
public DcMotor frontRightMotor;
|
||||
public DcMotor backRightMotor;
|
||||
|
||||
public enum TravelState {
|
||||
STOPPED, MOVING
|
||||
}
|
||||
|
||||
public TravelState travelState;
|
||||
|
||||
public double power;
|
||||
|
||||
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
|
||||
this.hardwareMap = hardwareMap;
|
||||
this.telemetry = telemetry;
|
||||
this.power = 1.0;
|
||||
}
|
||||
|
||||
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) {
|
||||
this.hardwareMap = hardwareMap;
|
||||
this.telemetry = telemetry;
|
||||
this.power = power;
|
||||
}
|
||||
|
||||
public void 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);
|
||||
|
||||
frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
this.setState(TravelState.STOPPED);
|
||||
}
|
||||
|
||||
public void setFrontLeftMotorPower(double power) {
|
||||
frontLeftMotor.setPower(power);
|
||||
}
|
||||
|
||||
public void setBackLeftMotorPower(double power) {
|
||||
backLeftMotor.setPower(power);
|
||||
}
|
||||
|
||||
public void setFrontRightMotorPower(double power) {
|
||||
frontRightMotor.setPower(power);
|
||||
}
|
||||
|
||||
public void setBackRightMotorPower(double power) {
|
||||
backRightMotor.setPower(power);
|
||||
}
|
||||
|
||||
public void calculateTrajectory(Gamepad gamepad1) {
|
||||
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 leftBackPower = axial - lateral + yaw;
|
||||
|
||||
double rightFrontPower = 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
|
||||
this.setFrontLeftMotorPower(leftFrontPower * this.power);
|
||||
this.setFrontRightMotorPower(rightFrontPower * this.power);
|
||||
this.setBackLeftMotorPower(leftBackPower * this.power);
|
||||
this.setBackRightMotorPower(rightBackPower * this.power);
|
||||
|
||||
// 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());
|
||||
}
|
||||
|
||||
public void setState(TravelState travelState) {
|
||||
this.travelState = travelState;
|
||||
}
|
||||
|
||||
public TravelState getState() {
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
|
||||
}
|
||||
@ -6,19 +6,16 @@ repositories {
|
||||
}
|
||||
|
||||
dependencies {
|
||||
implementation 'org.firstinspires.ftc:Inspection:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:Blocks:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:RobotCore:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:RobotServer:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:OnBotJava:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:Hardware:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:FtcCommon:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:Vision:10.1.0'
|
||||
implementation 'org.firstinspires.ftc:Inspection:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:Blocks:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:RobotCore:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:RobotServer:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:OnBotJava:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:Hardware:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:FtcCommon:10.0.0'
|
||||
implementation 'org.firstinspires.ftc:Vision:10.0.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'
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user