Motion profiling

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

@@ -53,7 +53,7 @@ public class Robot extends IterativeRobot {
 	public static OI oi;
 
 	public static Map<String, ArrayList<String[]>> autoScripts;
-
+	public static boolean auto;
 	public static boolean stopIntake = false;
 
 	Command autonomousCommand;
@@ -210,6 +210,7 @@ public void disabledPeriodic() {
 	 */
 	@Override
 	public void autonomousInit() {
+		auto = true;
 		dt.resetAHRS();
 		AutoUtils.state = new State(0, 0, 0);
 		Scheduler.getInstance().add(new ShiftLowGear());
@@ -239,6 +240,7 @@ public void autonomousPeriodic() {
 
 	@Override
 	public void teleopInit() {
+		auto = false;
 		System.out.println("In teleopInit()");
 		dt.resetAHRS();
 		AutoUtils.state = new State(0, 0, 0);

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

@@ -73,6 +73,7 @@ public class RobotMap {
 	public static DoubleSolenoid dtGear;
 
 	private final double DIST_PER_PULSE_RATIO = (5.0 * Math.PI) * (17.0 / 25) / (3.0 * 256);
+	public final double gravity = 9.80665;
 
 	/**
 	 * This function takes in a TalonSRX motorController and sets nominal and peak

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

@@ -0,0 +1,119 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+import org.usfirst.frc.team199.Robot2018.RobotMap;
+import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
+import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
+
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Trajectory;
+import jaci.pathfinder.Waypoint;
+
+/**
+ *
+ */
+public class MotionProfilingMove extends Command {
+
+	private double target;
+	private DrivetrainInterface dt;
+	private PIDSource avg;
+	private SmartDashboardInterface sd;
+	private double[] point;
+	private boolean usePoint;
+	Trajectory traj;
+	int i;
+
+	public MotionProfilingMove(double target, double[] point, DrivetrainInterface dt, SmartDashboardInterface sd,
+			PIDSource avg, boolean usePoint) {
+		// Use requires() here to declare subsystem dependencies
+		// eg. requires(chassis);
+		this.point = point;
+		this.target = target;
+		this.usePoint = usePoint;
+		this.dt = dt;
+		this.avg = avg;
+		this.sd = sd;
+		if (Robot.dt != null) {
+			requires(Robot.dt);
+		}
+	}
+
+	public MotionProfilingMove(double target, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) {
+		this(target, null, dt, sd, avg, false);
+	}
+
+	public MotionProfilingMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) {
+		this(0, point, dt, sd, avg, true);
+	}
+
+	// Called just before this Command runs the first time
+	protected void initialize() {
+		if (!usePoint) {
+			// use point because easier for motion profiling
+			point[0] = 0;
+			point[1] = target;
+		}
+		Waypoint[] pt = new Waypoint[1];
+		pt[0] = new Waypoint(point[0], point[1], 0);
+		Trajectory.FitMethod fit = Robot.getBool("MP_CubicTraj", true) ? Trajectory.FitMethod.HERMITE_CUBIC : Trajectory.FitMethod.HERMITE_QUINTIC;
+		//0 samples is fast (1k), 1 is low (10k), and 2 is fast (100k)
+		int samples = findSamples(Robot.getConst("MP_AmtSamples", 0));
+//		int samples = (int) Robot.getConst("MP_AmtSamples", 1000);
+		double timestep = Robot.rmap.getCycleTime();
+		double maxVelocity = Robot.dt.getCurrentMaxSpeed();
+		LiftHeight currPos = Robot.lift.getCurrPos();
+		if (Robot.auto && currPos != LiftHeight.GROUND && currPos != LiftHeight.HOLD_CUBE) maxVelocity *= Robot.getConst("Auto Max Speed", 0.5);
+		double maxAccel = Robot.rmap.gravity;
+			//the acceleration = F/m, and F = coefficient of friction (1) * weight (mg), so a = mg/m = g
+		double maxJerk = Robot.getConst("MP_MaxJerk", 4);
+			//Kevin said that a good random small number was 4
+		Trajectory.Config config = new Trajectory.Config(fit, samples, timestep, maxVelocity, maxAccel, maxJerk);
+		traj = Pathfinder.generate(pt, config);
+		i = 0;
+		//TODO Use the built-in modifications to make everything better
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	protected void execute() {
+		Trajectory.Segment seg = traj.get(i);
+		double prevX = i == 0 ? 0 : traj.get(i-1).x;
+		double prevY = i == 0 ? 0 : traj.get(i-1).y;
+		double[] newPoint = new double[2];
+		newPoint[0] = seg.x - prevX;
+		newPoint[1] = seg.y - prevY;
+		Scheduler.getInstance().add(new PIDMove(newPoint, dt, sd, avg));
+		i++;
+	}
+
+	// Make this return true when this Command no longer needs to run execute()
+	protected boolean isFinished() {
+		return i >= traj.length();
+	}
+
+	// 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() {
+		end();
+	}
+
+	private int findSamples(double key) {
+		switch ((int)key) {
+		case 0:
+			return Trajectory.Config.SAMPLES_FAST;
+		case 1:
+			return Trajectory.Config.SAMPLES_LOW;
+		case 2:
+			return Trajectory.Config.SAMPLES_HIGH;
+		default:
+			return Trajectory.Config.SAMPLES_FAST;
+		}
+	}
+}

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

@@ -4,6 +4,7 @@
 import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
 import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
 import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
 
 import edu.wpi.first.wpilibj.PIDController;
 import edu.wpi.first.wpilibj.PIDOutput;
@@ -133,7 +134,13 @@ public void initialize() {
 		dt.resetDistEncs();
 		moveController.disable();
 		// output in "motor units" (arcade and tank only accept values [-1, 1]
-		moveController.setOutputRange(-1.0, 1.0);
+		LiftHeight currPos = Robot.lift.getCurrPos();
+		if (Robot.auto && currPos != LiftHeight.GROUND && currPos != LiftHeight.HOLD_CUBE) {
+			moveController.setOutputRange(Robot.getConst("Auto Min Speed", -0.5),
+					Robot.getConst("Auto Max Speed", 0.5));
+		} else {
+			moveController.setOutputRange(-1.0, 1.0);
+		}
 		moveController.setContinuous(false);
 		moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.5));
 		System.out.println("move target = " + target);