diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java index 6e5c723..8cff5c2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java @@ -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(); - } - -} \ No newline at end of file +//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(); +// } +// +//} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java index 9aab7d3..e70c269 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java @@ -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); +// +// } +// +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java index 73a3805..4a61659 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java @@ -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(); +// +// } +// +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java index d57ea90..cd47905 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java @@ -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") diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java index 0ad4cfd..ead4ed1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -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 */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java index d4e0460..9338f21 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java @@ -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; +// } +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java index a80893d..76d8279 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java @@ -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(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java index 8564430..aa2bc46 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java @@ -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); +// } +// } +// } +// +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java index eae8c58..cf1cdc1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java @@ -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); +// } +//}