Drive.java

package org.usfirst.frc4388.robot.subsystems;

import java.util.ArrayList;
import java.util.Set;

import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.robot.OI;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.RobotMap;
import org.usfirst.frc4388.utility.AdaptivePurePursuitController;
import org.usfirst.frc4388.utility.BHRMathUtils;
import org.usfirst.frc4388.utility.CANTalonEncoder;
import org.usfirst.frc4388.utility.ControlLoopable;
import org.usfirst.frc4388.utility.Kinematics;
import org.usfirst.frc4388.utility.MMTalonPIDController;
import org.usfirst.frc4388.utility.MPSoftwarePIDController;
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
import org.usfirst.frc4388.utility.MPTalonPIDController;
import org.usfirst.frc4388.utility.MPTalonPIDPathController;
import org.usfirst.frc4388.utility.MPTalonPIDPathVelocityController;
//import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
import org.usfirst.frc4388.utility.MotionProfilePoint;
//import org.usfirst.frc4388.utility.MotionProfileBoxCar;
import org.usfirst.frc4388.utility.PIDParams;
import org.usfirst.frc4388.utility.Path;
import org.usfirst.frc4388.utility.PathGenerator;
import org.usfirst.frc4388.utility.RigidTransform2d;
import org.usfirst.frc4388.utility.Rotation2d;
import org.usfirst.frc4388.utility.SoftwarePIDController;
import org.usfirst.frc4388.utility.Translation2d;

import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
//import com.ctre.PigeonImu;
//import com.ctre.PigeonImu.CalibrationMode;
import com.ctre.phoenix.motorcontrol.NeutralMode;

import com.kauailabs.navx.frc.AHRS;

//import edu.wpi.first.wpilibj.DigitalInput;
//import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
//import edu.wpi.first.wpilibj.Solenoid;
//import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;


/**
 *
 */
public class Drive extends Subsystem implements ControlLoopable
{
	public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, MOVE_POSITION_MAX_SCALE, MOVE_POSITION_LOW_SCALE, MOVE_POSITION_SWITCH, MOVE_POSITION_LOWEST, STOP_MOTORS, RAW};
	public static enum SpeedShiftState { HI, LO };
	public static enum ClimberState { DEPLOYED, RETRACTED };

	public static final double TRACK_WIDTH_INCHES = Constants.kTrackWidthInches;
	public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch;
	
	public static final double CLIMB_SPEED = 0.45;
	
	public static final double VOLTAGE_RAMP_RATE = 150;  // Volts per second
	public static final double OPEN_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE;	// Seconds from neutral to full
	public static final double CLOSED_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE;	// Seconds from neutral to full

	// Motion profile max velocities and accel times
	public static final double MAX_TURN_RATE_DEG_PER_SEC = 320;
	public static final double MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC =  120;  //72;
	public static final double MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC =  200;  
	public static final double MP_AUTON_MAX_LO_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC =  320;  
	public static final double MP_AUTON_MAX_HIGH_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC =  400;  
	public static final double MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC =  270;
	public static final double MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC =  400;
	public static final double MP_GEAR_DEPLOY_VELOCITY_INCHES_PER_SEC = 25;
	public static final double MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC = 80;
	
	
	
	
	public static final double MP_STRAIGHT_T1 = 600;
	public static final double MP_STRAIGHT_T2 = 300;
	public static final double MP_TURN_T1 = 600;
	public static final double MP_TURN_T2 = 300;
	public static final double MP_MAX_TURN_T1 = 400;
	public static final double MP_MAX_TURN_T2 = 200;
	
	// Motor controllers
	private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();	

	private CANTalonEncoder leftDrive1;
	private WPI_TalonSRX leftDrive2;
//	private WPI_TalonSRX leftDrive3;
	
	private CANTalonEncoder rightDrive1;
	private WPI_TalonSRX rightDrive2;
