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 02c4a10..e79a7cb 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -41,12 +41,12 @@ public class PhoenixBot1 { public TFObjectDetector tfod; - public Servo LowRiserLeft, LowRiserRight, CameraServo; + public Servo LowRiserLeft, LowRiserRight, /*HighRiserLeft, HighRiserRight, */CameraServo; private final CyberarmEngine engine; - public Rev2mDistanceSensor collectorDistance, /*downSensor,*/ leftPoleDistance, rightPoleDistance; + public Rev2mDistanceSensor collectorDistance, /*downSensor, */leftPoleDistance, rightPoleDistance; - public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizonatal, HighRiseMotor; + public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoder, OdometerEncoderLeft, ArmMotor, armMotorEncoder; public CRServo collectorLeft, collectorRight; @@ -145,64 +145,64 @@ public class PhoenixBot1 { // 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"); + ArmMotor = engine.hardwareMap.dcMotor.get("ArmMotor"); + armMotorEncoder = engine.hardwareMap.dcMotor.get("Arm Motor Encoder"); //motors direction and encoders frontLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE); frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); frontRightDrive.setDirection(DcMotorSimple.Direction.FORWARD); frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE); backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD); backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // Dead Wheel encoder for driving - OdometerEncoderRight = engine.hardwareMap.dcMotor.get("odometerEncoderR"); + OdometerEncoder = engine.hardwareMap.dcMotor.get("odometerEncoderR"); - OdometerEncoderRight.setDirection(DcMotorSimple.Direction.REVERSE); - OdometerEncoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + OdometerEncoder.setDirection(DcMotorSimple.Direction.REVERSE); + OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); OdometerEncoderLeft = engine.hardwareMap.dcMotor.get("odometerEncoderL"); OdometerEncoderLeft.setDirection(DcMotorSimple.Direction.REVERSE); OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - OdometerEncoderHorizonatal = engine.hardwareMap.dcMotor.get("odometerEncoderH"); - OdometerEncoderHorizonatal.setDirection(DcMotorSimple.Direction.REVERSE); - OdometerEncoderHorizonatal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - OdometerEncoderHorizonatal.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - HighRiseMotor = engine.hardwareMap.dcMotor.get("High Riser Motor"); - HighRiseMotor.setDirection(DcMotorSimple.Direction.REVERSE); - HighRiseMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - HighRiseMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - HighRiseMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - + OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// HighRiserLeft.setDirection(Servo.Direction.REVERSE); +// 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); CameraServo.setDirection(Servo.Direction.FORWARD); LowRiserLeft.setPosition(0.35); LowRiserRight.setPosition(0.35); +// HighRiserLeft.setPosition(0.40); +// HighRiserRight.setPosition(0.40); CameraServo.setPosition(0.775); 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 0d6790f..9ba701d 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java @@ -1,5 +1,6 @@ package org.timecrafters.TeleOp.states; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; import org.cyberarm.engine.V2.CyberarmState; @@ -11,17 +12,21 @@ public class TeleOPArmDriver extends CyberarmState { private long lastStepTime = 0; private int CyclingArmUpAndDown = 0; private GamepadChecker gamepad2Checker; - private int Opportunity, Endeavour; - private double drivePower; + private int Opportunity, Endeavour, Peanut; + private double drivePower, armPower; private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05; private double servoCollectLow = 0.40; //Low servos, A button - private double servoCollectHigh = 0.40; //High servos, A button +// private double servoCollectHigh = 0.40; //High servos, A button private double servoLowLow = 0.5; //Low servos, X button - private double servoLowHigh = 0.75; //High servos, X button +// private double servoLowHigh = 0.75; //High servos, X button private double servoMedLow = 0.5; //Low servos, B button - private double servoMedHigh = 0.9; //High servos, B button +// private double servoMedHigh = 0.9; //High servos, B button private double servoHighLow = 0.8; //Low servos, Y button - private double servoHighHigh = 0.9; //High servos, Y button +// private double servoHighHigh = 0.9; //High servos, Y button + private int armMotorCollect = 0; + private int armMotorLow = 100; + private int armMotorMed = 1000; + private int armMotorHigh = 1600; public TeleOPArmDriver(PhoenixBot1 robot) { this.robot = robot; @@ -30,31 +35,30 @@ public class TeleOPArmDriver extends CyberarmState { @Override public void telemetry() { engine.telemetry.addLine("Arm Driver:"); -// engine.telemetry.addData("High Riser Right Position", robot.HighRiserRight.getPosition()); -// engine.telemetry.addData("High Riser Left Position", robot.HighRiserLeft.getPosition()); + engine.telemetry.addData("Arm Motor Power", robot.ArmMotor.getPower()); engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition()); engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition()); } @Override public void init() { -// robot.HighRiserLeft.setDirection(Servo.Direction.REVERSE); -// robot.HighRiserRight.setDirection(Servo.Direction.FORWARD); + robot.ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD); robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD); robot.LowRiserRight.setDirection(Servo.Direction.REVERSE); robot.LowRiserLeft.setPosition(0.45); robot.LowRiserRight.setPosition(0.45); -// robot.HighRiserLeft.setPosition(0.45); -// robot.HighRiserRight.setPosition(0.45); + robot.ArmMotor.setPower(0); + robot.armMotorEncoder.setPower(0); Opportunity = 0; Endeavour = 0; + gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); } - @Override - public void exec() { +@Override +public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO MM HEIGHTS!! if (engine.gamepad2.y) { Endeavour = 4; @@ -70,127 +74,147 @@ public class TeleOPArmDriver extends CyberarmState { } if (Endeavour == 4) { -// if (robot.HighRiserLeft.getPosition() < servoHighHigh - 0.01)/* <-- high level too low*/ { -// if (System.currentTimeMillis() - lastStepTime >= 100) { -// lastStepTime = System.currentTimeMillis(); -// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); -// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); -// } - } - if (robot.LowRiserLeft.getPosition() < servoHighLow - 0.01)/* <-- low level too low*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } - -// if (robot.HighRiserLeft.getPosition() >= servoHighHigh && -// robot.LowRiserLeft.getPosition() >= servoHighLow) { -// Endeavour = 0; -// } - } - - if (Endeavour == 3) { - if (robot.LowRiserLeft.getPosition() > servoMedLow + 0.01)/* <-- low level too high*/ { + if (robot.armMotorEncoder.getCurrentPosition() < armMotorHigh - 5)/* <-- high level too low*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); + robot.ArmMotor.setPower(0.5); } } - if (robot.LowRiserLeft.getPosition() < servoMedLow - 0.01)/* <-- low level too low*/ { + if (robot.LowRiserLeft.getPosition() < servoHighLow - 5)/* <-- low level too low*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); } } -// if (robot.LowRiserLeft.getPosition() <= servoMedLow + 0.01 && -// robot.HighRiserLeft.getPosition() > servoMedHigh + 0.01)/* <-- high level too high*/ { -// if (System.currentTimeMillis() - lastStepTime >= 100) { -// lastStepTime = System.currentTimeMillis(); -// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); -// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); -// } -// } -// if (robot.LowRiserLeft.getPosition() < servoMedLow + 0.01 && -// robot.HighRiserLeft.getPosition() < servoMedHigh - 0.01)/* <-- high level too low*/ { -// if (System.currentTimeMillis() - lastStepTime >= 100) { -// lastStepTime = System.currentTimeMillis(); -// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); -// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); -// } -// } -// if (robot.LowRiserLeft.getPosition() > servoMedLow - 0.01 && -// robot.LowRiserLeft.getPosition() <= servoMedLow && -// robot.HighRiserLeft.getPosition() > servoMedHigh - 0.01 && -// robot.HighRiserLeft.getPosition() <= servoMedHigh) { -// Endeavour = 0; -// } -// } - - if (Endeavour == 2) { - if (robot.LowRiserLeft.getPosition() > servoLowLow + 0.01)/* <-- low level too high*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - } - } - if (robot.LowRiserLeft.getPosition() < servoLowLow - 0.01)/* <-- low level too low*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } - } -// if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 && -// robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 && -// robot.HighRiserLeft.getPosition() > servoLowHigh)/* <-- high level too high*/ { -// if (System.currentTimeMillis() - lastStepTime >= 100) { -// lastStepTime = System.currentTimeMillis(); -// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); -// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); -// } -// } -// if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 && -// robot.HighRiserLeft.getPosition() < servoLowHigh - 0.01)/* <-- high level too low*/ { -// if (System.currentTimeMillis() - lastStepTime >= 100) { -// lastStepTime = System.currentTimeMillis(); -// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); -// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); -// } -// } -// if (robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 && -// robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 && -// robot.HighRiserLeft.getPosition() > servoLowHigh - 0.01 && -// robot.HighRiserLeft.getPosition() <= servoLowHigh + 0.01) { -// Endeavour = 0; -// } -// } - - if (Endeavour == 1) { - if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 0.01)/* <-- low level too high*/ { - if (System.currentTimeMillis() - lastStepTime >= 100) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - } -// } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && -// robot.HighRiserLeft.getPosition() > servoCollectHigh)/* <-- high level too high*/ { -// if (System.currentTimeMillis() - lastStepTime >= 100) { -// lastStepTime = System.currentTimeMillis(); -// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); -// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); -// } -// } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 0.01 && -// robot.HighRiserLeft.getPosition() <= servoCollectHigh) { -// Endeavour = 0; -// } - } - - } + if (robot.armMotorEncoder.getCurrentPosition() >= armMotorHigh && + robot.LowRiserLeft.getPosition() >= servoHighLow) { + Endeavour = 0; } } + + if (Endeavour == 3) { + if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); + } + } + if (robot.LowRiserLeft.getPosition() < servoMedLow - 5)/* <-- low level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); + } + } + if (robot.LowRiserLeft.getPosition() <= servoMedLow + 5 && + robot.armMotorEncoder.getCurrentPosition() > armMotorMed + 5)/* <-- high level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setPower(-0.5); + } + } + if (robot.LowRiserLeft.getPosition() < servoMedLow + 5 && + robot.armMotorEncoder.getCurrentPosition() < armMotorMed - 5)/* <-- high level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setPower(0.5); + } + } + if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 && + robot.LowRiserLeft.getPosition() <= servoMedLow && + robot.armMotorEncoder.getCurrentPosition() > armMotorMed - 5) { + Endeavour = 0; + } + } + + if (Endeavour == 2) { + if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); + } + } + if (robot.LowRiserLeft.getPosition() < servoLowLow - 5)/* <-- low level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); + } + } + if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && + robot.LowRiserLeft.getPosition() > servoLowLow - 5 && + robot.armMotorEncoder.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setPower(-0.5); + } + } + if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && + robot.armMotorEncoder.getCurrentPosition() < armMotorLow - 5)/* <-- high level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setPower(0.5); + } + } + if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 && + robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && + robot.armMotorEncoder.getCurrentPosition() > armMotorLow - 5) { + Endeavour = 0; + } + } + + if (Endeavour == 1) { + if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 5)/* <-- low level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); + } + } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && + robot.armMotorEncoder.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.ArmMotor.setPower(-0.5); + } + } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 && + robot.armMotorEncoder.getCurrentPosition() <= armMotorCollect) { + Endeavour = 0; + } + } + + if (engine.gamepad2.dpad_left && Peanut != 1) { + Peanut = 1; + } + + if (engine.gamepad2.dpad_right && Peanut != 2) { + Peanut = 2; + } + + if (engine.gamepad2.dpad_left && Peanut == 1 || engine.gamepad2.dpad_right && Peanut == 2) { + robot.collectorLeft.setPower(0); + robot.collectorRight.setPower(0); + } + + if (Peanut == 1) { + robot.collectorRight.setPower(1); + robot.collectorLeft.setPower(-1); + } + + if (Peanut == 2) { + robot.collectorLeft.setPower(1); + robot.collectorRight.setPower(-1); + } + + if (engine.gamepad2.right_trigger > 0.1) { + armPower = engine.gamepad2.right_trigger; + robot.ArmMotor.setPower(armPower); + } else if (engine.gamepad2.left_trigger > 0.1) { + armPower = -(engine.gamepad2.left_trigger); + robot.ArmMotor.setPower(armPower); + } + } } \ No newline at end of file