fixed drive distance command

Author: colen

TorontoRobot/src/robot/RobotConst.java

@@ -60,8 +60,8 @@ public class RobotConst {
 			MAX_LOW_GEAR_SPEED = 365.0;    // Encoder counts/sec
 			MAX_HIGH_GEAR_SPEED = 830.0;
 
-			DRIVE_GYRO_PID_KP = .05;
-			DRIVE_GYRO_PID_KI = 0.01;
+			DRIVE_GYRO_PID_KP = 0.05;
+			DRIVE_GYRO_PID_KI = 0.001;
 
 			DRIVE_SPEED_PID_KP = 0.75;
 			ENCODER_COUNTS_PER_INCH = 51.5;

TorontoRobot/src/robot/commands/AutonomousCommand.java

@@ -35,5 +35,6 @@ public AutonomousCommand() {
 
     	addSequential(new ArcCommand(100, 0, 310, 1.0));
     	addSequential(new ArcCommand(140, 310, 350, 1.0));
+    	//addSequential(new DriveDistanceCommand(140, 310, 350, 1.0));
     }
 }

TorontoRobot/src/robot/commands/drive/DefaultChassisCommand.java

@@ -46,7 +46,7 @@ protected void execute() {
 
 
 		if (Robot.oi.getForwardThrust()) {
-			Scheduler.getInstance().add(new DriveTimeCommand(0));
+			Scheduler.getInstance().add(new DriveDistanceCommand(50, Robot.chassisSubsystem.getGryoAngle(), 0.5, 5.0, true));
 		}
 
 		if (Robot.oi.getStartDriveDirection()) {

TorontoRobot/src/robot/commands/drive/DriveDistanceCommand.java

@@ -11,7 +11,7 @@ public DriveDistanceCommand(double distance, double direction, double speed, dou
 		this.distance = distance;
 	}
 	protected void initialize() {
-
+		//System.out.println("starting drive");
 		Robot.chassisSubsystem.resetEncoders();
 
 	}
@@ -20,7 +20,8 @@ protected boolean isFinished() {
 		if (super.isFinished()) {
 			return true;
 		}
-		if (distance * RobotConst.ENCODER_COUNTS_PER_INCH >= Robot.chassisSubsystem.getEncoderDistance() + 20){
+		if (distance * RobotConst.ENCODER_COUNTS_PER_INCH < Robot.chassisSubsystem.getEncoderDistance() + 20){
+			//System.out.println("ending drive");
 			return true;
 		}
 		return false;