in the middle of a change

Author: Zhang

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

@@ -241,20 +241,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

@@ -45,7 +45,25 @@ 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 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);
+			}
+		};
 	}
 
 	/**

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

@@ -56,7 +56,27 @@ 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 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));
+			}
+		};
 		// tim = new Timer();
 	}
 

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,8 @@ 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;
-	}
-
+	
+	
 	private double convertNtokG(double newtons) {
 		// weight / accel due to grav = kg
 		return newtons / 9.81;