Merge pull request #54 from frc3197/pid-but-actually

St Louis Changes

src/main/java/org/team3197/frc2019/robot/OI.java

@@ -106,7 +106,7 @@ public static double wristSpeed() {
         }
 
         public static double erectorSpeed() {
-                return driver.getTriggerAxis(Hand.kRight) - driver.getTriggerAxis(Hand.kLeft);
+                return driver.getTriggerAxis(Hand.kLeft) - driver.getTriggerAxis(Hand.kRight);
         }
 
         public static double manipulatorSpeed() {

src/main/java/org/team3197/frc2019/robot/Robot.java

@@ -16,10 +16,12 @@
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PowerDistributionPanel;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Robot extends TimedRobot {
 
@@ -48,6 +50,8 @@ public void robotInit() {
 
   @Override
   public void robotPeriodic() {
+    SmartDashboard.putNumber("Time", DriverStation.getInstance().getMatchTime());
+
     ElbowPreset.kHatchOne.elbowPos = prefs.getDouble("kHatchLevelOne", ElbowPreset.kHatchOne.elbowPos);
     ElbowPreset.kHatchTwo.elbowPos = prefs.getDouble("kHatchLevelTwo", ElbowPreset.kHatchTwo.elbowPos);
     ElbowPreset.kHatchThree.elbowPos = prefs.getDouble("kHatchLevelThree", ElbowPreset.kHatchThree.elbowPos);
@@ -121,6 +125,7 @@ public void testPeriodic() {
 
   private void reset() {
     arm.resetGyro.start();
+    erector.resetPID.start();
     if (resetEncoders) {
       elevator.reset.start();
       arm.resetEncoder.start();

src/main/java/org/team3197/frc2019/robot/RobotMap.java

@@ -79,7 +79,7 @@ private Channel(int channel) {
   };
 
   public static enum GyroSensitivity {
-    kDrive(0.01), kArm(-0.01);
+    kDrive(0.01), kArm(-0.005);
     public double val;
 
     private GyroSensitivity(double val) {
@@ -88,7 +88,7 @@ private GyroSensitivity(double val) {
   };
 
   public static enum MaxSpeeds {
-    kElevator(.7, -.5), kArm(.6), kWrist(.45), kHatch(1), kCargo(1), kClimberVertical(.5), kClimberHorizontal(.75);
+    kElevator(.7, -.5), kArm(.55), kWrist(.30), kHatch(1), kCargo(1), kClimberVertical(.7), kClimberHorizontal(.9);
 
     public double forwardSpeed;
     public double reverseSpeed;
@@ -105,7 +105,7 @@ private MaxSpeeds(double speed) {
   }
 
   // TODO this is bad
-  public static final RobotType current = RobotType.B;
+  public static final RobotType current = RobotType.A;
 
   public static enum RobotType {
     A, B;

src/main/java/org/team3197/frc2019/robot/subsystems/Arm.java

@@ -37,6 +37,27 @@ public class Arm extends Subsystem implements Drivable {
   public FunctionCommand toggleGyro = new FunctionCommand(this::toggleGyro);
 
   private boolean useGyro = true;
+  final double kP = 5e-5;
+
+  final double kI = 1e-6;
+
+  final double kD = 0;
+
+  final double kIz = 0;
+
+  final double kFF = 0.000156;
+
+  final double kMaxOutput = 1;
+
+  final double kMinOutput = -1;
+
+  final double maxRPM = 3360;
+
+  // Smart Motion Coefficients
+
+  final double maxVel = 2000; // rpm
+
+  final double maxAcc = 1500;
 
   public Arm() {
     super();
@@ -50,6 +71,30 @@ public Arm() {
       wrist.setInverted(false);
     }
 
+    // set PID coefficients
+
+    elbow.getPIDController().setP(kP);
+
+    elbow.getPIDController().setI(kI);
+
+    elbow.getPIDController().setD(kD);
+
+    elbow.getPIDController().setIZone(kIz);
+
+    elbow.getPIDController().setFF(kFF);
+
+    elbow.getPIDController().setOutputRange(kMinOutput, kMaxOutput);
+
+    final int smartMotionSlot = 0;
+
+    elbow.getPIDController().setSmartMotionMaxVelocity(maxVel, smartMotionSlot);
+
+    // elbow.getPIDController().setSmartMotionMinOutputVelocity(minVel,
+
+    // smartMotionSlot);
+
+    elbow.getPIDController().setSmartMotionMaxAccel(maxAcc, smartMotionSlot);
+
     // elbow.getPIDController().setSmartMotionAllowedClosedLoopError(allowedErr,
     // smartMotionSlot);
 
@@ -85,7 +130,9 @@ public void elbow(double speed) {
     } else {
       // elbow.set(output);
       pidLast = false;
-      elbow.getPIDController().setReference(output, ControlType.kDutyCycle);
+      // elbow.getPIDController().setReference(output, ControlType.kDutyCycle);
+      double rpm = output * maxRPM;
+      elbow.getPIDController().setReference(rpm, ControlType.kSmartVelocity);
     }
     SmartDashboard.putNumber("ElbowEncoder", getElbowEncoderPosition());
 
@@ -107,7 +154,7 @@ public void wrist(double speed) {
     }
 
     SmartDashboard.putNumber("output", output);
-    wrist.set(output);
+    wrist.set(-output);
   }
 
   private double getAngle() {

src/main/java/org/team3197/frc2019/robot/subsystems/Erector.java

@@ -9,6 +9,7 @@
 import org.team3197.frc2019.robot.RobotMap.DeadbandType;
 import org.team3197.frc2019.robot.commands.defaults.Erect;
 import org.team3197.frc2019.robot.utilities.Drivable;
+import org.team3197.frc2019.robot.utilities.FunctionCommand;
 
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.command.Subsystem;
@@ -68,17 +69,23 @@ public void initDefaultCommand() {
   }
 
   boolean stoppedLast = false;
-  double encoderLastLeft = 0;
-  double encoderLastRight = 0;
+  private double encoderLastLeft = 0;
+  private double encoderLastRight = 0;
+
+  public FunctionCommand resetPID = new FunctionCommand(this::resetPID);
+
+  private void resetPID() {
+    encoderLastLeft = left.getEncoder().getPosition();
+    encoderLastRight = right.getEncoder().getPosition();
+    stoppedLast = true;
+  }
 
   public void drive(double speed, boolean hold) {
     // erectorGroup.set(speed);
     SmartDashboard.putNumber("speed1", speed);
     if (hold && Math.abs(speed) < DeadbandType.kErector.speed) {
       if (!stoppedLast) {
-        stoppedLast = true;
-        encoderLastLeft = left.getEncoder().getPosition();
-        encoderLastRight = right.getEncoder().getPosition();
+        resetPID();
       }
       right.getPIDController().setReference(encoderLastRight, ControlType.kPosition);
       left.getPIDController().setReference(encoderLastLeft, ControlType.kPosition);