diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java index e72e4c6..e761c65 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -16,6 +16,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; +import org.timecrafters.minibots.cyberarm.chiron.states.autonomous.Arm; public class PhoenixBot1 { @@ -46,7 +47,7 @@ public class PhoenixBot1 { public Rev2mDistanceSensor collectorDistance, /*downSensor, */leftPoleDistance, rightPoleDistance; - public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizontal, ArmMotor, armMotorEncoder; + public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizontal, ArmMotor; public CRServo collectorLeft, collectorRight; @@ -148,7 +149,6 @@ public class PhoenixBot1 { // HighRiserLeft = engine.hardwareMap.servo.get("HighRiserLeft"); // HighRiserRight = engine.hardwareMap.servo.get("HighRiserRight"); ArmMotor = engine.hardwareMap.dcMotor.get("ArmMotor"); - armMotorEncoder = engine.hardwareMap.dcMotor.get("Arm Motor Encoder"); //motors direction and encoders @@ -162,6 +162,7 @@ public class PhoenixBot1 { frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE); backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE); backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -199,11 +200,11 @@ public class PhoenixBot1 { // HighRiserRight.setDirection(Servo.Direction.FORWARD); LowRiserLeft.setDirection(Servo.Direction.FORWARD); LowRiserRight.setDirection(Servo.Direction.REVERSE); - armMotorEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - armMotorEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD); ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + ArmMotor.setTargetPosition(0); + ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); CameraServo.setDirection(Servo.Direction.FORWARD); @@ -216,6 +217,13 @@ public class PhoenixBot1 { } + public int AngleToTicks(double angle) { + double d = (60 * 28) / 360.0; + + // Casting to float so that the int version of Math.round is used. + return Math.round((float)d * (float)angle); + } + } diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java index 9ba701d..fd3457c 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java @@ -42,19 +42,14 @@ public class TeleOPArmDriver extends CyberarmState { @Override public void init() { - robot.ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD); + robot.ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE); robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD); robot.LowRiserRight.setDirection(Servo.Direction.REVERSE); robot.LowRiserLeft.setPosition(0.45); robot.LowRiserRight.setPosition(0.45); robot.ArmMotor.setPower(0); - robot.armMotorEncoder.setPower(0); Opportunity = 0; Endeavour = 0; - - - - gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); } @Override @@ -74,10 +69,11 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO } if (Endeavour == 4) { - if (robot.armMotorEncoder.getCurrentPosition() < armMotorHigh - 5)/* <-- high level too low*/ { + if (robot.ArmMotor.getCurrentPosition() < armMotorHigh - 5)/* <-- high level too low*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); robot.ArmMotor.setPower(0.5); + robot.ArmMotor.setTargetPosition(robot.AngleToTicks(90)); } } if (robot.LowRiserLeft.getPosition() < servoHighLow - 5)/* <-- low level too low*/ { @@ -87,7 +83,7 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); } } - if (robot.armMotorEncoder.getCurrentPosition() >= armMotorHigh && + if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh && robot.LowRiserLeft.getPosition() >= servoHighLow) { Endeavour = 0; } @@ -109,22 +105,24 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO } } if (robot.LowRiserLeft.getPosition() <= servoMedLow + 5 && - robot.armMotorEncoder.getCurrentPosition() > armMotorMed + 5)/* <-- high level too high*/ { + robot.ArmMotor.getCurrentPosition() > armMotorMed + 5)/* <-- high level too high*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); robot.ArmMotor.setPower(-0.5); + robot.ArmMotor.setTargetPosition(robot.AngleToTicks(90)); } } if (robot.LowRiserLeft.getPosition() < servoMedLow + 5 && - robot.armMotorEncoder.getCurrentPosition() < armMotorMed - 5)/* <-- high level too low*/ { + robot.ArmMotor.getCurrentPosition() < armMotorMed - 5)/* <-- high level too low*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); robot.ArmMotor.setPower(0.5); + robot.ArmMotor.setTargetPosition(robot.AngleToTicks(90)); } } if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 && robot.LowRiserLeft.getPosition() <= servoMedLow && - robot.armMotorEncoder.getCurrentPosition() > armMotorMed - 5) { + robot.ArmMotor.getCurrentPosition() > armMotorMed - 5) { Endeavour = 0; } } @@ -146,22 +144,26 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO } if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && robot.LowRiserLeft.getPosition() > servoLowLow - 5 && - robot.armMotorEncoder.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ { + robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); robot.ArmMotor.setPower(-0.5); + robot.ArmMotor.setTargetPosition(robot.AngleToTicks(67)); + } } if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && - robot.armMotorEncoder.getCurrentPosition() < armMotorLow - 5)/* <-- high level too low*/ { + robot.ArmMotor.getCurrentPosition() < armMotorLow - 5)/* <-- high level too low*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); robot.ArmMotor.setPower(0.5); + robot.ArmMotor.setTargetPosition(robot.AngleToTicks(67)); + } } if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 && robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && - robot.armMotorEncoder.getCurrentPosition() > armMotorLow - 5) { + robot.ArmMotor.getCurrentPosition() > armMotorLow - 5) { Endeavour = 0; } } @@ -174,13 +176,15 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); } } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && - robot.armMotorEncoder.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ { + robot.ArmMotor.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); robot.ArmMotor.setPower(-0.5); + robot.ArmMotor.setTargetPosition(robot.AngleToTicks(45)); + } } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 && - robot.armMotorEncoder.getCurrentPosition() <= armMotorCollect) { + robot.ArmMotor.getCurrentPosition() <= armMotorCollect) { Endeavour = 0; } } @@ -214,6 +218,9 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO } else if (engine.gamepad2.left_trigger > 0.1) { armPower = -(engine.gamepad2.left_trigger); robot.ArmMotor.setPower(armPower); + } else if (engine.gamepad2.right_trigger == 0 && engine.gamepad2.left_trigger == 0 && Endeavour == 0) { + armPower = 0; + robot.ArmMotor.setPower(armPower); } } diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java index a3dc815..e6a2b4d 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java @@ -33,7 +33,6 @@ public class TeleOPTankDriver extends CyberarmState { @Override public void init() { - gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); FreeSpirit = false; }