finish MoveDistance command with acceleration function

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
index 88827f985666456683e4ac627c3a19ae8927de38..2243bc7a88e1912eebcbfb646033f4a341956988 100644 (file)
--- a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
@@ -1,43 +1,48 @@
 package org.usfirst.frc3501.RiceCatRobot.commands;
 
 import org.usfirst.frc3501.RiceCatRobot.Robot;
+import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain;
 
 import edu.wpi.first.wpilibj.command.Command;
 
 public class MoveDistance extends Command {
 
-       double distance;
-       
-       public MoveDistance(double distance){
+       double distance, minSpeed, maxSpeed;
+
+       public MoveDistance(double distance, double minSpeed, double maxSpeed) {
                requires(Robot.driveTrain);
                this.distance = distance;
+               this.minSpeed = minSpeed;
+               this.maxSpeed = maxSpeed;
        }
-       
-       protected void initialize(){
-               
+
+       protected void initialize() {
+               Robot.driveTrain.resetEncoders();
        }
 
        @Override
        protected void execute() {
-               // TODO Auto-generated method stub
-               
+               double speed = 4 * (minSpeed - maxSpeed) * 
+                               Math.pow((Robot.driveTrain.getAverageSpeed() / distance - 0.5), 2) 
+                               + maxSpeed;
+               Robot.driveTrain.setMotorSpeeds(speed, speed);
        }
 
        @Override
        protected boolean isFinished() {
-               // TODO Auto-generated method stub
+               if (Robot.driveTrain.getLeftDistance() > distance
+                               && Robot.driveTrain.getRightDistance() > distance)
+                       return true;
                return false;
        }
-       
+
        @Override
        protected void end() {
-               // TODO Auto-generated method stub
-               
+               Robot.driveTrain.stop();
        }
 
        @Override
        protected void interrupted() {
-               // TODO Auto-generated method stub
-               
+               end();
        }
 }

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java
index 1dfe9d8759989964c41b5ec4fb0c059702130821..a8716b6ba9f472edf2e0ddd3d2bc3090e8fab7d6 100644 (file)
--- a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java
@@ -37,6 +37,10 @@ public class DriveTrain extends Subsystem {
     public double getLeftSpeed() {
         return leftEncoder.getRate();
     }
+    
+    public double getAverageSpeed() {
+       return (getRightSpeed() + getLeftSpeed())/2;
+    }
 
     public double getRightDistance() {
         // Returns distance in inches
@@ -48,6 +52,10 @@ public class DriveTrain extends Subsystem {
         return leftEncoder.getDistance();
     }
 
+    public void stop() {
+       setMotorSpeeds(0, 0);
+    }
+    
     public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
         if (Math.abs(leftSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
             leftSpeed = 0;