Drive.java

// RobotBuilder Version: 1.5
//
// 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.frc5122.Fred2.subsystems;

import org.usfirst.frc5122.Fred2.RobotMap;
import org.usfirst.frc5122.Fred2.U;
import org.usfirst.frc5122.Fred2.commands.oi_HDrive;

import com.kauailabs.navx_mxp.AHRS;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;


/**
 *
 */
public class Drive extends Subsystem {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    SpeedController leftFront = RobotMap.driveLeftFront;
    SpeedController leftRear = RobotMap.driveLeftRear;
    SpeedController rightFront = RobotMap.driveRightFront;
    SpeedController rightRear = RobotMap.driveRightRear;
    RobotDrive drivetrain = RobotMap.driveDrivetrain;
    SpeedController strafe = RobotMap.driveStrafe;
    Gyro gyro = RobotMap.drivegyro;
    Encoder leftEncoder = RobotMap.driveLeftEncoder;
    Encoder rightEncoder = RobotMap.driveRightEncoder;

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    public Encoder leftEnc = RobotMap.driveLeftEncoder;
    public Encoder rightEnc = RobotMap.driveRightEncoder;
    
    public Gyro gyro1 = gyro;
    SerialPort serial_port;
    public AHRS imu;			// This class can only be used w/the navX MXP.
    private static double voltsPerDegreePerSecond = 0.0065;
    
    public Drive() {
    	gyro.setSensitivity(voltsPerDegreePerSecond);
    	SmartDashboard.putNumber("straight kp", .03);
    	
    	try {
        	
        	// Use SerialPort.Port.kOnboard if connecting nav6 to Roborio Rs-232 port
        	// Use SerialPort.Port.kMXP if connecting navX MXP to the RoboRio MXP port
        	// Use SerialPort.Port.kUSB if connecting nav6 or navX MXP to the RoboRio USB port
        		
        	serial_port = new SerialPort(57600, SerialPort.Port.kUSB);
        	System.out.println("Serial: "+(serial_port==null));
        	
        	// You can add a second parameter to modify the 
    		// update rate (in hz) from.  The minimum is 4.  
        	// The maximum (and the default) is 100 on a nav6, 60 on a navX MXP.
    		// If you need to minimize CPU load, you can set it to a
    		// lower value, as shown here, depending upon your needs.
        	// The recommended maximum update rate is 50Hz
    		
    		// You can also use the IMUAdvanced class for advanced
    		// features on a nav6 or a navX MXP.
        	
        	// You can also use the AHRS class for advanced features on 
        	// a navX MXP.  This offers superior performance to the
        	// IMU Advanced class, and also access to 9-axis headings
        	// and magnetic disturbance detection.  This class also offers
        	// access to altitude/barometric pressure data from a
        	// navX MXP Aero.
    		
    		byte update_rate_hz = 60;
    		//imu = new IMU(serial_port,update_rate_hz);
    		//imu = new IMUAdvanced(serial_port,update_rate_hz);
    		imu = new AHRS(serial_port,update_rate_hz);
    		System.out.println("Checking IMU! " + (imu==null));
    		
    	} catch( Exception ex ) {
    		System.err.println("Constructor "+ex.toString());
    		//throw ex;
    	}
        if ( imu != null ) {
        	System.out.println("IMU not NULL");
            LiveWindow.addSensor("IMU", "Gyro", imu);
        } else {
        	System.out.println("IMU IS NULL");
        }
    }

    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    public void initDefaultCommand() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
        setDefaultCommand(new oi_HDrive());

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
	
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    public void HDrive(double moveValue, double turnValue, double strafeValue) {
    	drivetrain.arcadeDrive(-moveValue, -turnValue);
    	strafe.set(strafeValue);
    }
    public void stop(){
    	drivetrain.arcadeDrive(0, 0);
    	strafe.set(0);
    }
    private double deadzone(double input, double deadzone) {
    	if (Math.abs(input) < deadzone) {
    		return (0);
    	} else {
    		return (input);
    	}
    }
    private double deadzone(double input) {
    	return deadzone(input, .2);
    }
    
    //when the robot resets clear the gyro error
    public void resetGyro() {
    	System.out.println("Resetting Gyro");
    	gyro.reset();
    	try {
    		imu.zeroYaw();
    	} catch (Exception ex) {
    		System.err.println("ResetGyro "+ex.toString());
    	}
    	prevTurn = 0;
    	targetHeading = 0;
    	error = 0;
    }
    
    private double prevTurn = 0;
    private double targetHeading = 0;
    private double error = 0;
    private final double Kp = 0.03; //.05
    //Hdrive with strafe gyro compensation
    public void hDrive(double drive, double turn, double slide){
    	HDrive(drive, turn, slide);
    	return;
//    	drive = deadzone(drive);
//    	turn = deadzone(turn);
//    	slide = deadzone(slide);
//    	// if any input is given, snap out of Move state
////    	if (drive != 0 || turn != 0 || slide != 0) {
////    		moveStateFB = STATE_IDLE;
////    	}
//    	// Heading hold
//    	double heading = gyro.getAngle();
//
//
//    	if (turn == 0 ) {
//    		
//    		if (prevTurn != 0) {
//    			targetHeading = heading;
//    			prevTurn = 0;
//    		}
//    		
//    		
//    		// Calculate PID
//    		error = targetHeading - heading;
//    		SmartDashboard.putNumber("error", error);
//    		if (error > 180) {
//    			error = heading - (360 - targetHeading);
//    		}
//    		
//
//    		turn = error * Kp;
//    		if (turn > 1.0) {
//    			turn = 1.0;
//    		} else if (turn < -1.0) {
//    			
//    			turn = -1.0;
//    		}
//
//    	// Turn rate was given, don't hold heading
//    	} else {
//    		prevTurn = turn;
//    	}
//    	// Drive
//    	HDrive(drive, turn, slide);
    }
    
    private double ds_e = 0;
    double ds_kp = 0.03;
    public void driveStraight(double speed) {
    	ds_kp = SmartDashboard.getNumber("straight kp");
    	double angle = imu.getYaw(); // get current heading
    	double output = -angle*Kp;
    	
    	output = U.constrain(-1, output, 1);
    	if (Math.abs(output) < .4 && output != 0) {
    		output = (output/Math.abs(output))*.4;
    	}
        HDrive(speed, output, 0.0);
//    	getGyroAngle()
    }
    
    public double leftDistance() {
    	return leftEncoder.getDistance();
    }
    public double rightDistance() {
    	return rightEncoder.getDistance();
    }
    
    public void resetLeftDistance() {
    	leftEncoder.reset();
    }
    public void ResetRightDistance() {
    	rightEncoder.reset();
    }
    
    //we'll backup the NavX with the analog Gyro
    public double getGyroAngle() {
//    	System.out.println("Get Gyro Angle");
    	try {
    		if (imu.isConnected() && !imu.isCalibrating())
    		{
    			return imu.getYaw();
    		}
    	} catch (Exception ex) {
    		System.err.println("GetGyroAngle "+ex.toString());
    		return -1;
    	}
    	return gyro.getAngle();
    }
    public double getOldGyroAngle() {
    	return 0; //gyro.getAngle();
    }
    
    public double getDegreesFromEncoderValues()
    {
    	final double trackwidth = 24;

    	double robotArcLength = (leftDistance()-rightDistance())/2;
    	double robotDegrees = 360*(robotArcLength/(Math.PI*trackwidth));

    	return robotDegrees;
	}
}