/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.stuy.subsystems;

import edu.stuy.Devmode;
import edu.stuy.RobotMap;
import edu.stuy.commands.DriveManualJoystickControl;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
 *
 * @author Kevin Wang
 */
public class Drivetrain extends Subsystem {

    RobotDrive drive;
    Solenoid gearShift;
    AnalogChannel sonar;
    Encoder encoderLeft;
    Encoder encoderRight;
    Gyro gyro;
    final int WHEEL_RADIUS                 = 3; 
    final double CIRCUMFERENCE             = 2 * Math.PI * WHEEL_RADIUS;
    final int ENCODER_CODES_PER_REV        = 360;
    final double DISTANCE_PER_PULSE        = CIRCUMFERENCE / ENCODER_CODES_PER_REV;
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    public Drivetrain() {
        drive = new RobotDrive(RobotMap.FRONT_LEFT_MOTOR, RobotMap.REAR_LEFT_MOTOR, RobotMap.FRONT_RIGHT_MOTOR, RobotMap.REAR_RIGHT_MOTOR);

        encoderLeft = new Encoder(RobotMap.ENCODER_CHANNEL_1A, RobotMap.ENCODER_CHANNEL_1B, true);
        encoderRight = new Encoder(RobotMap.ENCODER_CHANNEL_2A, RobotMap.ENCODER_CHANNEL_2B, true);

        encoderLeft.setDistancePerPulse(DISTANCE_PER_PULSE);
        encoderRight.setDistancePerPulse(DISTANCE_PER_PULSE);

        gyro = new Gyro(RobotMap.GYRO_CHANNEL);

        if (!Devmode.DEV_MODE) {
            gearShift = new Solenoid(RobotMap.GEAR_SHIFT);
        }

        sonar = new AnalogChannel(RobotMap.SONAR_CHANNEL);
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new DriveManualJoystickControl());
    }

    public void tankDrive(double leftValue, double rightValue) {
        drive.tankDrive(leftValue, rightValue);
    }

    public void setGear(boolean high) {
        gearShift.set(high);
    }
}

@@ -344,16 +344,56 @@
 j.Gyro;%0A
+import edu.wpi.first.wpilibj.PIDOutput;%0A
 import e
@@ -425,16 +425,16 @@
 tDrive;%0A
-
 import e
@@ -511,16 +511,83 @@
 bsystem;
+%0Aimport edu.wpi.first.wpilibj.smartdashboard.SendablePIDController;
 %0A%0A/**%0A *
@@ -795,16 +795,60 @@
 o gyro;%0A
+    %0A    SendablePIDController controller;%0A%0A
     fina
@@ -1096,24 +1096,97 @@
 R_REV;%0A    %0A
+    double Kp = 0.035;%0A    double Ki = 0.0005;%0A    double Kd = 1.0;%0A    %0A
     // Put m
@@ -1800,16 +1800,268 @@
 HANNEL);
+%0A        gyro.setSensitivity(0.007);%0A        controller = new SendablePIDController(Kp, Ki, Kd, gyro, new PIDOutput() %7B%0A%0A            public void pidWrite(double output) %7B%0A                drive.arcadeDrive(-1, -output);%0A            %7D%0A%0A        %7D, 0.005);
 %0A%0A      
@@ -2596,24 +2596,24 @@
 ean high) %7B%0A
-
         gear
@@ -2635,10 +2635,76 @@
 ;%0A    %7D%0A
+    %0A    public void driveStraight(int distance) %7B%0A        %0A    %7D%0A
 %7D%0A

Add PID drive straight controller to drivetrain subsystem