diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/CameraTestCommon.java b/TeamCode/src/main/java/org/timecrafters/testing/states/CameraTestCommon.java index 3092442..0697b14 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/CameraTestCommon.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/CameraTestCommon.java @@ -9,7 +9,7 @@ import java.util.List; public class CameraTestCommon { - private static final String TENSORFLOW_MODEL_ASSET = ""; + private static final String TENSORFLOW_MODEL_ASSET = "model_20221009_154335.tflite"; private static final String[] TENSORFLOW_MODEL_LABELS = { "1 dalek", "2 steve", diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java index 208371d..f82bcc8 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -31,18 +31,21 @@ public class PrototypeBot1 { frontRightDrive = engine.hardwareMap.dcMotor.get("Front Right"); backRightDrive = engine.hardwareMap.dcMotor.get("Back Left"); backLeftDrive = engine.hardwareMap.dcMotor.get("Back Right"); -// armMotor = engine.hardwareMap.dcMotor.get("Arm Motor"); // servo configuration + + // Collector collectorLeft = engine.hardwareMap.crservo.get("Collector Left"); collectorRight = engine.hardwareMap.crservo.get("Collector Right"); -// collectorWrist = engine.hardwareMap.servo.get("Collector Wrist"); + + // Arm LowRiserLeft = engine.hardwareMap.servo.get("LowRiserLeft"); LowRiserRight = engine.hardwareMap.servo.get("LowRiserRight"); HighRiserLeft = engine.hardwareMap.servo.get("HighRiserLeft"); HighRiserRight = engine.hardwareMap.servo.get("HighRiserRight"); //motors direction and encoders + frontLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD); frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -55,8 +58,5 @@ public class PrototypeBot1 { backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); -// armMotor.setDirection(DcMotorSimple.Direction.FORWARD); -// armMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } } diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java index bccb7c7..0c010cd 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java @@ -14,7 +14,7 @@ public class PrototypeTeleOPState extends CyberarmState { public boolean A; public boolean X; public boolean Y; - private boolean bprev; + private boolean bprev; // sticky key variable for the gamepad private double drivePower = 1; private boolean UpDPad; private int armTargetPosition = 0, armCollectPosition = 125, armDeliverPosition = 400; @@ -22,8 +22,9 @@ public class PrototypeTeleOPState extends CyberarmState { private boolean raiseHighRiser = true; private long lastStepTime = 0; private boolean raiseLowRiser = true; - private double speed = 0.5; - private double slowSpeed = 0.25; + private double speed = 0.5; // used for the normal speed while driving + private double slowSpeed = 0.25; // used for slow mode speed while driving + private int CyclingArmUpAndDown = 0; public PrototypeTeleOPState(PrototypeBot1 robot) { this.robot = robot; @@ -77,13 +78,16 @@ public class PrototypeTeleOPState extends CyberarmState { boolean b = engine.gamepad1.b; if (b && !bprev) { - + bprev = true; if (drivePower == speed) { drivePower = slowSpeed; } else { drivePower = speed; } } + if (!b){ + bprev = false; + } //Drivetrain Variables double y = -engine.gamepad1.left_stick_y * 0.5; // Remember, this is reversed! because of the negative @@ -100,17 +104,11 @@ public class PrototypeTeleOPState extends CyberarmState { double frontRightPower = (y - x - rx) / denominator; double backRightPower = (y + x - rx) / denominator; - // As I programmed this and ran it, I realized everything was backwards - // in direction so to fix that I either went in the robot object state and reversed - // directions on drive motors or put a negative in behind the joystick power to reverse it. - // I put negatives in to reverse it because it was the easiest at the moment. - robot.frontLeftDrive.setPower(-frontLeftPower * speed); robot.backLeftDrive.setPower(backLeftPower * speed); robot.frontRightDrive.setPower(-frontRightPower * speed); robot.backRightDrive.setPower(backRightPower * speed); -// robot.armMotor.setPower(engine.gamepad2.left_stick_y * 0.5); if (engine.gamepad2.y) { @@ -205,29 +203,83 @@ public class PrototypeTeleOPState extends CyberarmState { robot.LowRiserLeft.setPosition(0.45); } - if (engine.gamepad2.start) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); +// if (engine.gamepad2.start) { +// if (System.currentTimeMillis() - lastStepTime >= 150) { +// lastStepTime = System.currentTimeMillis(); +// +// if (raiseHighRiser) { +// if (robot.HighRiserLeft.getPosition() >= 1) { +// if (raiseLowRiser) { +// raiseHighRiser = false; +// } +// } else { +// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); +// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); +// } +// } else { +// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.035); +// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.035); +// +// if (robot.HighRiserLeft.getPosition() <= 0.45) { +// raiseHighRiser = true; +// } +// } +// } +// } + // SPENCER____________________________________________________________________ + if (engine.gamepad2.start){ - if (raiseHighRiser) { - if (robot.HighRiserLeft.getPosition() >= 1) { - if (raiseLowRiser) { - raiseHighRiser = false; - } - } else { - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); - } - } else { - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.035); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.035); + if (System.currentTimeMillis() - lastStepTime >= 150) { + lastStepTime = System.currentTimeMillis(); - if (robot.HighRiserLeft.getPosition() <= 0.45) { - raiseHighRiser = true; - } - } - } - } + switch (CyclingArmUpAndDown) { + + // upper arm up + case 0: + if (robot.HighRiserLeft.getPosition() <= 1) { + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); + } else { + CyclingArmUpAndDown = CyclingArmUpAndDown +1; + } + break; + + // lower arm up + case 1: + if (robot.LowRiserLeft.getPosition() <= 1) { + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); + } else { + CyclingArmUpAndDown = CyclingArmUpAndDown +1; + } + break; + + // lower arm down + case 2: + if (robot.LowRiserLeft.getPosition() >= 0.45) { + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); + } else { + CyclingArmUpAndDown = CyclingArmUpAndDown +1; + } + break; + + // upper arm down + case 3: + if (robot.HighRiserLeft.getPosition() >= 0.45) { + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); + } else { + CyclingArmUpAndDown = 0; + } + break; + + default: + break; + + } // end of switch + } // end of time if statement + } // end of start button press // if (engine.gamepad2.left_stick_y > 0.1) { // robot.HighRiserLeft.setPosition(0.5);