//	private WPI_TalonSRX rightDrive3;

	private DifferentialDrive m_drive;
	
	//PID encoder and motor
	private CANTalonEncoder elevatorRight;
	private WPI_TalonSRX elevatorLeft;

	//private DigitalInput hopperSensorRed;
	//private DigitalInput hopperSensorBlue;

	
	private double climbSpeed;
	
	private boolean isRed = true;
	
	private double periodMs;
	private double lastControlLoopUpdatePeriod = 0.0;
	private double lastControlLoopUpdateTimestamp = 0.0;

	// Pneumatics
	//private Solenoid speedShift;

	// Input devices
	public static final int DRIVER_INPUT_JOYSTICK_ARCADE = 0;
	public static final int DRIVER_INPUT_JOYSTICK_TANK = 1;
	public static final int DRIVER_INPUT_JOYSTICK_CHEESY = 2;
	public static final int DRIVER_INPUT_XBOX_CHEESY = 3;
	public static final int DRIVER_INPUT_XBOX_ARCADE_LEFT = 4;
	public static final int DRIVER_INPUT_XBOX_ARCADE_RIGHT = 5;
	public static final int DRIVER_INPUT_WHEEL = 6;

	public static final double STEER_NON_LINEARITY = 0.5;
	public static final double MOVE_NON_LINEARITY = 1.0;
	
	public static final double STICK_DEADBAND = 0.02;

	private int m_moveNonLinear = 0;
	private int m_steerNonLinear = -3;

	private double m_moveScale = 1.0;
	private double m_steerScale = 1.0;

	private double m_moveInput = 0.0;
	private double m_steerInput = 0.0;

	private double m_moveOutput = 0.0;
	private double m_steerOutput = 0.0;

	private double m_moveTrim = 0.0;
	private double m_steerTrim = 0.0;

	private boolean isFinished;
	private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
	
	private MPTalonPIDController mpStraightController;
	private PIDParams mpStraightPIDParams = new PIDParams(.7, 0.0, 0.000000, .001, 0.001, .7);  // 4 colsons
//	private PIDParams mpStraightPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.03);  // 4 omni
	private PIDParams mpHoldPIDParams = new PIDParams(1, 0, 0, 0.0, 0.0, 0.0); 

	private MMTalonPIDController mmStraightController;
	private PIDParams mmStraightPIDParams = new PIDParams(0, 0, 0, 0.24);  
	
	private MPSoftwarePIDController mpTurnController; // p    i   d     a      v      g    izone
	private PIDParams mpTurnPIDParams = new PIDParams(0.07, 0.00002, 0.5, 0.00025, 0.008, 0.0, 100);  // 4 colson wheels
//	private PIDParams mpTurnPIDParams = new PIDParams(0.03, 0.00002, 0.4, 0.0004, 0.0030, 0.0, 100);  // 4 omni
	
	private SoftwarePIDController pidTurnController;
	//private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.001, 0.4, 0, 0, 0.0, 100); //i=0.0008
	private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.0, 0.0, 0, 0, 0.0, 100); //i=0.0008
	private double targetPIDAngle;

	private MPTalonPIDPathController mpPathController;
	private PIDParams mpPathPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.28, 100);  // 4 omni   g=.3

	//PID target
	private double targetPIDPosition;

	private MPTalonPIDPathVelocityController mpPathVelocityController;
	private PIDParams mpPathVelocityPIDParams = new PIDParams(0.5, 0.001, 5, 0.44); 

	private AdaptivePurePursuitController adaptivePursuitController;
	private PIDParams adaptivePursuitPIDParams = new PIDParams(0.1, 0.00, 1, 0.44); 
	
	private RigidTransform2d zeroPose = new RigidTransform2d(new Translation2d(0,0), Rotation2d.fromDegrees(0));
	private RigidTransform2d currentPose = zeroPose;
	private RigidTransform2d lastPose = zeroPose;
    private double left_encoder_prev_distance_ = 0;
    private double right_encoder_prev_distance_ = 0;

	//private PigeonImu gyroPigeon;
	//private double[] yprPigeon = new double[3];
    private AHRS gyroNavX;
	private boolean useGyroLock;
	private double gyroLockAngleDeg;
	//private double kPGyro = 0.04;
	private double kPGyro = 0.0625;
	private boolean isCalibrating = false;
	private double gyroOffsetDeg = 0;

	public Drive() {
		try {
			leftDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder);
			leftDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID);
//			leftDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID);
			
			rightDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder);
			rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID);
