package com.team254.frc2015.auto.modes;

import com.team254.frc2015.Constants;
import com.team254.frc2015.auto.AutoMode;
import com.team254.frc2015.auto.AutoModeEndedException;

/**
 * Created by tombot on 2/28/15.
 */
public class ThreeBinNoCanAutoMode extends AutoMode {

    @Override
    protected void routine() throws AutoModeEndedException {
        intake.open();


        top_carriage.setPositionSetpoint(Constants.kTopCarriageMaxPositionInches, true);
        drive.setDistanceSetpoint(70, 30);
        waitTime(.7);
        bottom_carriage.setPositionSetpoint(18, true);

        waitForDrive(5.0);

        // Intake 2nd tote
        intake.close();
        intake.setSpeed(1.0);
        waitTime(.5);
        intake.setSpeed(0);

        // Index 2nd tote
        bottom_carriage.setPositionSetpoint(1, false);
        waitForCarriage(bottom_carriage, 2.0);
        intake.open();
        bottom_carriage.setPositionSetpoint(24, true);

        intake.setSpeed(.6);

        // Knock can
        drive.setDistanceSetpoint(115, 30);
        waitTime(1.1);

        intake.close();
        intake.setSpeed(0);
        waitForDrive(2.0);

        drive.setTurnSetPoint(Math.PI/3.0);
        waitTime(.5);
        intake.open();

        waitForDrive(.25);


        drive.setTurnSetPoint(0);
        waitForDrive(1.5);

        drive.reset();
        drive.setDistanceSetpoint(40);
        bottom_carriage.setPositionSetpoint(18, true);
        intake.open();


        waitForDrive(2.0);
        intake.setSpeed(1.0);
        intake.close();
        drive.setTurnSetPoint(Math.PI / 3.0);

        waitForDrive(1.2);

        drive.reset();
        drive.setDistanceSetpoint(137);
        intake.close();
        intake.setSpeed(0.0);
        // Index 3rd tote
        bottom_carriage.setPositionSetpoint(1, false);
        waitForCarriage(bottom_carriage, 2.0);


        bottom_carriage.setPositionSetpoint(10, true);

        waitForDrive(3.0);

        drive.setTurnSetPoint(0);
        bottom_carriage.setPositionSetpoint(1, true);
        waitForCarriage(bottom_carriage, 2.0);
        waitForDrive(1.0);

        bottom_carriage.setFlapperOpen(true);

        drive.reset();
        drive.setDistanceSetpoint(-60);
        intake.open();


        waitForDrive(3.0);
        bottom_carriage.setFlapperOpen(false);


    }
}

@@ -358,32 +358,93 @@
 intake.open();%0A%0A
+        // Get first can out of way, drive slowly to 2nd tote
 %0A        top_car
@@ -629,33 +629,32 @@
 oint(18, true);%0A
-%0A
         waitForD
@@ -996,32 +996,53 @@
 int(24, true);%0A%0A
+        // Knock can%0A
         intake.s
@@ -1053,38 +1053,16 @@
 eed(.6);
-%0A%0A        // Knock can
 %0A       
@@ -1118,24 +1118,55 @@
 Time(1.1);%0A%0A
+        // Close intake on can%0A
         inta
@@ -1229,24 +1229,65 @@
 rive(2.0);%0A%0A
+        // Turn with can intake and open%0A
         driv
@@ -1355,33 +1355,32 @@
  intake.open();%0A
-%0A
         waitForD
@@ -1387,24 +1387,59 @@
 rive(.25);%0A%0A
+        // Turn back to center line
 %0A        dri
@@ -1485,24 +1485,54 @@
 rive(1.5);%0A%0A
+        // Drive to last tote%0A
         driv
@@ -1652,32 +1652,56 @@
 intake.open();%0A%0A
+        // Grab 3rd tote
 %0A        waitFor
@@ -1757,32 +1757,67 @@
  intake.close();
+%0A%0A        // Turn to face auto zone
 %0A        drive.s
@@ -1843,25 +1843,24 @@
 .PI / 3.0);%0A
-%0A
         wait
@@ -1871,24 +1871,54 @@
 rive(1.2);%0A%0A
+        // Drive to auto zone%0A
         driv
@@ -2018,24 +2018,25 @@
 Speed(0.0);%0A
+%0A
         // I
@@ -2040,32 +2040,46 @@
 / Index 3rd tote
+ while driving
 %0A        bottom_
@@ -2165,18 +2165,16 @@
 , 2.0);%0A
-%0A%0A
         
@@ -2212,33 +2212,32 @@
 oint(10, true);%0A
-%0A
         waitForD
@@ -2244,24 +2244,59 @@
 rive(3.0);%0A%0A
+        // Turn back to horizontal%0A
         driv
@@ -2442,24 +2442,66 @@
 rive(1.0);%0A%0A
+        // Let go of stack and drive away%0A
         bott
@@ -2530,25 +2530,24 @@
 Open(true);%0A
-%0A
         driv

Comment auto mode a bit