Merge remote-tracking branch 'origin/master'

TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java

@@ -18,7 +18,17 @@ public class DriverStateWithOdometer extends CyberarmState {
     private double CurrentPosition;
     public final double WHEEL_CIRCUMFERENCE = 7.42108499;
     public final int COUNTS_PER_REVOLUTION = 8192;
+    public double MINIMUM_POWER = 0.25;
     public final double distanceMultiplier;
+    public double startOfRampUpRight;
+    public double startOfRampDownRight;
+    public double startOfRampUpLeft;
+    public double startOfRampDownLeft;
+    public double endOfRampUpRight;
+    public double endOfRampDownRight;
+    public double endOfRampUpLeft;
+    public double endOfRampDownLeft;
+
     public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
         this.robot = robot;
         this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
@@ -31,6 +41,7 @@ public class DriverStateWithOdometer extends CyberarmState {
 
         this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
     }
+
     private double drivePower, targetDrivePower;
     private int traveledDistance;
 
@@ -50,47 +61,73 @@ public class DriverStateWithOdometer extends CyberarmState {
         maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
 
 
+        if (drivePower > 0) {
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
+        } else {
+
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
+
+        }
 
     }
 
     @Override
     public void exec() {
 
+
         if (stateDisabled) {
             setHasFinished(true);
             return;
         }
 
-        double RightCurrentPosition = Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
+        double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
-        double LeftCurrentPosition = Math.abs(robot.OdometerEncoderLeft.getCurrentPosition());
+        double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
 
-        if (RightCurrentPosition > LeftCurrentPosition) CurrentPosition = RightCurrentPosition;
-        if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition;
-
-
-        if (Math.abs(CurrentPosition) <= RampUpDistance){
         // ramping up
-//            double ratio = (Math.abs(CurrentPosition) / RampUpDistance);
+        if (RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight &&
+                LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft) {
+
             if (targetDrivePower > 0) {
-                drivePower = (targetDrivePower - 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) + 0.25;
+                drivePower = (targetDrivePower - MINIMUM_POWER) * ((RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + MINIMUM_POWER;
             } else {
-                drivePower = (targetDrivePower + 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) - 0.25;
+                drivePower = (targetDrivePower + MINIMUM_POWER) * ((startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - MINIMUM_POWER;
             }
         }
-        else if (Math.abs(CurrentPosition) >= traveledDistance - RampDownDistance){
-            // ramping down
-            if (targetDrivePower > 0){
-                drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower - 0.25) + 0.25);
-            } else {
-                drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower + 0.25) - 0.25);
-            }
 
-        } else {
+        // Driving Normal
-            // middle ground
+        else if (RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight &&
+                LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft) {
+
             drivePower = targetDrivePower;
+
         }
 
-        if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
+        // Ramping down
+        else if (RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight &&
+                LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft) {
+            if (targetDrivePower > 0) {
+                drivePower = (targetDrivePower + MINIMUM_POWER) * ((startOfRampDownRight - RightCurrentPosition) / RampDownDistance) - MINIMUM_POWER;
+            } else {
+                drivePower = (targetDrivePower - MINIMUM_POWER) * ((RightCurrentPosition - startOfRampDownRight) / RampDownDistance) + MINIMUM_POWER;
+            }
+        }
+            if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
                 // This is limiting drive power to the targeted drive power
                 drivePower = targetDrivePower;
             }
@@ -99,15 +136,15 @@ public class DriverStateWithOdometer extends CyberarmState {
                 drivePower = drivePower * -1;
             }
 
-        if (Math.abs(LeftCurrentPosition) < traveledDistance - maximumTolerance || Math.abs(RightCurrentPosition) < traveledDistance - maximumTolerance){
+            if (LeftCurrentPosition - traveledDistance < maximumTolerance || Math.abs(RightCurrentPosition - traveledDistance) < maximumTolerance) {
                 if (targetAchieved) {
-                drivePower = drivePower * 0.25;
+                    drivePower = drivePower * 0.15;
 
-                if (Math.abs(drivePower) < 0.25){
+                    if (Math.abs(drivePower) < 0.15) {
                         if (drivePower < 0) {
-                        drivePower = -0.25;
+                            drivePower = -0.15;
                         } else {
-                        drivePower = 0.25;
+                            drivePower = 0.15;
                         }
                     }
                 }
@@ -116,17 +153,16 @@ public class DriverStateWithOdometer extends CyberarmState {
                 robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
                 robot.frontRightDrive.setPower(drivePower);
 
-        }
+            } else if (Math.abs(LeftCurrentPosition) > traveledDistance + maximumTolerance || Math.abs(RightCurrentPosition) > traveledDistance + maximumTolerance) {
-        else if (Math.abs(LeftCurrentPosition) > traveledDistance + maximumTolerance || Math.abs(RightCurrentPosition) > traveledDistance + maximumTolerance) {
                 targetAchieved = true;
 
-            drivePower = targetDrivePower * -0.25;
+                drivePower = targetDrivePower * -0.15;
 
-            if (Math.abs(drivePower) < 0.25){
+                if (Math.abs(drivePower) < 0.15) {
                     if (drivePower < 0) {
-                    drivePower = -0.25;
+                        drivePower = -0.15;
                     } else {
-                    drivePower = 0.25;
+                        drivePower = 0.15;
                     }
                 }
 
@@ -136,16 +172,14 @@ public class DriverStateWithOdometer extends CyberarmState {
                 robot.frontRightDrive.setPower(drivePower);
 
 
-        }
+            } else {
 
-        else {
+                if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)) {
-
-            if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)){
 
                     if (Math.abs(Math.abs(LeftCurrentPosition) - Math.abs(RightCurrentPosition)) < 20) {
                         drivePower = 0;
                     } else {
-                    drivePower = 0.25;
+                        drivePower = 0.15;
                         robot.backLeftDrive.setPower(-drivePower);
                         robot.backRightDrive.setPower(drivePower);
                         robot.frontLeftDrive.setPower(-drivePower);
@@ -153,20 +187,18 @@ public class DriverStateWithOdometer extends CyberarmState {
                     }
                 }
 
-            if (Math.abs(LeftCurrentPosition) < Math.abs(RightCurrentPosition)){
+                if (Math.abs(LeftCurrentPosition) < Math.abs(RightCurrentPosition)) {
 
-                if (Math.abs(LeftCurrentPosition) == Math.abs(RightCurrentPosition)){
+                    if (Math.abs(LeftCurrentPosition) == Math.abs(RightCurrentPosition)) {
                         drivePower = 0;
                     } else {
-                    drivePower = 0.25;
+                        drivePower = 0.15;
                         robot.backLeftDrive.setPower(drivePower);
                         robot.backRightDrive.setPower(-drivePower);
                         robot.frontLeftDrive.setPower(drivePower);
                         robot.frontRightDrive.setPower(-drivePower);
                     }
-            }
+                } else {
-
-            else {
                     robot.backLeftDrive.setPower(0);
                     robot.backRightDrive.setPower(0);
                     robot.frontLeftDrive.setPower(0);
@@ -179,7 +211,7 @@ public class DriverStateWithOdometer extends CyberarmState {
         }
 
         @Override
-    public void telemetry() {
+        public void telemetry () {
             engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
             engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
             engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
@@ -196,7 +228,6 @@ public class DriverStateWithOdometer extends CyberarmState {
             engine.telemetry.addData("Target Achieved", targetAchieved);
 
 
-
             engine.telemetry.addData("drivePower", drivePower);
             engine.telemetry.addData("targetDrivePower", targetDrivePower);
 
@@ -209,4 +240,5 @@ public class DriverStateWithOdometer extends CyberarmState {
             Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
 
         }
-}
+    }
+

TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java

@@ -212,11 +212,10 @@ public class PhoenixBot1 {
 //            HighRiserRight.setDirection(Servo.Direction.FORWARD);
             LowRiserLeft.setDirection(Servo.Direction.FORWARD);
             LowRiserRight.setDirection(Servo.Direction.REVERSE);
-            ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+//            ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE);
-            ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+//            ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-            ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+//            ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-            ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+//            ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
             CameraServo.setDirection(Servo.Direction.FORWARD);
 

TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java

@@ -113,14 +113,14 @@ public void exec() {
                     robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
                 }
             }
-            if (robot.LowRiserLeft.getPosition() <= servoMedLow + 5 &&
+            if (robot.LowRiserLeft.getPosition() <= servoMedLow &&
                     robot.ArmMotor.getCurrentPosition() > armMotorMed)/* <-- high level too high*/ {
                 if (System.currentTimeMillis() - lastStepTime >= 100) {
                     lastStepTime = System.currentTimeMillis();
                     robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
                 }
             }
-            if (robot.LowRiserLeft.getPosition() < servoMedLow + 5 &&
+            if (robot.LowRiserLeft.getPosition() < servoMedLow &&
                     robot.ArmMotor.getCurrentPosition() < armMotorMed)/* <-- high level too low*/ {
                 if (System.currentTimeMillis() - lastStepTime >= 100) {
                     lastStepTime = System.currentTimeMillis();
@@ -150,7 +150,7 @@ public void exec() {
                     robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
                 }
             }
-            if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
+            if (robot.LowRiserLeft.getPosition() <= servoLowLow &&
                     robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
                     robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ {
                 if (System.currentTimeMillis() - lastStepTime >= 100) {
@@ -158,7 +158,7 @@ public void exec() {
                     robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
                 }
             }
-            if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
+            if (robot.LowRiserLeft.getPosition() <= servoLowLow &&
                     robot.ArmMotor.getCurrentPosition() < armMotorLow)/* <-- high level too low*/ {
                 if (System.currentTimeMillis() - lastStepTime >= 100) {
                     lastStepTime = System.currentTimeMillis();
@@ -166,7 +166,7 @@ public void exec() {
                 }
             }
             if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
-                    robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
+                    robot.LowRiserLeft.getPosition() <= servoLowLow &&
                     robot.ArmMotor.getCurrentPosition() >= armMotorLow) {
                 Endeavour = 0;
             }
@@ -174,19 +174,19 @@ public void exec() {
 
         if (Endeavour == 1) {
             robot.ArmMotor.setTargetPosition(armMotorCollect);
-            if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 5)/* <-- low level too high*/ {
+            if (robot.LowRiserLeft.getPosition() >= servoCollectLow)/* <-- 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 + 5 &&
+            } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
                     robot.ArmMotor.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ {
                 if (System.currentTimeMillis() - lastStepTime >= 100) {
                     lastStepTime = System.currentTimeMillis();
                     robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
                 }
-            } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 &&
+            } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
                     robot.ArmMotor.getCurrentPosition() <= armMotorCollect) {
                 Endeavour = 0;
             }
@@ -203,6 +203,7 @@ public void exec() {
         if (engine.gamepad2.dpad_left && Peanut == 1 || engine.gamepad2.dpad_right && Peanut == 2) {
             robot.collectorLeft.setPower(0);
             robot.collectorRight.setPower(0);
+            Peanut = 0;
         }
 
         if (Peanut == 1) {

TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java

@@ -12,7 +12,6 @@ public class TeleOPTankDriver extends CyberarmState {
     private long lastStepTime = 0;
     private double drivePower = 0.3;
     private double RobotRotation;
-    private double currentDriveCommand;
     private double RotationTarget, DeltaRotation;
     private double MinimalPower = 0.2;
     private int DeltaOdometerR, Endeavour, Spirit;
@@ -37,7 +36,16 @@ public class TeleOPTankDriver extends CyberarmState {
     @Override
     public void exec() {
 
-        if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
+        double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
+        double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
+        double rx = engine.gamepad1.right_stick_x;
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+        double frontLeftPower = (y + x + rx) / denominator;
+        double backLeftPower = (y - x + rx) / denominator;
+        double frontRightPower = (y - x - rx) / denominator;
+        double backRightPower = (y + x - rx) / denominator;
+
+        if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) {
             drivePower = engine.gamepad1.left_stick_y;
             robot.backLeftDrive.setPower(drivePower);
             robot.backRightDrive.setPower(drivePower);
@@ -45,7 +53,7 @@ public class TeleOPTankDriver extends CyberarmState {
             robot.frontRightDrive.setPower(drivePower);
         }
 
-        if (Math.abs(engine.gamepad1.left_stick_x) > 0.1) {
+        if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) < 0.1) {
             drivePower = engine.gamepad1.left_stick_x;
             robot.backLeftDrive.setPower(drivePower);
             robot.backRightDrive.setPower(-drivePower);
@@ -53,12 +61,27 @@ public class TeleOPTankDriver extends CyberarmState {
             robot.frontRightDrive.setPower(drivePower);
         }
 
+        if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
+            robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
+            robot.backLeftDrive.setPower(backLeftPower * drivePower);
+            robot.frontRightDrive.setPower(frontRightPower * drivePower);
+            robot.backRightDrive.setPower(backRightPower * drivePower);
+        }
+
         if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
             drivePower = engine.gamepad1.right_stick_x;
+            robot.backLeftDrive.setPower(-drivePower);
+            robot.backRightDrive.setPower(drivePower);
+            robot.frontLeftDrive.setPower(-drivePower);
+            robot.frontRightDrive.setPower(drivePower);
+        }
+
+        if (engine.gamepad1.left_stick_y == 0 && engine.gamepad1.left_stick_x == 0 && engine.gamepad1.right_stick_x == 0) {
+            drivePower = 0;
             robot.backLeftDrive.setPower(drivePower);
-            robot.backRightDrive.setPower(-drivePower);
+            robot.backRightDrive.setPower(drivePower);
             robot.frontLeftDrive.setPower(drivePower);
-            robot.frontRightDrive.setPower(-drivePower);
+            robot.frontRightDrive.setPower(drivePower);
         }
 
 
@@ -85,21 +108,6 @@ public class TeleOPTankDriver extends CyberarmState {
         }
     }
 
-    public void getCurrentDriveCommand() {
-        if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
-            currentDriveCommand = engine.gamepad1.left_stick_y;
-        } else if (Math.abs(engine.gamepad1.left_stick_x) > 0.1) {
-            currentDriveCommand = engine.gamepad1.left_stick_x;
-        } else if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
-            currentDriveCommand = engine.gamepad1.right_stick_x;
-        } else if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
-            currentDriveCommand = engine.gamepad1.right_stick_y;
-        } else if ((Math.abs(engine.gamepad1.left_stick_y)) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
-            currentDriveCommand = 0;
-        }
-
-    }
-
     @Override
     public void buttonUp(Gamepad gamepad, String button) {
         if (engine.gamepad1 == gamepad && button.equals("start")) {