Merge remote-tracking branch 'origin/master'

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java

@@ -11,7 +11,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
-@Autonomous(name = "BURNSVILLE blue backdrop")
+@Autonomous(name = "BURNSVILLE blue audience")
 
 public class Competition2BlueBackStage extends CyberarmEngine {
 
@@ -30,34 +30,77 @@ public class Competition2BlueBackStage extends CyberarmEngine {
 
     @Override
     public void setup() {
-        this.robot = new CompetitionRobotV1("Burnsville backdrop blue");
+        this.robot = new CompetitionRobotV1("burnsville audience blue");
         this.robot.setup();
-        addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "0-00-0"));
-        addState(new ClawArmState(robot,"burnsville backdrop blue", "0-01-0"));
-        addState(new CameraVisionState(robot));
-        addState(new ClawArmState(robot,"burnsville backdrop blue", "0-02-0"));
-
-        addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-03-0"));
-
-        addState(new ClawFingerState(robot,"burnsville backdrop blue", "0-04-0"));
-
-        addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-05-0"));
-        addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-06-0"));
-
-
-
-
-
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "0-00-0"));
 
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-01-0"));
 
+//        addState(new CameraVisionState(robot));
 
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-01-1"));
+
+        // drive to the left, center, or right spike mark
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-02-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-02-0"));
+
+        // place pixel
+        addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-1"));
+
+        // drive to search pos
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-02-2"));
+
+        // close right finger
+        addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-3"));
+
+        // drive back and away from the spike mark (x,y) (1050, 1000) H = 0
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-03-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-03-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-03-0"));
 
 
+        // drive to the middle truss (right version) (1130,980) H = -90
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-04-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-04-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-04-0"));
+
+        // drive to hover mode to clear the middle truss
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-04-1"));
 
 
+        // drive under the middle truss across the field (right version) (1170,1080) H = -90
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-05-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-05-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-05-0"));
+
+        // drive to deposit mode to place on back drop
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-05-1"));
+
+        // drive to back drop
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-06-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-06-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-06-0"));
+
+        // open claw to deposit gold
+        addState(new ClawFingerState(robot,"burnsville audience blue", "0-06-1"));
+
+        // drive back from backdrop
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-07-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-07-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-07-0"));
+
+        // close left claw
+        addState(new ClawFingerState(robot,"burnsville audience blue", "0-07-1"));
 
 
+        // drive to parking spot
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-08-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-08-0"));
+        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-08-0"));
 
+        // arm to collect pos
+        addState(new ClawArmState(robot,"burnsville audience blue", "0-08-1"));
 
     }
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java

