Merge branch 'master' of

https://github.com/theEgotisticalGiraffe/RobotCode2018.git into vel-pid

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

Author: DriverStationComputer

Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java

@@ -239,20 +239,44 @@ public double calcDefkD(double maxSpeed) {
 		 * be converted from rpm to radians per second, so divide by 60 and multiply to
 		 * get radians.
 		 */
-		double gearReduction = Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
-				: Robot.getConst("Low Gear Gear Reduction", 12.255);
-		double radius = Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
-		double timeConstant = Robot.getConst("Omega Max", 5330) / gearReduction / 60 * 2 * Math.PI
-				* convertNtokG(Robot.getConst("Weight of Robot", 342)) / 2 * radius * radius
-				/ (Robot.getConst("Stall Torque", 2.41) * gearReduction * 2);
-		double cycleTime = Robot.getConst("Code cycle time", 0.05);
+		double gearReduction = getGearRatio();
+		double radius = getRadius();
+		double timeConstant = getOmegaMax() / gearReduction / 60 * 2 * Math.PI
+				* convertNtokG(getWeight()) / 2 * radius * radius
+				/ (getStallTorque() * gearReduction * 2);
+		double cycleTime = getCycleTime();
 		/*
 		 * The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is
 		 * one.
 		 */
 		double denominator = Math.pow(Math.E, 1 * cycleTime / timeConstant) - 1;
 		return 1 / denominator / maxSpeed;
 	}
+	
+	public double getGearRatio() {
+		return Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
+				: Robot.getConst("Low Gear Gear Reduction", 12.255);
+	}
+	
+	public double getRadius() {
+		return Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
+	}
+	
+	public double getOmegaMax() {
+		return Robot.getConst("Omega Max", 5330);
+	}
+	
+	public double getWeight() {
+		return Robot.getConst("Weight of Robot", 342);
+	}
+	
+	public double getCycleTime() {
+		return Robot.getConst("Code cycle time", 0.05);
+	}
+	
+	public double getStallTorque() {
+		return Robot.getConst("Stall Torque", 2.41);
+	}
 
 	private double convertLbsTokG(double lbs) {
 		// number from google ;)

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

@@ -51,7 +51,18 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 		}
 		double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
 		moveController = new PIDController(sd.getConst("MovekP", 0.1), sd.getConst("MovekI", 0),
-				sd.getConst("MovekD", 0), kf, avg, this);
+				sd.getConst("MovekD", 0), kf, avg, this) {
+			/**
+			 * 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 feedForwardConst = dt.getPIDMoveConstant();
+				double setPt = getSetpoint();
+				return (setPt / Math.abs(setPt)) * feedForwardConst * Math.sqrt(Math.abs(setPt));
+			}
+		};
 		sd.putData("Move PID", moveController);
 	}
 
@@ -86,6 +97,20 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
 		moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0),
 				kf, avg, this);
 		sd.putData("Move PID", moveController);
+		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) where T = max torque of wheels G = gear
+			 * ratio d = distance remaining R = radius of wheels m = mass
+			 */
+			@Override
+			protected double calculateFeedForward() {
+				double feedForwardConst = dt.getPIDMoveConstant();
+				double setPt = getSetpoint();
+				return (setPt / Math.abs(setPt)) * feedForwardConst * Math.sqrt(Math.abs(setPt));
+			}
+		};
+		sd.putData("Move PID", moveController);
 	}
 
 	/**

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

@@ -68,7 +68,20 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 		double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * sd.getConst("Distance Between Wheels", 26.25));
 		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);
+				kf, ahrs, this) {
+			/**
+			 * Turn Velocity: V = 4r sqrt((T*G*theta) / (R*m)) where r = half of distance
+			 * between wheels T = max torque of wheels G = gear ratio theta = rotational
+			 * distance to end of turn R = radius of wheels m = mass
+			 */
+			@Override
+			protected double calculateFeedForward() {
+				double feedForwardConst = dt.getPIDTurnConstant();
+				double setpt = getSetpoint();
+				return feedForwardConst * (getDistanceBetweenWheels() / 2) * (setpt / Math.abs(setpt))
+						* Math.sqrt(Math.abs(setpt));
+			}
+		};
 		// tim = new Timer();
 	}
 
