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

Committing working code

Browse Source
This commit is contained in:
Carlos Rivas
2024-11-11 18:39:49 -08:00
parent 345ea7d185
commit 6ccedc49b0
9 changed files with 621 additions and 626 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java
View File

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

194
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java
View File

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

214
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java
View File

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

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java
View File

@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunMode;
import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunModeCompetition; import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunModeCompetition;
@TeleOp(name = "Dev Teleop RR Actions", group = "Debug") @TeleOp(name = "Dev Teleop RR Actions", group = "Debug")

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

@ -23,6 +23,11 @@ public class PedroConstants {
public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD; public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD; public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
/*
Motor Max Power
*/
public static final double MAX_POWER = .75;
/* /*
IMU IMU
*/ */

217
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java
View File

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

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java
View File

@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.runmodes; package org.firstinspires.ftc.teamcode.runmodes;
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
import com.acmerobotics.roadrunner.SequentialAction; import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction; import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions; import com.acmerobotics.roadrunner.ftc.Actions;
@ -64,7 +66,7 @@ public class CometBotTeleopCompetition {
this.wrist.init(); this.wrist.init();
this.lift.init(); this.lift.init();
this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
follower.setMaxPower(.75); follower.setMaxPower(MAX_POWER);
follower.startTeleopDrive(); follower.startTeleopDrive();
} }

364
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java
View File

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

178
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java
View File

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