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 9def35f..d9ca819 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java @@ -15,7 +15,6 @@ public class PrototypeTeleOPState extends CyberarmState { private boolean bprev; // sticky key variable for the gamepad private double drivePower = 1; private boolean UpDPad; - private int armTargetPosition = 0, armCollectPosition = 125, armDeliverPosition = 400; private double collectorRiserPosition; private boolean raiseHighRiser = true; private long lastStepTime = 0, BeginningofActionTime; @@ -237,6 +236,20 @@ public class PrototypeTeleOPState extends CyberarmState { } } + if (engine.gamepad1.dpad_left) { + robot.collectorLeft.setPower(1); + robot.collectorRight.setPower(1); + } else if (engine.gamepad1.dpad_right) { + robot.collectorLeft.setPower(-1); + robot.collectorRight.setPower(-1); + } else { + robot.collectorLeft.setPower(0); + robot.collectorRight.setPower(0); + } + + + + // // } // @@ -254,13 +267,6 @@ public class PrototypeTeleOPState extends CyberarmState { // // } //// -//// if (engine.gamepad1.a) { -//// robot.LowRiserLeft.setPosition(0); -//// robot.LowRiserRight.setPosition(1); -//// } -//// if (engine.gamepad1.x) { -//// robot.LowRiserLeft.setPosition(1); -//// robot.LowRiserRight.setPosition(0); //// } //// //// if (engine.gamepad2.right_stick_y < -0.1) { @@ -365,59 +371,59 @@ public class PrototypeTeleOPState extends CyberarmState { // // } // -// if (engine.gamepad2.start){ -// -// if (System.currentTimeMillis() - lastStepTime >= 150) { +//// if (engine.gamepad2.start){ +//// +//// if (System.currentTimeMillis() - lastStepTime >= 150) { // lastStepTime = System.currentTimeMillis(); -// -// 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.44) { -// 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 + 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.44) { + 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); @@ -440,204 +446,204 @@ public class PrototypeTeleOPState extends CyberarmState { // } // For raising, high risers ALWAYS raise first, for lowering, low risers ALWAYS lower first. - if (engine.gamepad2.back) { - RobotPosition = robot.backRightDrive.getCurrentPosition(); - - switch (AutoChain) { - - case 0://Initialize - RobotStartingPosition = RobotPosition; - AutoChain += 1; - break; - - case 1://Drive 1 square forward - if (RobotPosition - RobotStartingPosition < 2500){ - drivePower = 1; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - } else - { - AutoChain += 1; - } - break; - - case 2://Rotate Counterclockwise for 45 degrees - RobotRotation = robot.imu.getAngularOrientation().firstAngle; - if (RobotRotation <= 45) { - drivePower = 0.4; - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(drivePower); - } else - { - AutoChain += 1; - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - } - break; - - case 3://Raise upper arm fully - if (robot.HighRiserLeft.getPosition() < 1) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); - } - } else { - AutoChain +=1; - } - break; - - case 4://Raise lower arm fully - if (robot.LowRiserLeft.getPosition() < 1) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } - } else { - AutoChain += 1; - - } - break; - - case 5://initialize for moving forward - RobotStartingPosition = robot.backRightDrive.getCurrentPosition(); - AutoChain += 1; - break; - - case 6://Drive forward 1/4 square - if (RobotPosition - RobotStartingPosition < 1200){ - drivePower = 1; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - } else - { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - AutoChain += 1; - } - break; - - case 7://Lower low arm fully - if (robot.LowRiserLeft.getPosition() > 0.5) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - - } - }else - { - AutoChain += 1; - } - break; - - case 8: - BeginningofActionTime = System.currentTimeMillis(); - AutoChain += 1; - - break; - case 9://Eject - if (System.currentTimeMillis() - BeginningofActionTime < 2000) { - robot.collectorRight.setPower(-1); - robot.collectorLeft.setPower(1); - - } else { - robot.collectorLeft.setPower(0); - robot.collectorRight.setPower(0); - AutoChain += 1; - } - break; - - case 10://Raise low arm - if (robot.LowRiserLeft.getPosition() < 1) { - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } else { - AutoChain += 1; - } - break; - - case 11://Initialize backup - RobotStartingPosition = RobotPosition; - AutoChain += 1; - break; - - case 12://Drive backwards 1/4 square - if (RobotPosition - RobotStartingPosition > -1200){ - drivePower = -1; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - } else - { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - AutoChain += 1; - } - break; - - case 13://Turn 45 degrees clockwise - RobotRotation = robot.imu.getAngularOrientation().firstAngle; - if (RobotRotation > 0) { - drivePower = 0.4; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } else - { - AutoChain += 1; - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - } - break; - - case 14://Initialize - RobotStartingPosition = RobotPosition; - AutoChain += 1; - break; - - case 15://Drive 1 square forward - if (RobotPosition - RobotStartingPosition < 2500){ - drivePower = 1; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - } else - { - AutoChain = 999; - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - } - break; - - case 999: - - break; +// if (engine.gamepad2.back) { +// RobotPosition = robot.backRightDrive.getCurrentPosition(); +// +// switch (AutoChain) { +// +// case 0://Initialize +// RobotStartingPosition = RobotPosition; +// AutoChain += 1; +// break; +// +// case 1://Drive 1 square forward +// if (RobotPosition - RobotStartingPosition < 2500){ +// drivePower = 1; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } else +// { +// AutoChain += 1; +// } +// break; +// +// case 2://Rotate Counterclockwise for 45 degrees +// RobotRotation = robot.imu.getAngularOrientation().firstAngle; +// if (RobotRotation <= 45) { +// drivePower = 0.4; +// robot.backLeftDrive.setPower(-drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(-drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } else +// { +// AutoChain += 1; +// drivePower = 0; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } +// break; +// +// case 3://Raise upper arm fully +// if (robot.HighRiserLeft.getPosition() < 1) { +// if (System.currentTimeMillis() - lastStepTime >= 150) { +// lastStepTime = System.currentTimeMillis(); +// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); +// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); +// } +// } else { +// AutoChain +=1; +// } +// break; +// +// case 4://Raise lower arm fully +// if (robot.LowRiserLeft.getPosition() < 1) { +// if (System.currentTimeMillis() - lastStepTime >= 150) { +// lastStepTime = System.currentTimeMillis(); +// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); +// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); +// } +// } else { +// AutoChain += 1; +// +// } +// break; +// +// case 5://initialize for moving forward +// RobotStartingPosition = robot.backRightDrive.getCurrentPosition(); +// AutoChain += 1; +// break; +// +// case 6://Drive forward 1/4 square +// if (RobotPosition - RobotStartingPosition < 1200){ +// drivePower = 1; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } else +// { +// drivePower = 0; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// AutoChain += 1; +// } +// break; +// +// case 7://Lower low arm fully +// if (robot.LowRiserLeft.getPosition() > 0.5) { +// if (System.currentTimeMillis() - lastStepTime >= 150) { +// lastStepTime = System.currentTimeMillis(); +// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); +// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); +// +// } +// }else +// { +// AutoChain += 1; +// } +// break; +// +// case 8: +// BeginningofActionTime = System.currentTimeMillis(); +// AutoChain += 1; +// +// break; +// case 9://Eject +// if (System.currentTimeMillis() - BeginningofActionTime < 2000) { +// robot.collectorRight.setPower(-1); +// robot.collectorLeft.setPower(1); +// +// } else { +// robot.collectorLeft.setPower(0); +// robot.collectorRight.setPower(0); +// AutoChain += 1; +// } +// break; +// +// case 10://Raise low arm +// if (robot.LowRiserLeft.getPosition() < 1) { +// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); +// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); +// } else { +// AutoChain += 1; +// } +// break; +// +// case 11://Initialize backup +// RobotStartingPosition = RobotPosition; +// AutoChain += 1; +// break; +// +// case 12://Drive backwards 1/4 square +// if (RobotPosition - RobotStartingPosition > -1200){ +// drivePower = -1; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } else +// { +// drivePower = 0; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// AutoChain += 1; +// } +// break; +// +// case 13://Turn 45 degrees clockwise +// RobotRotation = robot.imu.getAngularOrientation().firstAngle; +// if (RobotRotation > 0) { +// drivePower = 0.4; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(-drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(-drivePower); +// } else +// { +// AutoChain += 1; +// drivePower = 0; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } +// break; +// +// case 14://Initialize +// RobotStartingPosition = RobotPosition; +// AutoChain += 1; +// break; +// +// case 15://Drive 1 square forward +// if (RobotPosition - RobotStartingPosition < 2500){ +// drivePower = 1; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } else +// { +// AutoChain = 999; +// drivePower = 0; +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// } +// break; +// +// case 999: +// +// break; @@ -645,6 +651,3 @@ public class PrototypeTeleOPState extends CyberarmState { } - - } -} \ No newline at end of file