package org.usfirst.frc2832.Robot_2016.commands.autonomous;

import org.usfirst.frc2832.Robot_2016.DriveEncoders;
import org.usfirst.frc2832.Robot_2016.RobotMap;
import org.usfirst.frc2832.Robot_2016.TrajectoryController;

import edu.wpi.first.wpilibj.command.Command;

/**
 *
 */
public class  MoveForward extends Command {
	
	static double dist, initVal;
	static final double TOLERANCE = 0.05;
	static TrajectoryController tc;
	
    public MoveForward(double distance) {
        // Use requires() here to declare subsystem dependencies
        // e.g. requires(chassis);
    	dist = distance;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	initVal = DriveEncoders.getAbsoluteValue();
    	tc = new TrajectoryController(dist, 0.4, 0.4, 0.6, 0.5, -0.5); //TO-DO: would be nice to test these numbers!
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	RobotMap.driveTrain.arcadeDrive(-tc.get(DriveEncoders.getAbsoluteValue() - initVal), 0); //set speed to one given by Trajectory Controller.
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return Math.abs(DriveEncoders.getAbsoluteValue() - initVal - dist) < TOLERANCE;
    }

    // Called once after isFinished returns true
    protected void end() {
    	RobotMap.driveTrain.arcadeDrive(0, 0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}

@@ -361,16 +361,45 @@
  initVal
+, startAngle, correctiveForce
 ;%0D%0A%09stat
@@ -430,17 +430,61 @@
 E = 0.05
-;
+, kP = 0.05; //constant of proportion for PID
 %0D%0A%09stati
@@ -951,16 +951,58 @@
 mbers!%0D%0A
+    %09startAngle = RobotMap.imu.getYaw();%0D%0A
     %7D%0D%0A%0D
@@ -1094,24 +1094,184 @@
 xecute() %7B%0D%0A
+    %09correctiveForce = kP * (RobotMap.imu.getYaw() - startAngle); //corrects for displaced angles while moving forward (e.g. over Ramparts); sign may be wrong%0D%0A
     %09RobotMa
@@ -1348,17 +1348,31 @@
 itVal), 
-0
+correctiveForce
 ); //set

Add crude gyro correction to MoveForward using PID (really just P)