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

Erector PID and Tweaks
frc3197 · Mar 13, 2019 · 16c9b7a · 16c9b7a
2 parents 9c0e8b0 + 0a88707
commit 16c9b7a
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 30 deletions.
diff --git a/src/main/java/org/team3197/frc2019/robot/RobotMap.java b/src/main/java/org/team3197/frc2019/robot/RobotMap.java
@@ -18,7 +18,7 @@ private CANSparkMaxID(int id, String name) {
   };
 
   public static enum ElbowPreset {
-    kHatchOne(77), kHatchTwo(196), kHatchThree(196), kCargoOne(196), kCargoTwo(196), kCargoThree(196), kCargoShip(196);
+    kHatchOne(-17), kHatchTwo(-38), kHatchThree(-38), kCargoOne(-38), kCargoTwo(-38), kCargoThree(-38), kCargoShip(-38);
     public double elbowPos;
 
     private ElbowPreset(double elbowPos) {
@@ -27,7 +27,7 @@ private ElbowPreset(double elbowPos) {
   }
 
   public static enum ElevatorPreset {
-    kHatchLevelOne(-27), kHatchLevelTwo(-12.5), kHatchLevelThree(28), kCargoLevelOne(20), kCargoLevelTwo(20),
+    kHatchLevelOne(-27), kHatchLevelTwo(-12.5), kHatchLevelThree(30), kCargoLevelOne(20), kCargoLevelTwo(20),
     kCargoLevelThree(53), kCargoLoadingLevel(7.85), kCargoShipCargo(25);
     public double pos;
 

diff --git a/src/main/java/org/team3197/frc2019/robot/subsystems/Arm.java b/src/main/java/org/team3197/frc2019/robot/subsystems/Arm.java
@@ -50,32 +50,6 @@ public Arm() {
       wrist.setInverted(false);
     }
 
-    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 = 5700;
-
-    // Smart Motion Coefficients
-    final double maxVel = 2000; // rpm
-    final double maxAcc = 1500;
-
-    // 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);
 

diff --git a/src/main/java/org/team3197/frc2019/robot/subsystems/Erector.java b/src/main/java/org/team3197/frc2019/robot/subsystems/Erector.java
@@ -1,15 +1,18 @@
 package org.team3197.frc2019.robot.subsystems;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.ControlType;
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import org.team3197.frc2019.robot.RobotMap;
+import org.team3197.frc2019.robot.RobotMap.DeadbandType;
 import org.team3197.frc2019.robot.commands.defaults.Erect;
 import org.team3197.frc2019.robot.utilities.Drivable;
 
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Erector extends Subsystem implements Drivable {
 
@@ -24,15 +27,65 @@ public Erector() {
     left.setIdleMode(IdleMode.kBrake);
     right.setIdleMode(IdleMode.kBrake);
 
-    right.setInverted(true);
+    right.follow(left, true);
+
+    final double kP = 5e-2;
+    final double kI = 1e-4;
+    final double kD = 0;
+    final double kIz = 0;
+    final double kFF = 0.000156;
+    final double kMaxOutput = 1;
+    final double kMinOutput = -1;
+
+    // Smart Motion Coefficients
+    final double maxVel = 2000; // rpm
+    final double maxAcc = 1500;
+
+    // set PID coefficients
+    left.getPIDController().setP(kP);
+    left.getPIDController().setI(kI);
+    left.getPIDController().setD(kD);
+    left.getPIDController().setIZone(kIz);
+    left.getPIDController().setFF(kFF);
+    left.getPIDController().setOutputRange(kMinOutput, kMaxOutput);
+    right.getPIDController().setP(kP);
+    right.getPIDController().setI(kI);
+    right.getPIDController().setD(kD);
+    right.getPIDController().setIZone(kIz);
+    right.getPIDController().setFF(kFF);
+    right.getPIDController().setOutputRange(kMinOutput, kMaxOutput);
+
+    final int smartMotionSlot = 0;
+    left.getPIDController().setSmartMotionMaxVelocity(maxVel, smartMotionSlot);
+    left.getPIDController().setSmartMotionMaxAccel(maxAcc, smartMotionSlot);
+    right.getPIDController().setSmartMotionMaxVelocity(maxVel, smartMotionSlot);
+    right.getPIDController().setSmartMotionMaxAccel(maxAcc, smartMotionSlot);
   }
 
   @Override
   public void initDefaultCommand() {
     setDefaultCommand(new Erect(this));
   }
 
+  boolean stoppedLast = false;
+  double encoderLastLeft = 0;
+  double encoderLastRight = 0;
+
   public void drive(double speed, boolean hold) {
-    erectorGroup.set(speed);
+    // 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();
+      }
+      right.getPIDController().setReference(encoderLastRight, ControlType.kPosition);
+      left.getPIDController().setReference(encoderLastLeft, ControlType.kPosition);
+    } else {
+      stoppedLast = false;
+      // left.getPIDController().setReference(speed, ControlType.kDutyCycle);
+      left.set(speed);
+    }
   }
 }