Changes from competition!

src/main/java/frc/robot/Robot.java

@@ -31,7 +31,8 @@ public class Robot extends TimedRobot {
           CTerminalReturn = new BallCtoTerminalReturn(),
           BTerminalReturn = new BallBtoTerminalReturn(),
           simpleB         = new BallSimpleB(),
-          calibration = new Calibration();
+          calibration = new Calibration(),
+          shortRun = new ShortRun();
 
   GenericRobot robot = new Lightning();
   Joystick joystick = new Joystick(0);
@@ -564,11 +565,13 @@ public void disabledPeriodic() {
     if (joystick.getRawButton(5)) autonomous = ATerminalReturn;
     if (joystick.getRawButton(6)) autonomous = BTerminalReturn;
     if (joystick.getRawButton(9)) autonomous = CTerminalReturn;
+    if (joystick.getRawButton(7)) autonomous = shortRun;
 
   }
 
   @Override
   public void testInit() {
+    joystick.getRawButtonPressed(4);
   }
 
   @Override

src/main/java/frc/robot/autonomous/BallAtoTerminalReturn.java

@@ -75,6 +75,7 @@ public void autonomousPeriodic(GenericRobot robot) {
         if (autonomousStep >= 1){
             robot.getCargo();
             robot.shoot();
+            robot.setShooterTargetRPM(3700);
         }
         if (autonomousStep >= 1 && autonomousStep <=10){
             robot.setTurretPitchPosition(.38);

src/main/java/frc/robot/autonomous/BallBtoTerminalReturn.java

@@ -17,7 +17,7 @@ public class BallBtoTerminalReturn extends GenericAutonomous {
     double correction;
 
     double distanceB = 61.5;
-    double distanceTerminal = 154.6;
+    double distanceTerminal = 142.6;
     double rampDownDist = 10;
 
     PIDController PIDTurret;
@@ -69,6 +69,7 @@ public void autonomousPeriodic(GenericRobot robot) {
         if (autonomousStep >= 1){
             robot.getCargo();
             robot.shoot();
+            robot.setShooterTargetRPM(3700);
         }
         if (autonomousStep >= 1 && autonomousStep <=10){
             robot.setTurretPitchPosition(.38);

src/main/java/frc/robot/autonomous/BallCtoTerminalReturn.java

@@ -20,8 +20,8 @@ public class BallCtoTerminalReturn extends GenericAutonomous {
     boolean time = false;
 
     double distanceC = 47.9;
-    double distanceTerminal = 225;
-    double angleC = 82.74;
+    double distanceTerminal = 219;
+    double angleC = 84.74;
     double rampDownDist = 10;
 
     PIDController PIDDriveStraight;
@@ -92,6 +92,12 @@ public void autonomousPeriodic(GenericRobot robot) {
         if (autonomousStep >= 1){
             robot.getCargo();
             robot.shoot();
+            if (autonomousStep <= 8){
+                robot.setShooterTargetRPM(3700);
+            }
+            else{
+                robot.setShooterTargetRPM(3900);
+            }
         }
         if (autonomousStep >= 1 && autonomousStep <=10){
             robot.setTurretPitchPosition(.38);

src/main/java/frc/robot/autonomous/ShortRun.java

@@ -0,0 +1,164 @@
+package frc.robot.autonomous;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.generic.GenericRobot;
+
+//Simple autonomous code for ball C, closest ball to the hangar, and driving to the ball at terminal
+//Setup: Line the robot straight between ball C and the center point of the hub
+public class ShortRun extends GenericAutonomous {
+    double startingYaw;
+    double startDistance;
+    double startTime;
+
+    double leftpower;
+    double rightpower;
+    double defaultPower = .6;
+    double correction;
+    boolean time = false;
+
+    double distanceC = 47.9;
+    double distanceTerminal = 225;
+    double angleC = 82.74;
+    double rampDownDist = 10;
+
+    PIDController PIDDriveStraight;
+    PIDController PIDTurret;
+    PIDController PIDPivot;
+
+    int averageTurretXSize = 2;
+    double[] averageTurretX = new double [averageTurretXSize];
+    int counter = 0;
+
+    double shooterTargetRPM;
+    boolean readyToShoot;
+
+    double indexerPct;
+    double collectorPct;
+
+
+
+    @Override
+    public void autonomousInit(GenericRobot robot) {
+        autonomousStep = 0;
+        startingYaw = robot.getYaw();
+        startTime = System.currentTimeMillis();
+        shooterTargetRPM = 0;
+        indexerPct = 0;
+        collectorPct = 0;
+        readyToShoot = false;
+        PIDDriveStraight = new PIDController(robot.getPIDmaneuverP(), robot.getPIDmaneuverI(), robot.getPIDmaneuverD());
+        PIDPivot = new PIDController(robot.getPIDpivotP(), robot.getPIDpivotI(), robot.getPIDpivotD());
+        PIDTurret = new PIDController(robot.turretPIDgetP(), robot.turretPIDgetI(), robot.turretPIDgetD());
+        robot.setPipeline(0);
+    }
+
+    @Override
+    public void autonomousPeriodic(GenericRobot robot) {
+
+        if(robot.isTargetFound()) {
+            averageTurretX[counter % averageTurretXSize] = robot.getTargetX();
+            counter++;
+        }
+
+        double average = 0;
+        for(double i: averageTurretX){
+            average += i;
+        }
+        average /= averageTurretXSize;
+
+        double currentTurretPower = 0;
+
+        if(robot.isTargetFound()){
+            currentTurretPower = PIDTurret.calculate(average);
+        }else{
+            PIDTurret.reset();
+        }
+
+        if (autonomousStep < 4){
+            if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) {
+                currentTurretPower = .3;
+            }
+        }
+        if ((autonomousStep>=4) && (autonomousStep < 8)){
+            if((!robot.isTargetFound()) && (System.currentTimeMillis() - startTime < 5000)) {
+                currentTurretPower = -.2;
+            }
+        }
+        robot.setTurretPowerPct(currentTurretPower);
+
+        if (autonomousStep >= 1){
+            robot.getCargo();
+            robot.shoot();
+        }
+        if (autonomousStep >= 1 && autonomousStep <=10){
+            robot.setTurretPitchPosition(.38);
+        }
+        else{
+            robot.setCollectorIntakePercentage(0);
+            robot.setTurretPowerPct(0);
+        }
+        switch(autonomousStep){
+            case 0: //reset
+                robot.lowerCollector();
+                PIDPivot.reset();
+                PIDPivot.enableContinuousInput(-180,180);
+                PIDDriveStraight.reset();
+                PIDDriveStraight.enableContinuousInput(-180,180);
+                robot.resetEncoders();
+                robot.resetAttitude();
+                if (System.currentTimeMillis() - startTime > 500){
+                    autonomousStep += 1;
+                    startingYaw = robot.getYaw();
+                    startDistance = robot.getDriveDistanceInchesLeft();
+                }
+                break;
+            case 1: //drive to ball C
+                collectorPct = 1;
+                correction = PIDDriveStraight.calculate(robot.getYaw() - startingYaw);
+
+                leftpower = defaultPower + correction;
+                rightpower = defaultPower - correction;
+
+                if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC - rampDownDist){
+                    double ramp = rampDown(defaultPower, .1, startDistance, rampDownDist,
+                            robot.getDriveDistanceInchesLeft(), distanceC);
+                    leftpower = ramp;
+                    rightpower = ramp;
+                }
+                if(robot.getDriveDistanceInchesLeft() - startDistance >= distanceC){
+                    autonomousStep += 1;
+                    startTime = System.currentTimeMillis();
+                }
+                break;
+            case 2: //stop
+                leftpower = 0;
+                rightpower = 0;
+                startDistance = robot.getDriveDistanceInchesLeft();
+                autonomousStep += 1;
+                time = false;
+                break;
+            case 3: //create a target shooter value and see if shooter reaches it.
+                if (robot.isTargetFound() && robot.canShoot() && (-5 < average) && (average< 5)){
+                    robot.setActivelyShooting(true);
+                    startTime = System.currentTimeMillis();
+                    autonomousStep += 1;
+                }
+                break;
+            case 4: //turn the shooter off
+                if (System.currentTimeMillis() - startTime >= 2000){
+                    robot.setActivelyShooting(false);
+                    autonomousStep += 1;
+                }
+                break;
+
+            case 5: //End of autonomous, wait for Teleop
+                leftpower = 0;
+                rightpower = 0;
+                break;
+        }
+        robot.drivePercent(leftpower, rightpower);
+    }
+}

src/main/java/frc/robot/command/Hang.java

@@ -100,7 +100,6 @@ public void step(GenericRobot robot){
                 }
             }
             robot.setTurretPowerPct(turretPower);
-            robot.raiseCollector();
             switch (commandStep) { /////////////tapeAlign Code
                 case -1:
                     robot.resetEncoders();
@@ -214,7 +213,7 @@ else if (!robot.getFloorSensorLeft()) {
                     rightArmPower = 0;
                     countLeft = 0;
                     countRight = 0;
-                    if (System.currentTimeMillis() - startingTime >= 5000){
+                    if (System.currentTimeMillis() - startingTime >= 2000){
                         SmartDashboard.putNumber("driveOutputCurrent", robot.getDriveCurrent());
                         commandStep = 2; ///TODO: fix numbering
                     }
@@ -482,10 +481,10 @@ else if (!robot.getFloorSensorLeft()) {
         if (robot.getRoll() - level < - leveltol){
             leftArmPower *= .8;
         }
-        if (Math.abs(robot.getRoll()-level) >= 3*leveltol){
+        /*if (Math.abs(robot.getRoll()-level) >= 3*leveltol){
             rightArmPower = 0;
             leftArmPower = 0;
-        }
+        }*/
         robot.armPower(leftArmPower, rightArmPower);
         }
     }

src/main/java/frc/robot/command/HangWithoutAlign.java

@@ -360,10 +360,7 @@ public void step(GenericRobot robot){
         if (robot.getRoll() - level < - leveltol){
             leftArmPower *= .8;
         }
-        if (Math.abs(robot.getRoll()-level) >= 3*leveltol){
-            rightArmPower = 0;
-            leftArmPower = 0;
-        }
+
         robot.armPower(leftArmPower, rightArmPower);
     }
 

src/main/java/frc/robot/generic/Lightning.java

@@ -341,7 +341,7 @@ public double encoderTurretTicksPerDegree(){
     public double getAlternateTurretAngle(){
         double raw = encoderTurretAlt.getPosition();
         double out;
-        double offset = 185-45+30+5+163+15;
+        double offset = 78 + 15;
         out = (raw *  136.467) - 5.73 - offset;
         if (out>360)
         {