Finished all the cases in which there are two points traveling in the…

… same direction and the first point has a custom max velocity. About to change the code and probably break a whole bunch of cases to get the cases working where the second point has a custom max velocity

eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/RobotMap.java~

@@ -25,20 +25,20 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow;
  */
 public class RobotMap {
     // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
-    public static CANTalon driveTrainUpLeft;
-    public static CANTalon driveTrainUpRight;
+    public static CANTalon driveTrainFrontLeft;
+    public static CANTalon driveTrainFrontRight;
     public static CANTalon driveTrainBackLeft;
     public static CANTalon driveTrainBackRight;
 
     // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
 
     public static void init() {
         // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
-        driveTrainUpLeft = new CANTalon(0);
-        LiveWindow.addActuator("DriveTrain", "UpLeft", driveTrainUpLeft);
+        driveTrainFrontLeft = new CANTalon(0);
+        LiveWindow.addActuator("DriveTrain", "FrontLeft", driveTrainFrontLeft);
 
-        driveTrainUpRight = new CANTalon(1);
-        LiveWindow.addActuator("DriveTrain", "UpRight", driveTrainUpRight);
+        driveTrainFrontRight = new CANTalon(1);
+        LiveWindow.addActuator("DriveTrain", "FrontRight", driveTrainFrontRight);
 
         driveTrainBackLeft = new CANTalon(2);
         LiveWindow.addActuator("DriveTrain", "BackLeft", driveTrainBackLeft);

eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.java~

@@ -69,7 +69,7 @@ public class MoveWithEncoder extends Command {
 		Robot.driveTrain.moveToEncoderRevolutions(trajectoryPoint.m_position * m_positionToEncoderRevolutionsRatio);
 		m_PIDOut = Robot.driveTrain.getEncoderPIDOutput();
 
-		Robot.driveTrain.setDriveControllers(trajectoryPoint.m_currentVelocity * m_velocityToMotorOutputRatio
+		Robot.driveTrain.setAllDriveControllers(trajectoryPoint.m_currentVelocity * m_velocityToMotorOutputRatio
 				+ trajectoryPoint.m_acceleration * m_accelerationToMotorOutputRatio + m_PIDOut);
 	}
 

eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java~

@@ -0,0 +1,94 @@
+// RobotBuilder Version: 2.0
+//
+// This file was generated by RobotBuilder. It contains sections of
+// code that are automatically generated and assigned by robotbuilder.
+// These sections will be updated in the future when you export to
+// Java from RobotBuilder. Do not put any code or make any change in
+// the blocks indicating autogenerated code or it will be lost on an
+// update. Deleting the comments indicating the section will prevent
+// it from being updated in the future.
+
+package org.usfirst.frc4905.MotionProfiling.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+
+import java.util.Vector;
+
+import org.usfirst.frc4905.MotionProfiling.Robot;
+
+import Utilities.Trace;
+
+/**
+ *
+ */
+public class RunWheelsAndLog extends Command {
+	Vector<String> m_header = new Vector<String>();
+	double m_initialTimeStamp = 0.0;
+	double m_previousVelocity = 0.0;
+	double m_previousTimeStamp = 0.0;
+	// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
+
+	// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
+
+	// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
+	public RunWheelsAndLog() {
+
+		// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
+		// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
+
+		// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
+		// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
+		requires(Robot.driveTrain);
+
+		// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
+	}
+
+	// Called just before this Command runs the first time
+	protected void initialize() {
+		Vector<String> header = new Vector<String>();
+		header.add(new String("Velocities"));
+		header.add(new String("Acceleration"));
+		header.addElement(new String("Time Between Runs"));
+		Trace.getInstance().addTrace("Motion Profiling Data", header);
+		m_header = header;
+		m_initialTimeStamp = Timer.getFPGATimestamp();
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	protected void execute() {
+		Robot.driveTrain.setAllDriveControllers(1.0);
+
+		Vector<Double> entry = new Vector<Double>();
+		double currentTimeStamp = Timer.getFPGATimestamp();
+		double currentDeltaTimeStampFromStart = currentTimeStamp - m_initialTimeStamp;
+		double deltaTimeFromLastExecute = currentTimeStamp - m_previousTimeStamp;
+
+		double currentPosition = Robot.driveTrain.getAllEncoderPosition();
+		double currentVelocity = currentPosition / currentDeltaTimeStampFromStart;
+		double currentAcceleration = currentVelocity - m_previousVelocity;
+
+		entry.add(currentVelocity);
+		entry.add(currentAcceleration);
+		entry.add(deltaTimeFromLastExecute);
+
+		Trace.getInstance().addEntry("Motion Profiling Data", entry);
+
+		m_previousTimeStamp = currentTimeStamp;
+		m_previousVelocity = currentVelocity;
+	}
+
+	// Make this return true when this Command no longer needs to run execute()
+	protected boolean isFinished() {
+		return false;
+	}
+
+	// Called once after isFinished returns true
+	protected void end() {
+	}
+
+	// Called when another command which requires one or more of the same
+	// subsystems is scheduled to run
+	protected void interrupted() {
+	}
+}

eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.java~

@@ -32,8 +32,8 @@ public class DriveTrain extends Subsystem {
     // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS
 
 	// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
-    private final CANTalon upLeft = RobotMap.driveTrainUpLeft;
-    private final CANTalon upRight = RobotMap.driveTrainUpRight;
+    private final CANTalon frontLeft = RobotMap.driveTrainFrontLeft;
+    private final CANTalon frontRight = RobotMap.driveTrainFrontRight;
     private final CANTalon backLeft = RobotMap.driveTrainBackLeft;
     private final CANTalon backRight = RobotMap.driveTrainBackRight;
 
@@ -58,21 +58,27 @@ public class DriveTrain extends Subsystem {
 		double frontRight;
 		double frontLeft;
 	}
+
 	public double getEncoderPIDOutput() {
 		return m_encoderPIDOutput;
 	}
-	public void setDriveControllers(double value) {
-
+
+	public void setAllDriveControllers(double value) {
+		backLeft.set(value);
+		backRight.set(value);
+		frontLeft.set(value);
+		frontRight.set(value);
 	}
 
-	private double getEncoderPosition() {
-		return 0.0;
+	public double getAllEncoderPosition() {
+		return backLeft.get() + backRight.get() + frontLeft.get() + frontRight.get();
 	}
+
 	private void resetEncoderPosition(){
-		m_resetEncoderPositions.backLeft = 0.0;
-		m_resetEncoderPositions.backRight = 0.0;
-		m_resetEncoderPositions.frontRight = 0.0;
-		m_resetEncoderPositions.frontLeft = 0.0;
+		m_resetEncoderPositions.backLeft = backLeft.get();
+		m_resetEncoderPositions.backRight = backRight.get();
+		m_resetEncoderPositions.frontRight = frontRight.get();
+		m_resetEncoderPositions.frontLeft = frontLeft.get();
 	}
 	// Encoder PID controller
 	private PIDController m_moveWithEncoderPID;
@@ -95,7 +101,7 @@ public class DriveTrain extends Subsystem {
 
 		@Override
 		public double pidGet() {
-			return getEncoderPosition();
+			return getAllEncoderPosition();
 		}
 
 	}