@@ -13,9 +13,18 @@ public class ClawArmState extends CyberarmState {
 
     CompetitionRobotV1 robot;
     public String armPos;
+    public long wait;
+    public long initTime;
     public ClawArmState(CompetitionRobotV1 robot, String groupName, String actionName) {
         this.robot = robot;
         this.armPos = robot.configuration.variable(groupName, actionName, "armPos").value();
+        this.wait = robot.configuration.variable(groupName, actionName, "wait").value();
+    }
+
+    @Override
+    public void start() {
+        super.start();
+        initTime = System.currentTimeMillis();
     }
 
     @Override
@@ -28,7 +37,7 @@ public class ClawArmState extends CyberarmState {
         robot.armPos = armPos;
         robot.clawArmControl();
 
-        if (robot.clawArm.getCurrentPosition() > robot.target - 20 || robot.clawArm.getCurrentPosition() < robot.target + 20) {
+        if ((robot.clawArm.getCurrentPosition() > robot.target - 20 || robot.clawArm.getCurrentPosition() < robot.target + 20) && wait < System.currentTimeMillis() - initTime) {
             setHasFinished(true);
         }
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java

@@ -21,7 +21,6 @@ public class DriveToCoordinatesState extends CyberarmState {
     public boolean posSpecific;
     public double maxXPower;
     public double maxYPower;
-    public long initTime;
 
     public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
         this.robot = robot;
@@ -38,43 +37,38 @@ public class DriveToCoordinatesState extends CyberarmState {
     @Override
     public void start() {
         super.start();
-        initTime = System.currentTimeMillis();
+        robot.hTarget = hTarget;
+        robot.yTarget = yTarget;
+        robot.xTarget = xTarget;
+        robot.yMaxPower = maxYPower;
+        robot.xMaxPower = maxXPower;
+
     }
 
     @Override
     public void exec() {
-        if (!posSpecific) {
-            // enter loop
-        } else {
+        if (posSpecific) {
             if (objectPos != robot.objectPos) {
                 // enter the ending loop
-//                setHasFinished(true);
+                setHasFinished(true);
             }
         }
-        if (posAchieved) {
-//            setHasFinished(true);
-        }
 
         if (armDrive) {
             robot.clawArmControl();
         }
 
-        robot.yMaxPower = maxYPower;
-        robot.xMaxPower = maxXPower;
-
-        robot.hTarget = hTarget;
-        robot.yTarget = yTarget;
-        robot.xTarget = xTarget;
-
+        robot.OdometryLocalizer();
+        robot.XDrivePowerModifier();
+        robot.YDrivePowerModifier();
         robot.DriveToCoordinates();
         robot.OdometryLocalizer();
 
-
-        if ((Math.abs(robot.backLeftPower) < 0.18 &&
-                Math.abs(robot.backRightPower) < 0.18 &&
-                Math.abs(robot.frontLeftPower) < 0.18 &&
-                Math.abs(robot.frontRightPower) < 0.18) || (System.currentTimeMillis() - initTime > 5000)) {
-            posAchieved = true;
+        if ((Math.abs(robot.backLeftPower) < 0.15 &&
+                Math.abs(robot.backRightPower) < 0.15 &&
+                Math.abs(robot.frontLeftPower) < .15 &&
+                Math.abs(robot.frontRightPower) < 0.15)){
+                setHasFinished(true);
         }
     }
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java

@@ -47,8 +47,8 @@ public class CompetitionRobotV1 extends Robot {
 
     // ----------------------------------------------------------------------------------------------------------------- odometry variables:
     public static double Hp = 0.8, Hi = 0, Hd = 0;
-    public static double Xp = -0.035, Xi = 0, Xd = 0;
-    public static double Yp = 0.035, Yi = 0, Yd = 0.0013;
+    public static double Xp = -0.03, Xi = 0, Xd = 0;
+    public static double Yp = 0.03, Yi = 0, Yd = 0;
     public double xMultiplier = 1;
     public double yMultiplier = 1;
     public double positionX = 1000;
@@ -175,9 +175,10 @@ public class CompetitionRobotV1 extends Robot {
         // ----------------------------------------------------------------------------------------------------------------------------- IMU
         IMU.Parameters parameters = new IMU.Parameters(
                 new RevHubOrientationOnRobot(
-                        RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
+                        RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
                         RevHubOrientationOnRobot.UsbFacingDirection.UP));
 
+
         imu.initialize(parameters);
 
         configuration = new TimeCraftersConfiguration("Blue Crab");
@@ -285,44 +286,43 @@ public class CompetitionRobotV1 extends Robot {
         return output;
     }
 
+    public void YDrivePowerModifier () {
+
+        rawPidY = XPIDControl(xTarget, positionX);
+
+        if (Math.abs(rawPidY) > yMaxPower) {
+            if (rawPidY < 0) {
+                pidY = -yMaxPower;
+            } else {
+                pidY = yMaxPower;
+            }
+        } else {
+            pidY = rawPidY;
+        }
+    }
+
+    public void XDrivePowerModifier () {
+
+        rawPidX = YPIDControl(yTarget, positionY);
+
+        if (Math.abs(rawPidX) > xMaxPower) {
+            if (rawPidX < 0) {
+                pidX = -xMaxPower;
+            } else {
+                pidX = xMaxPower;
+            }
+        } else {
+            pidX = rawPidX;
+        }
+    }
+
     public void DriveToCoordinates () {
         // determine the powers needed for each direction
         // this uses PID to adjust needed Power for robot to move to target
 
-//        rawPidY = XPIDControl(xTarget, positionX);
-//        rawPidX = YPIDControl(yTarget, positionY);
-
-        if (Math.abs(yTarget - positionY) > 5) {
-            if (Math.abs(XPIDControl(xTarget, positionX)) >= yMaxPower) {
-                if (XPIDControl(xTarget, positionX) < 0) {
-                    pidY = yMaxPower * -1;
-                } else {
-                    pidY = yMaxPower;
-                }
-            } else {
-                pidY = rawPidX;
-            }
-        } else {
-            pidY = rawPidX;
-        }
-
         double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
         double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);
 
-        if (Math.abs(xTarget - positionX) > 5) {
-            if (Math.abs(YPIDControl(yTarget, positionY)) >= xMaxPower) {
-                if (YPIDControl(yTarget, positionY) < 0) {
-                    pidX = xMaxPower * -1;
-                } else {
-                    pidX = xMaxPower;
-                }
-            } else {
-                pidX = rawPidY;
-            }
-        } else {
-            pidX = rawPidY;
-        }
-
         double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1);
 
         // field oriented math, (rotating the global field to the relative field)
@@ -366,7 +366,7 @@ public class CompetitionRobotV1 extends Robot {
                 lift.setPower(-0.6);
             } else {
                 shoulder.setPosition(0.38);
-                target = 100;
+                target = 120;
             }
 
         }

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java

@@ -23,17 +23,18 @@ public class CompetitionTeleOpState extends CyberarmState {
     public  double p = 0.007, i = 0,  d = 0.0001, f = 0;
     public int target = 0;
 
+
     // ------------------------------------------------------------------------------------------------------------- Heading lock variables:
     public double integralSum = 0;
     private double targetHeading;
-    public double collectLock = Math.toRadians(-90);
-    public double backDropLock = Math.toRadians(90);
-
+    public double collectLock = Math.toRadians(90);
+    public double backDropLock = Math.toRadians(-90);
+    public double boost;
     public double armPower;
     private double currentHeading;
     private boolean headingLock = false;
 
-    public static double Kp = 1;
+    public static double Kp = 0.8;
     public static double Ki = 0;
     public static double Kd = 0;
     private double lastError = 0;
@@ -53,7 +54,7 @@ public class CompetitionTeleOpState extends CyberarmState {
     //---------------------------------------------------------------------------------------------------------------- Drivetrain Variables:
     private boolean lbsVar1;
     private double drivePower = 1;
-    public double rx;
+    public double rx = engine.gamepad1.right_stick_x / 2;
 
     // ------------------------------------------------------------------------------------------------------------------- Slider Variables:
     private int maxExtension = 2000;
@@ -117,8 +118,12 @@ public class CompetitionTeleOpState extends CyberarmState {
         }
         lbsVar1 = lbs1;
 
-        double x = engine.gamepad1.left_stick_x;
-        double y = -engine.gamepad1.left_stick_y;
+        if (engine.gamepad1.left_stick_x != 0 || engine.gamepad1.left_stick_y != 0){
+            boost = engine.gamepad1.right_trigger + 1;
+        }
+
+        double x = -((engine.gamepad1.left_stick_x * 0.5)  * boost);
+        double y = ((engine.gamepad1.left_stick_y * 0.5)  * boost);
         double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
 
         // angle math to make things field oriented
@@ -134,10 +139,10 @@ public class CompetitionTeleOpState extends CyberarmState {
 
         // setting each power determined previously from the math above
         // as well as multiplying it by a drive power that can be changed.
-        robot.backLeft.setPower(backLeftPower * drivePower);
-        robot.backRight.setPower(-backRightPower * drivePower);
-        robot.frontLeft.setPower(frontLeftPower * drivePower);
-        robot.frontRight.setPower(frontRightPower * drivePower);
+        robot.backLeft.setPower(backLeftPower);
+        robot.backRight.setPower(-backRightPower);
+        robot.frontLeft.setPower(frontLeftPower);
+        robot.frontRight.setPower(frontRightPower);
     }
 
     public double angleWrap(double radians) {
@@ -200,9 +205,6 @@ public class CompetitionTeleOpState extends CyberarmState {
         if (engine.gamepad2.a) {
             armPos = "collect";
             depositMode = false;
-//        } else if (engine.gamepad2.x) {
-//            armPos = "passive";
-//            depositMode = false;
         } else if (engine.gamepad2.y) {
             armPos = "deposit";
             depositMode = true;
@@ -286,6 +288,7 @@ public class CompetitionTeleOpState extends CyberarmState {
     public void init() {
             super.init();
             pidController = new PIDController(p, i, d);
+            robot.imu.resetYaw();
 
     }