This is the code as updated at our last meeting before our regional. We'd really like to get an idea of how well it'd work before we get there:
package edu.wpi.first.ReboundRumble.subsystems;
mport edu.wpi.first.ReboundRumble.OI;
import edu.wpi.first.ReboundRumble.RobotMap;
import edu.wpi.first.ReboundRumble.commands.DriveWithJoys ticks;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.ReboundRumble.commands.CommandBase;
/**
*
* @author Developer
*/
public class DriveSystem extends PIDSubsystem {
private static final double Kp = 3;
private static final double Ki = 0.2;
private static final double Kd = 0.1;
RobotDrive drive= new RobotDrive(RobotMap.LeftDrive, RobotMap.RightDrive);
Gyro gyro = new Gyro(RobotMap.gyroPort);
// Initialize your subsystem here
public DriveSystem() {super("DriveSystem", Kp, Ki, Kd);
setSetpoint(0);
// Use these to get going:
// setSetpoint() - Sets where the PID controller should move the system
// to
// enable() - Enables the PID controller.
}
public void initDefaultCommand() {// Set the default command for a subsystem here.
//setDefaultCommand(new MySpecialCommand());
setDefaultCommand(new DriveWithJoysticks());
}
protected double returnPIDInput() { // Return your input value for the PID loop
// e.g. a sensor, like a potentiometer:
// yourPot.getAverageVoltage() / kYourMaxVoltage;
return gyro.getAngle();
}
protected void usePIDOutput(double output) { // Use output to drive your system, like a motor
// e.g. yourMotor.set(output);
if (gyro.getAngle() > 1) {
DriveReg(0, output);
}
if (gyro.getAngle() < -1){
DriveReg(output, 0);
}
drive.tankDrive(.5,.5);
}