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

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

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

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

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

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.TeleOp;
import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunMode;
import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunModeCompetition;
@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 BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
/*
Motor Max Power
*/
public static final double MAX_POWER = .75;
/*
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;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
import org.firstinspires.ftc.teamcode.pedroPathing.util.Timer;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.*;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.util.action.Action;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
import org.firstinspires.ftc.teamcode.util.action.SequentialAction;
import org.firstinspires.ftc.teamcode.util.action.SleepAction;
public class Auto {
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public Timer clawTimer = new Timer();
public Timer armTimer = new Timer();
public Timer wristTimer = new Timer();
public Follower follower;
public Telemetry telemetry;
public int caseState = 1;
public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) {
claw = new ClawSubsystem(hardwareMap, 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;
}
}
}
//package org.firstinspires.ftc.teamcode.runmodes;
//
//import com.qualcomm.robotcore.hardware.HardwareMap;
//
//import org.firstinspires.ftc.robotcore.external.Telemetry;
//import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
//import org.firstinspires.ftc.teamcode.pedroPathing.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;
// }
// }
//}

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

@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.runmodes;
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;
@ -64,7 +66,7 @@ public class CometBotTeleopCompetition {
this.wrist.init();
this.lift.init();
this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
follower.setMaxPower(.75);
follower.setMaxPower(MAX_POWER);
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;
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);
}
}
}
}
//package org.firstinspires.ftc.teamcode.runmodes;
//
//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);
// }
// }
// }
//
//}

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

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