@@ -114,6 +127,7 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 	 */
 	public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) {
 		this.dt = dt;
+		this.sd = sd;
 		this.ahrs = ahrs;
 		this.sd = sd;
 
@@ -127,10 +141,23 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
 		}
 		// 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);
+				kf, ahrs, this) {
+			/**
+			 * Turn Velocity: V = 4r sqrt((T*G*theta) / (R*m)) where r = half of distance
+			 * between wheels T = max torque of wheels G = gear ratio theta = rotational
+			 * distance to end of turn R = radius of wheels m = mass
+			 */
+			@Override
+			protected double calculateFeedForward() {
+				double feedForwardConst = dt.getPIDTurnConstant();
+				double setpt = getSetpoint();
+				return feedForwardConst * (getDistanceBetweenWheels() / 2) * (setpt / Math.abs(setpt))
+						* Math.sqrt(Math.abs(setpt));
+			}
+		};
 		// tim = new Timer();
 		sd.putData("Turn PID", turnController);
 	}
@@ -259,4 +286,13 @@ 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

@@ -44,7 +44,7 @@ public class Drivetrain extends Subsystem implements DrivetrainInterface {
 	private final DifferentialDrive robotDrive = RobotMap.robotDrive;
 	private final VelocityPIDController leftVelocityController = RobotMap.leftVelocityController;
 	private final VelocityPIDController rightVelocityController = RobotMap.rightVelocityController;
-
+	
 	private final AHRS fancyGyro = RobotMap.fancyGyro;
 	private final DoubleSolenoid dtGear = RobotMap.dtGear;
 
@@ -222,8 +222,8 @@ public double getDtRightSpeed() {
 	 */
 	@Override
 	public void updatePidConstants() {
-		leftVelocityController.setPID(Robot.getConst("VelocityLeftkI", 0), 0, calcDefkD(getCurrentMaxSpeed()));
-		rightVelocityController.setPID(Robot.getConst("VelocityRightkI", 0), 0, calcDefkD(getCurrentMaxSpeed()));
+		leftVelocityController.setPID(Robot.getConst("VelocityLeftkI", 0), 0, Robot.rmap.calcDefkD(getCurrentMaxSpeed()));
+		rightVelocityController.setPID(Robot.getConst("VelocityRightkI", 0), 0, Robot.rmap.calcDefkD(getCurrentMaxSpeed()));
 		resetVelocityPIDkFConsts();
 	}
 
@@ -403,33 +403,22 @@ public void putVelocityControllersToDashboard() {
 		SmartDashboard.putData("Right PID Controller", rightVelocityController);
 	}
 
-	/**
-	 * Uses SmartDashboard and math to calculate a *great* default kD
-	 */
-	public double calcDefkD(double maxSpeed) {
-		/*
-		 * timeConstant is proportional to max speed of the shaft (which is the max
-		 * speed of the cim divided by the gear reduction), half the mass (because the
-		 * half of the drivetrain only has to support half of the robot), and radius of
-		 * the drivetrain wheels squared. It's inversely proportional to the stall
-		 * torque of the shaft, which is found by multiplying the stall torque of the
-		 * motor with the gear reduction by the amount of motors.
-		 */
-		double gearReduction = Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
-				: Robot.getConst("Low Gear Gear Reduction", 12.255);
-		double radius = Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
-		double timeConstant = Robot.getConst("Omega Max", 5330) / gearReduction / 60 * 2 * Math.PI
-				* convertNtokG(Robot.getConst("Weight of Robot", 342)) / 2 * radius * radius
-				/ (Robot.getConst("Stall Torque", 2.41) * gearReduction * 2);
-		double cycleTime = Robot.getConst("Code cycle time", 0.05);
-		/*
-		 * The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is
-		 * one.
-		 */
-		double denominator = Math.pow(Math.E, 1 * cycleTime / timeConstant) - 1;
-		return 1 / denominator / maxSpeed;
+	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
 		return newtons / 9.81;

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();
 }