//			rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID);
			
			//gyroPigeon = new PigeonImu(leftDrive2);
			gyroNavX = new AHRS(SPI.Port.kMXP);

			//hopperSensorRed = new DigitalInput(RobotMap.HOPPER_SENSOR_RED_DIO_ID);
			//hopperSensorBlue = new DigitalInput(RobotMap.HOPPER_SENSOR_BLUE_DIO_ID);
	
			//leftDrive1.clearStickyFaults();
			//leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
			//leftDrive1.setNominalClosedLoopVoltage(12.0);
			leftDrive1.clearStickyFaults(0);
			leftDrive1.setInverted(false);//false on comp 2108, false on microbot
			leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot
			leftDrive1.setSafetyEnabled(false);
			//leftDrive1.setCurrentLimit(15);
			//leftDrive1.enableCurrentLimit(true);
			leftDrive1.setNeutralMode(NeutralMode.Brake);
			leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
			leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
			leftDrive1.configNominalOutputForward(+1.0f, 0);
			leftDrive1.configNominalOutputReverse(-1.0f, 0);

			
//	        if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
//	            Driver.reportError("Could not detect left drive encoder encoder!", false);
//	        }
			

			leftDrive2.setInverted(false);//false on comp 2108, false on microbot
			leftDrive2.setSafetyEnabled(false);
			leftDrive2.setNeutralMode(NeutralMode.Brake);
			leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID());
	

			
			//rightDrive1.clearStickyFaults();
			//rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
			//rightDrive1.setNominalClosedLoopVoltage(12.0);
			rightDrive1.clearStickyFaults(0);
			rightDrive1.setInverted(true);//true on comp 2108, false on microbot
			rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot
			rightDrive1.setSafetyEnabled(false);
			rightDrive1.setNeutralMode(NeutralMode.Brake);
			rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
			rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
			rightDrive1.configNominalOutputForward(+1.0f, 0);
			rightDrive1.configNominalOutputReverse(-1.0f, 0);
