package org.robockets.stronghold.robot.subsystems;

import org.robockets.stronghold.robot.RobotMap;

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

/**
 *
 */
public class Drivetrain extends Subsystem {

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    public void driveTank(double leftValue, double rightValue) {
    	RobotMap.robotDrive.tankDrive(leftValue, rightValue);
    }
    
    public void driveArcade(double moveValue, double rotateValue) {
    	RobotMap.robotDrive.arcadeDrive(moveValue, rotateValue);
    }
    
    public void stop() {
    	driveTank(0, 0);
    }
}


@@ -87,606 +87,1750 @@
 bot.
-RobotMap;%0A%0Aimport edu.wpi.first.wpilibj.command.Subsystem;%0A%0A/**%0A *%0A */%0Apublic class Drivetrain extends Subsystem %7B%0A%0A    public void initDefaultCommand() %7B%0A        // Set the default command for a subsystem here.%0A        //setDefaultCommand(new MySpecialCommand());%0A    %7D%0A    %0A    public void driveTank(double leftValue, double rightValue) %7B%0A    %09RobotMap.robotDrive.tankDrive(leftValue, rightValue);%0A    %7D%0A    %0A    public void driveArcade(double moveValue, double rotateValue) %7B%0A    %09RobotMap.robotDrive.arcadeDrive(moveValue, rotateValue);%0A    %7D%0A    %0A    public void stop() %7B%0A    %09driveTank(0, 0);
+CompassPIDSource;%0Aimport org.robockets.stronghold.robot.RobotMap;%0Aimport org.robockets.stronghold.robot.drivetrain.DrivePIDOutput;%0A%0Aimport edu.wpi.first.wpilibj.PIDController;%0Aimport edu.wpi.first.wpilibj.command.Subsystem;%0A%0A/**%0A *%0A */%0Apublic class Drivetrain extends Subsystem %7B%0A%09private final PIDController compassPID = new PIDController(0.1, 0, 0, new CompassPIDSource(), new DrivePIDOutput());%0A%0A%09public Drivetrain() %7B%0A%09%09compassPID.disable();%0A%09%7D%0A%09%0A    public void initDefaultCommand() %7B%0A        // Set the default command for a subsystem here.%0A        //setDefaultCommand(new MySpecialCommand());%0A    %7D%0A    %0A    public void driveTank(double leftValue, double rightValue) %7B%0A    %09RobotMap.robotDrive.tankDrive(leftValue, rightValue);%0A    %7D%0A    %0A    public void driveArcade(double moveValue, double rotateValue) %7B%0A    %09RobotMap.robotDrive.arcadeDrive(moveValue, rotateValue);%0A    %7D%0A    %0A    /*%0A     * Drive with PID. First must enable the correct PID%0A     */%0A    public void driveAssisted(double moveValue, double angle, boolean compassAssist) %7B%0A    %09if (compassAssist) %7B // Use compass for PID%0A    %09%09driveArcade(moveValue, compassPID.get());%0A    %09%7D else %7B%0A    %09%09// TODO Implement gyro PID logic%0A    %09%7D%0A    %7D%0A    %0A    public void setAngle(double angle, boolean compassAssist) %7B%0A    %09if (compassAssist) %7B%0A    %09%09compassPID.setSetpoint(angle);%0A    %09%7D else %7B%0A    %09%09// TODO Implement gyro PID logic%0A    %09%7D%0A    %7D%0A    %0A    public void stop() %7B%0A    %09driveTank(0, 0);%0A    %7D%0A    %0A    public void enableCompassPID() %7B%0A    %09// TODO Once gyro PID is implemented, first disable gyro PID%0A    %09%0A    %09compassPID.enable();%0A    %09compassPID.reset();%0A    %09compassPID.setSetpoint(RobotMap.navX.getCompassHeading()); // Make sure setpoint starts as current position
 %0A   

Implement basic compass PID skeleton