Override calculateFeedForward methods of PIDControllers in PIDMove and PIDTurn

Author: swzhangmit

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java

@@ -47,21 +47,17 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 		moveController = new PIDController(sd.getConst("MovekP", 0.1), sd.getConst("MovekI", 0),
 				sd.getConst("MovekD", 0), kf, avg, this) {
 			/**
-			 * Move Velocity: V = sqrt(8TGd) / (R*m)) 
+			 * Move Velocity: V = sqrt(8TGd) / (R*m) 
 			 * where T = max torque of wheels 
 			 * G = gear ratio 
 			 * d = distance remaining 
 			 * R = radius of wheels 
 			 * m = mass 
 			 */
 			@Override
-			protected double calculateFeedForward() {
-				double T = SmartDashboard.getNumber(,);
-				double G = SmartDashboard.getNumber(,);
-				double d = SmartDashboard.getNumber(,);
-				double R = SmartDashboard.getNumber(,);
-				double m = SmartDashboard.getNumber(,);
-				return (d / Math.abs(d)) * Math.sqrt(8*T*G*Math.abs(d)) / (R*m);
+			protected double calculateFeedForward() {		
+				double feedForwardConst = dt.getPIDMoveConstant();
+				return (targ / Math.abs(targ)) * feedForwardConst * Math.sqrt(Math.abs(targ));
 			}
 		};
 	}

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java

@@ -19,6 +19,7 @@ public class PIDTurn extends Command implements PIDOutput {
 
 	private double target;
 	private DrivetrainInterface dt;
+	private SmartDashboardInterface sd;
 	private PIDController turnController;
 	private PIDSource ahrs;
 	private Timer tim;
@@ -46,20 +47,21 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 		// Use requires() here to declare subsystem dependencies
 		target = targ;
 		this.dt = dt;
+		this.sd = sd;
 		this.ahrs = ahrs;
 
 		if (Robot.dt != null) {
 			requires(Robot.dt);
 		}
 		// calculates the maximum turning speed in degrees/sec based on the max linear
 		// speed in inches/s and the distance (inches) between sides of the DT
-		double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * sd.getConst("Distance Between Wheels", 26.25));
+		double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * getDistanceBetweenWheels());
 		double kf = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05));
 		turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0),
 				kf, ahrs, this) {
 			/**
 			 * Turn Velocity: V = 4r sqrt((T*G*theta) / (R*m))
-			 * where r = half of' distance between wheels
+			 * where r = half of distance between wheels
 			 * T = max torque of wheels
 			 * G = gear ratio
 			 * theta = rotational distance to end of turn
@@ -68,13 +70,8 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 			 */
 			@Override
 			protected double calculateFeedForward() {
-				double r = 
-				double T = SmartDashboard.getNumber(,);
-				double G = SmartDashboard.getNumber(,);
-				double theta = SmartDashboard.getNumber(,);
-				double R = SmartDashboard.getNumber(,);
-				double m = SmartDashboard.getNumber(,);
-				return 4*r*(theta / Math.abs(theta)) * Math.sqrt((T*G*theta) / (R*m));
+				double feedForwardConst = dt.getPIDTurnConstant();
+				return feedForwardConst * (getDistanceBetweenWheels()/2) * (targ / Math.abs(targ)) * Math.sqrt(Math.abs(targ));
 			}
 		};
 		// tim = new Timer();
@@ -183,4 +180,12 @@ public void pidWrite(double output) {
 		dt.arcadeDrive(0, output);
 		SmartDashboard.putNumber("Turn PID Output", output);
 	}
+	
+	/**
+	 * Gets the distance between the two middle wheels.
+	 * @return that distance
+	 */
+	private double getDistanceBetweenWheels() {
+		return sd.getConst("Distance Between Wheels", 26.25);
+	}
 }

Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java

@@ -403,7 +403,21 @@ public void putVelocityControllersToDashboard() {
 		SmartDashboard.putData("Right PID Controller", rightVelocityController);
 	}
 
+	public double getPIDMoveConstant() {
+		double G = Robot.rmap.getGearRatio();
+		double T = Robot.rmap.getStallTorque();
+		double R = Robot.rmap.getRadius();
+		double M = Robot.rmap.getWeight();
+		return Math.sqrt((8*T*G)/(R*M));
+	}
 
+	public double getPIDTurnConstant() {
+		double G = Robot.rmap.getGearRatio();
+		double T = Robot.rmap.getStallTorque();
+		double R = Robot.rmap.getRadius();
+		double M = Robot.rmap.getWeight();
+		return 4 * Math.sqrt((T*G) / (R*M));
+	}
 
 	private double convertNtokG(double newtons) {
 		// weight / accel due to grav = kg

Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java

@@ -172,4 +172,16 @@ public interface DrivetrainInterface {
 	 * Put left and right velocity controllers (PID) on SmartDashboard.
 	 */
 	public void putVelocityControllersToDashboard();
+	
+	/**
+	 * Calculates a constant for calculating feed forward in PIDMove
+	 * @return the constant
+	 */
+	public double getPIDMoveConstant();
+	
+	/**
+	 * Calculates a constant for calculating feed forward in PIDTurn
+	 * @return the constant
+	 */
+	public double getPIDTurnConstant();
 }