implement PID control for running shooter (motors)

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
index 04e0582beee858383d22fb1be947af965c36a212..590f4d59ee6ec6817c3071b770b00dc70b0d1209 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
@@ -2,7 +2,6 @@ package org.usfirst.frc.team3501.robot.subsystems;
 
 import org.usfirst.frc.team3501.robot.Constants;
 import org.usfirst.frc.team3501.robot.Lidar;
-import org.usfirst.frc.team3501.robot.MathLib;
 
 import edu.wpi.first.wpilibj.CANTalon;
 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
@@ -21,8 +20,9 @@ import edu.wpi.first.wpilibj.command.PIDSubsystem;
  */
 
 public class Shooter extends PIDSubsystem {
-  private static double kp = 0.013, ki = 0.000015, kd = -0.002;
+  private static double sp = 0.015, si = 0, sd = 0;
   private static double encoderTolerance = 5.0;
+  private static double pidOutput = 5.0;
 
   private CANTalon shooter;
   private DoubleSolenoid hood, punch;
@@ -30,7 +30,7 @@ public class Shooter extends PIDSubsystem {
   private Lidar lidar;
 
   public Shooter() {
-    super(kp, ki, kd);
+    super(sp, si, sd);
 
     shooter = new CANTalon(Constants.Shooter.PORT);
     hood = new DoubleSolenoid(Constants.Shooter.HOOD_FORWARD,
@@ -57,8 +57,8 @@ public class Shooter extends PIDSubsystem {
   }
 
   public void setSpeed(double speed) {
-    speed = MathLib.constrain(speed, -1, 1);
-    shooter.set(speed);
+    setShooterPID();
+    setSetpoint(speed);
   }
 
   // This getDistance() will return the distance using the lidar from the
@@ -110,20 +110,28 @@ public class Shooter extends PIDSubsystem {
 
   @Override
   protected double returnPIDInput() {
-    // TODO Auto-generated method stub
-    return 0;
+    return sensorFeedback();
   }
 
   @Override
   protected void usePIDOutput(double output) {
-    // TODO Auto-generated method stub
+    shooter.set(output);
+    pidOutput = output;
+  }
 
+  private double sensorFeedback() {
+    return this.getSpeed();
   }
 
-  public void setEncoderPID() {
+  public void setShooterPID() {
+    this.updatePID();
     this.setAbsoluteTolerance(encoderTolerance);
     this.setOutputRange(-1.0, 1.0);
-    this.setInputRange(-200.0, 200.0);
+    this.setInputRange(-500.0, 500.0);
     this.enable();
   }
+
+  public void updatePID() {
+    this.getPIDController().setPID(sp, si, sd);
+  }
 }