diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java new file mode 100644 index 0000000..6432cf4 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java @@ -0,0 +1,114 @@ +package org.timecrafters.Autonomous.States; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.TeleOp.states.PhoenixBot1; + +public class DriverStateWithOdometer extends CyberarmState { + private final boolean stateDisabled; + PhoenixBot1 robot; + private int RampUpDistance; + private int RampDownDistance; + public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); + this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value(); + this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value(); + this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value(); + + this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; + } + private double drivePower, targetDrivePower; + private int traveledDistance; + + @Override + public void start() { + robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + @Override + public void exec() { + if (stateDisabled) { + setHasFinished(true); + return; + } + + double CurrentPosition = robot.OdometerEncoder.getCurrentPosition(); + double delta = traveledDistance - Math.abs(CurrentPosition); + + if (Math.abs(CurrentPosition) <= RampUpDistance){ + // ramping up + drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25; + } + else if (Math.abs(CurrentPosition) >= delta){ + // ramping down + drivePower = ((delta / RampDownDistance) + 0.25); + } else { + // middle ground + drivePower = targetDrivePower; + } + + if (Math.abs(drivePower) > Math.abs(targetDrivePower)){ + // This is limiting drive power to the targeted drive power + drivePower = targetDrivePower; + } + + if (targetDrivePower < 0 && drivePower != targetDrivePower) { + drivePower = drivePower * -1; + } + + if (Math.abs(CurrentPosition) < traveledDistance){ + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower); + } else { + robot.backLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.frontRightDrive.setPower(0); + setHasFinished(true); + } + + + } + + @Override + public void telemetry() { + engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition()); + engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition()); + engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition()); + engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition()); + engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition()); + + + + + + + + + + + + + + + engine.telemetry.addData("drivePower", drivePower); + engine.telemetry.addData("targetDrivePower", targetDrivePower); + + engine.telemetry.addData("traveledDistance", traveledDistance); + engine.telemetry.addData("RampUpDistance", RampUpDistance); + engine.telemetry.addData("RampDownDistance", RampDownDistance); + + } +} 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 180c4a7..bbdf191 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -42,7 +42,7 @@ public class PhoenixBot1 { public Rev2mDistanceSensor collectorDistance, downSensor, leftPoleDistance, rightPoleDistance; - public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive; + public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoder; public CRServo collectorLeft, collectorRight; @@ -125,12 +125,19 @@ public class PhoenixBot1 { backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + // Dead Wheel encoder for driving + + OdometerEncoder.setDirection(DcMotorSimple.Direction.FORWARD); + OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + OdometerEncoder.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); + CameraServo.setDirection(Servo.Direction.FORWARD); LowRiserLeft.setPosition(0.35);