//	        if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
//	            DriverStation.reportError("Could not detect right drive encoder encoder!", false);
//	        }
			

			rightDrive2.setInverted(true);//True on comp 2108, false on microbot
			rightDrive2.setSafetyEnabled(false);
			rightDrive2.setNeutralMode(NeutralMode.Brake);
			rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID());

			

							
			motorControllers.add(leftDrive1);
			motorControllers.add(rightDrive1);

			//m_drive = new RobotDrive(leftDrive1, rightDrive1);
			//m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
			//m_drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
			//m_drive.setSafetyEnabled(false);
			m_drive = new DifferentialDrive(leftDrive1, rightDrive1);
			//m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearLeft, true);	//TODO URGENT: verify
			//m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true);	//TODO URGENT: verify
			m_drive.setSafetyEnabled(false);
			
			//speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID);
		}
		catch (Exception e) {
			System.err.println("An error occurred in the DriveTrain constructor");
		}
	}

	public void setToBrakeOnNeutral(boolean brakeVsCoast) {
		if (brakeVsCoast) {
			leftDrive1.setNeutralMode(NeutralMode.Brake);
			leftDrive2.setNeutralMode(NeutralMode.Brake);
			rightDrive1.setNeutralMode(NeutralMode.Brake);
			rightDrive2.setNeutralMode(NeutralMode.Brake);
		} else {
			leftDrive1.setNeutralMode(NeutralMode.Coast);
			leftDrive2.setNeutralMode(NeutralMode.Coast);
			rightDrive1.setNeutralMode(NeutralMode.Coast);
			rightDrive2.setNeutralMode(NeutralMode.Coast);
		}
	}
	
	@Override
	public void initDefaultCommand() {
	}
	
	public double getGyroAngleDeg() {
		//return getGyroPigeonAngleDeg();
		return getGyroNavXAngleDeg();
	}
	
	//public double getGyroPigeonAngleDeg() {
	//	gyroPigeon.GetYawPitchRoll(yprPigeon);
	//	return -yprPigeon[0] + gyroOffsetDeg;
	//}
	
	public double getGyroNavXAngleDeg() {
		return gyroNavX.getAngle() + gyroOffsetDeg;
	}

	public void resetGyro() {
		//gyroPigeon.SetYaw(0);
		gyroNavX.zeroYaw();
	}
	
	public void resetEncoders() {
		mpStraightController.resetZeroPosition();
	}
	
	public void calibrateGyro() {
		//gyroPigeon.EnterCalibrationMode(CalibrationMode.Temperature);
	}
	
	public void endGyroCalibration() {
		if (isCalibrating == true) {
			isCalibrating = false;
		}
	}
	
	public void setGyroOffset(double offsetDeg) {
		gyroOffsetDeg = offsetDeg;
	}
	
	//public boolean isHopperSensorRedOn() {
	//	return hopperSensorRed.get();
	//}

	//public boolean isHopperSensorBlueOn() {
	//	return hopperSensorBlue.get();
	//}
	
	//public boolean isHopperSensorOn() {
	//	if (isRed() == true) {
	//		return isHopperSensorRedOn();
	//	}		
	//	else {
	//		return isHopperSensorBlueOn();
	//	}
	//}

	public void setStraightMM(double distanceInches, double maxVelocity, double maxAcceleration, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
		double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
		mmStraightController.setPID(mmStraightPIDParams);
		mmStraightController.setMMStraightTarget(0, distanceInches, maxVelocity, maxAcceleration, useGyroLock, yawAngle, true); 
		setControlMode(DriveControlMode.MOTION_MAGIC);
	}
	
	public void setStraightMP(double distanceInches, double maxVelocity, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
		double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
		mpStraightController.setPID(mpStraightPIDParams);
		mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true); 
		setControlMode(DriveControlMode.MP_STRAIGHT);
	}
	
	//public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
	//	double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
	//	mpStraightController.setPID(mpStraightPIDParams);
	//	mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true); 
	//	setControlMode(DriveControlMode.MP_STRAIGHT);
	//}
	
	public void setRelativeTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) {
		mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES);
		setControlMode(DriveControlMode.MP_TURN);
	}
	
	//public void setRelativeTurnMPCached(String key, MPSoftwareTurnType turnType) {
	//	mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES);
	//	setControlMode(DriveControlMode.MP_TURN);
	//}
	
	public void setRelativeMaxTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) {
		mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_MAX_TURN_T1, MP_MAX_TURN_T2, turnType, TRACK_WIDTH_INCHES);
		setControlMode(DriveControlMode.MP_TURN);
	}
	
	public void setAbsoluteTurnMP(double absoluteTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) {
		mpTurnController.setMPTurnTarget(getGyroAngleDeg(), BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), absoluteTurnAngleDeg), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES);
		setControlMode(DriveControlMode.MP_TURN);
	}
	
	//public void setAbsoluteTurnMPCached(String key, MPSoftwareTurnType turnType) {
	//	mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES);
	//	setControlMode(DriveControlMode.MP_TURN);
	//}
	
	public void setRelativeTurnPID(double relativeTurnAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) {
		this.targetPIDAngle = relativeTurnAngleDeg + getGyroAngleDeg();
		pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType);
		setControlMode(DriveControlMode.PID_TURN);
	}

	public void setPathMP(PathGenerator path) {
		mpPathController.setPID(mpPathPIDParams);
		mpPathController.setMPPathTarget(path); 
		setControlMode(DriveControlMode.MP_PATH);
	}
	
	public void setPathVelocityMP(PathGenerator path) {
		mpPathVelocityController.setPID(mpPathPIDParams);
		mpPathVelocityController.setMPPathTarget(path); 
		setControlMode(DriveControlMode.MP_PATH_VELOCITY);
	}
	
    public void setPathAdaptivePursuit(Path path, boolean reversed) {
    	currentPose = zeroPose;
    	lastPose = zeroPose;
        left_encoder_prev_distance_ = 0;
        right_encoder_prev_distance_ = 0;
        adaptivePursuitController.setPID(adaptivePursuitPIDParams);
    	adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25); 
		setControlMode(DriveControlMode.ADAPTIVE_PURSUIT);
    }

    public void setDriveHold(boolean status) {
		if (status) {
			setControlMode(DriveControlMode.HOLD);
		}
		else {
			setControlMode(DriveControlMode.JOYSTICK);
		}
	}
    
    public void updatePose() {
        double left_distance = leftDrive1.getPositionWorld();
        double right_distance = rightDrive1.getPositionWorld();
        Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg());
        lastPose = currentPose;
        currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle);
        left_encoder_prev_distance_ = left_distance;
        right_encoder_prev_distance_ = right_distance;
    }
    
    public RigidTransform2d generateOdometryFromSensors(double left_encoder_delta_distance, double right_encoder_delta_distance, Rotation2d current_gyro_angle) {
        return Kinematics.integrateForwardKinematics(lastPose, left_encoder_delta_distance, right_encoder_delta_distance, current_gyro_angle);
    }
	
    /**
     * Path Markers are an optional functionality that name the various
     * Waypoints in a Path with a String. This can make defining set locations
     * much easier.
     * 
     * @return Set of Strings with Path Markers that the robot has crossed.
     */
    public synchronized Set<String> getPathMarkersCrossed() {
        if (controlMode != DriveControlMode.ADAPTIVE_PURSUIT) {
            return null;
        } else {
            return adaptivePursuitController.getMarkersCrossed();
        }
    }

    public synchronized void setControlMode(DriveControlMode controlMode) {
 		this.controlMode = controlMode;
		if (controlMode == DriveControlMode.JOYSTICK) {
			//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
			//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
			leftDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
			rightDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
		}
		else if (controlMode == DriveControlMode.MANUAL) {
			//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
			//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
			leftDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
			rightDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
		}
		else if (controlMode == DriveControlMode.CLIMB) {
			//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
			//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
			leftDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
			rightDrive1.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
		}
		else if (controlMode == DriveControlMode.HOLD) {
			mpStraightController.setPID(mpHoldPIDParams);
			//leftDrive1.changeControlMode(TalonControlMode.Position);
			//leftDrive1.setPosition(0);
			//leftDrive1.set(0);
			//rightDrive1.changeControlMode(TalonControlMode.Position);
			//rightDrive1.setPosition(0);
			//rightDrive1.set(0);
			leftDrive1.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
			leftDrive1.set(ControlMode.Position, 0);
			rightDrive1.setSelectedSensorPosition(0, 0, 0);	//TODO: verify want 0="Primary closed-loop", with no timeout
			rightDrive1.set(ControlMode.Position, 0);
		}
		isFinished = false;
	}
	
	public synchronized void controlLoopUpdate() {
		// Measure *actual* update period
		double currentTimestamp = Timer.getFPGATimestamp();
		if (lastControlLoopUpdateTimestamp > 0.001)	{	// ie, if this is NOT the first time
			lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp;
		}
		lastControlLoopUpdateTimestamp = currentTimestamp;
		
		// Do the update
		if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) {
			driveWithJoystick();
		}
		else if (!isFinished) {
			if (controlMode == DriveControlMode.MP_STRAIGHT) {
				isFinished = mpStraightController.controlLoopUpdate(getGyroAngleDeg()); 
			}
			else if (controlMode == DriveControlMode.MP_TURN) {
				isFinished = mpTurnController.controlLoopUpdate(getGyroAngleDeg()); 
			}
			else if (controlMode == DriveControlMode.PID_TURN) {
				isFinished = pidTurnController.controlLoopUpdate(getGyroAngleDeg()); 
			}
			else if (controlMode == DriveControlMode.MP_PATH) {
				isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg()); 
			}
			else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) {
				isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); 
			}
			else if (controlMode == DriveControlMode.MOTION_MAGIC) {
				isFinished = mmStraightController.controlLoopUpdate(getGyroAngleDeg()); 
			}
			else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) {
				updatePose();
				isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); 
			}
		}
	}
	
	public void setSpeed(double speed) {
		if (speed == 0) {
			setControlMode(DriveControlMode.JOYSTICK);
		}
		else {
			setControlMode(DriveControlMode.MANUAL);
			rightDrive1.set(-speed);
			leftDrive1.set(speed);
		}
	}
	
	public void setClimbSpeed(double climbSpeed) {
		this.climbSpeed = climbSpeed;
		if (climbSpeed == 0) {
			setControlMode(DriveControlMode.JOYSTICK);
		}
		else {
			setControlMode(DriveControlMode.CLIMB);
		}
	}
	
	public void setGyroLock(boolean useGyroLock, boolean snapToAbsolute0or180) {
		if (snapToAbsolute0or180) {
			gyroLockAngleDeg = BHRMathUtils.adjustAccumAngleToClosest180(getGyroAngleDeg());
		}
		else {
			gyroLockAngleDeg = getGyroAngleDeg();
		}
		this.useGyroLock = useGyroLock;
	}

	public void driveWithJoystick() {
		if(m_drive == null) return;
		// switch(m_controllerMode) {
		// case CONTROLLER_JOYSTICK_ARCADE:
		// m_moveInput = OI.getInstance().getJoystick1().getY();
		// m_steerInput = OI.getInstance().getJoystick1().getX();
		// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
		// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
		// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
		// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
		// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
		// break;
		// case CONTROLLER_JOYSTICK_TANK:
		// m_moveInput = OI.getInstance().getJoystick1().getY();
		// m_steerInput = OI.getInstance().getJoystick2().getY();
		// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
		// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
		// m_steerOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
		// m_steerInput, m_moveNonLinear, MOVE_NON_LINEARITY);
		// m_drive.tankDrive(m_moveOutput, m_steerOutput);
		// break;
		// case CONTROLLER_JOYSTICK_CHEESY:
		// m_moveInput = OI.getInstance().getJoystick1().getY();
		// m_steerInput = OI.getInstance().getJoystick2().getX();
		// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
		// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
		// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
		// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
		// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
		// break;
		// case CONTROLLER_XBOX_CHEESY:
		// boolean turbo = OI.getInstance().getDriveTrainController()
		// .getLeftJoystickButton();
		// boolean slow = OI.getInstance().getDriveTrainController()
		// .getRightJoystickButton();
		// double speedToUseMove, speedToUseSteer;
		// if (turbo && !slow) {
		// speedToUseMove = m_moveScaleTurbo;
		// speedToUseSteer = m_steerScaleTurbo;
		// } else if (!turbo && slow) {
		// speedToUseMove = m_moveScaleSlow;
		// speedToUseSteer = m_steerScaleSlow;
		// } else {
		// speedToUseMove = m_moveScale;
		// speedToUseSteer = m_steerScale;
		// }

		// m_moveInput =
		// OI.getInstance().getDriveTrainController().getLeftYAxis();
		// m_steerInput =
		// OI.getInstance().getDriveTrainController().getRightXAxis();
		m_moveInput = -OI.getInstance().getDriverController().getLeftYAxis();
		m_steerInput = OI.getInstance().getDriverController().getRightXAxis();

		if (controlMode == DriveControlMode.JOYSTICK) {
			m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
					m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
		}
		else if (controlMode == DriveControlMode.CLIMB) {
			m_moveOutput = climbSpeed;
		}
		
		if (useGyroLock) {											// If currently in gyro lock mode,
			if ((m_moveInput == 0.0) || (m_steerInput != 0.0)) {	// but stopped driving or started turning,
				setGyroLock(false, false);							// turn off gyro lock mode
			}
		} else {													// If not yet in gyro lock mode,
			if ((m_moveInput != 0.0) && (m_steerInput == 0.0)) {	// but just started driving without turning,
				setGyroLock(true, false);							// gyro lock to current heading
			}
		}
		
		if (useGyroLock) {
			double yawError = gyroLockAngleDeg - getGyroAngleDeg();
			m_steerOutput = kPGyro * yawError;
		} else {
			m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
					m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
		}
		
		m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75);
		// break;
		// case CONTROLLER_XBOX_ARCADE_RIGHT:
		// m_moveInput =
		// OI.getInstance().getDrivetrainController().getRightYAxis();
		// m_steerInput =
		// OI.getInstance().getDrivetrainController().getRightXAxis();
		// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
		// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
		// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
		// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
		// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
		// break;
		// case CONTROLLER_XBOX_ARCADE_LEFT:
		// m_moveInput =
		// OI.getInstance().getDrivetrainController().getLeftYAxis();
		// m_steerInput =
		// OI.getInstance().getDrivetrainController().getLeftXAxis();
		// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
		// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
		// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
		// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
		// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
		// break;
		// }
	}

	public void rawMoveSteer(double move, double steer) {
		m_drive.arcadeDrive(move, steer, false);
	}

	public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) {
		leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput);
		rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput);
	}

	private boolean inDeadZone(double input) {
		boolean inDeadZone;
		if (Math.abs(input) < STICK_DEADBAND) {
			inDeadZone = true;
		} else {
			inDeadZone = false;
		}
		return inDeadZone;
	}

	public double adjustForSensitivity(double scale, double trim,
			double steer, int nonLinearFactor, double wheelNonLinearity) {
		if (inDeadZone(steer))
			return 0;

		steer += trim;
		steer *= scale;
		steer = limitValue(steer);

		int iterations = Math.abs(nonLinearFactor);
		for (int i = 0; i < iterations; i++) {
			if (nonLinearFactor > 0) {
				steer = nonlinearStickCalcPositive(steer, wheelNonLinearity);
			} else {
				steer = nonlinearStickCalcNegative(steer, wheelNonLinearity);
			}
		}
		return steer;
	}

	private double limitValue(double value) {
		if (value > 1.0) {
			value = 1.0;
		} else if (value < -1.0) {
			value = -1.0;
		}
		return value;
	}

	private double nonlinearStickCalcPositive(double steer,
			double steerNonLinearity) {
		return Math.sin(Math.PI / 2.0 * steerNonLinearity * steer)
				/ Math.sin(Math.PI / 2.0 * steerNonLinearity);
	}

	private double nonlinearStickCalcNegative(double steer,
			double steerNonLinearity) {
		return Math.asin(steerNonLinearity * steer)
				/ Math.asin(steerNonLinearity);
	}

	//public void setShiftState(SpeedShiftState state) {
	//	if(state == SpeedShiftState.HI) {
	//		speedShift.set(true);
	//	}
	//	else if(state == SpeedShiftState.LO) {
	//		speedShift.set(false);
	//	}
	//}

	public synchronized boolean isFinished() {
		return isFinished;
	}
	
	public synchronized void setFinished(boolean isFinished) {
		this.isFinished = isFinished;
	}
	
	@Override
	public void setPeriodMs(long periodMs) {
		//mmStraightController = new MMTalonPIDController(periodMs, mmStraightPIDParams, motorControllers);
		mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers);
		mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers);
		pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers);
		mpPathController = new MPTalonPIDPathController(periodMs, mpPathPIDParams, motorControllers);
		mpPathVelocityController = new MPTalonPIDPathVelocityController(periodMs, mpPathVelocityPIDParams, motorControllers);
		adaptivePursuitController = new AdaptivePurePursuitController(periodMs, adaptivePursuitPIDParams, motorControllers);

		this.periodMs = periodMs;
	}
	
	public double getPeriodMs() {
		return periodMs;
	}
	
	public boolean isRed() {
		return isRed;
	}
	
	public void setIsRed(boolean status) {
		isRed = status;
	}
	
	public static double rotationsToInches(double rotations) {
        return rotations * (Constants.kDriveWheelDiameterInches * Math.PI);
    }

    public static double rpmToInchesPerSecond(double rpm) {
        return rotationsToInches(rpm) / 60;
    }

    public static double inchesToRotations(double inches) {
        return inches / (Constants.kDriveWheelDiameterInches * Math.PI);
    }

    public static double inchesPerSecondToRpm(double inches_per_second) {
        return inchesToRotations(inches_per_second) * 60;
    }

    public double getLeftPositionWorld() {
    	return leftDrive1.getPositionWorld();
    }

    public double getRightPositionWorld() {
    	return rightDrive1.getPositionWorld();
    }

	public void updateStatus(Robot.OperationMode operationMode) {
		if (operationMode == Robot.OperationMode.TEST) {
			try {
				SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0);
				SmartDashboard.putNumber("Right Pos Ticks", rightDrive1.getSelectedSensorPosition(0));
				SmartDashboard.putNumber("Left Pos Ticks", leftDrive1.getSelectedSensorPosition(0));
				SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld());
				SmartDashboard.putNumber("Left Pos Inches", leftDrive1.getPositionWorld());
				SmartDashboard.putNumber("Right Vel Ft-Sec", rightDrive1.getVelocityWorld() / 12);
				SmartDashboard.putNumber("Left Vel Ft-Sec", leftDrive1.getVelocityWorld() / 12);
				//SmartDashboard.putNumber("Left 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID));
				//SmartDashboard.putNumber("Left 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID));
//				SmartDashboard.putNumber("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID));
				//SmartDashboard.putNumber("Right 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID));
				//SmartDashboard.putNumber("Right 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID));
//				SmartDashboard.putNumber("Right 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID));
				//SmartDashboard.putBoolean("Hopper Sensor Red", isHopperSensorRedOn());
				//SmartDashboard.putBoolean("Hopper Sensor Blue", isHopperSensorBlueOn());
				SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD);
				//SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg());
				SmartDashboard.putNumber("Yaw Angle Deg", getGyroAngleDeg());
				MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint(); 
				double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0;
				SmartDashboard.putNumber("Gyro Delta", delta);
				//SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating);
				SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating());
				SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg);
				SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg());
				SmartDashboard.putNumber("Steer Output", m_steerOutput);
				SmartDashboard.putNumber("Move Output", m_moveOutput);
				SmartDashboard.putNumber("Steer Input", m_steerInput);
				SmartDashboard.putNumber("Move Input", m_moveInput);
			}
			catch (Exception e) {
				System.err.println("Drivetrain update status error");
			}
		}
		else {
			
		}
	}	
	
}