 
package ca.team4519.frc2014.actions;

import ca.team4519.frc2014.Constants;


public class DriveDistance extends Action {

    
    double distance;
    boolean doStop;
    double heading;
    double newHeading;
    double currentHeading;
    double currentDistance;
    boolean targetHeading;
    
  public DriveDistance(double distance, double heading){
        
        this.distance = distance;
        this.heading = heading;
        
     
    }
   

  
    public boolean execute() {
        return false;
    }

    public void init() {
         driveBase.resetEncoders();
         driveBase.resetGyro();
         
         currentHeading = driveBase.getGyroAngle();
         currentDistance = driveBase.getAverageDistance();
         
         
     if(currentHeading == heading || currentHeading < heading + 5 || currentHeading > heading - 5 ){
         driveBase.setLeftRightPower(0, 0);
         targetHeading =true;
       }else if(currentHeading != heading && currentHeading < heading){
         driveBase.setLeftRightPower(0.5, 0.5);
         targetHeading = false;
       }else if (currentHeading != heading && currentHeading > heading){
          driveBase.setLeftRightPower(-0.5, -0.5);
          targetHeading = false;
     }
 
     if(currentDistance == distance || currentDistance < distance + (1/12) || currentDistance > distance + (1/12)){
         driveBase.setLeftRightPower(0, 0);
     }else if((currentDistance != distance) && targetHeading){
         driveBase.setLeftRightPower(1, 1);
     }
     
    }

    public void done() {
        driveBase.setLeftRightPower(0, 0);
    }
}

@@ -1,10 +1,8 @@
- %0A
 package 
@@ -35,48 +35,8 @@
 s;%0A%0A
-import ca.team4519.frc2014.Constants;%0A%0A%0A
 publ
@@ -75,17 +75,16 @@
 ction %7B%0A
-%0A
     %0A   
@@ -391,30 +391,12 @@
     
+%7D%0A
   
-  %0A     %0A    %7D%0A   %0A%0A
   %0A 
@@ -597,15 +597,13 @@
 ase.
-getGyro
+smart
 Angl
@@ -975,16 +975,17 @@
 er(0.5, 
+-
 0.5);%0A  
@@ -1127,25 +1127,24 @@
 Power(-0.5, 
--
 0.5);%0A      
@@ -1178,16 +1178,46 @@
      %7D%0A 
+    driveBase.resetEncoders();
 %0A     if

reset encoders after reaching heading