package edu.stuy.commands.auton;

import static edu.stuy.PhysicalConstants.TOTE_HEIGHT;
import static edu.stuy.PhysicalConstants.TOTE_WIDTH;
import static edu.stuy.RobotMap.AUTON_DRIVETRAIN_TOTE_WIDTH_TIMEOUT;
import static edu.stuy.RobotMap.AUTON_DRIVE_FORWARD_DRIVER_SIDE_INCHES;
import static edu.stuy.RobotMap.AUTON_DRIVE_FORWARD_DRIVER_SIDE_TIMEOUT;
import static edu.stuy.RobotMap.AUTON_TOTE_COLLISION_AVOIDANCE_OFFSET;
import edu.stuy.commands.DrivetrainRotateNoPIDCommand;
import edu.stuy.commands.LiftDownInchesCommand;
import edu.stuy.commands.LiftUpInchesCommand;
import edu.wpi.first.wpilibj.command.CommandGroup;

/**
 * The robot starts collinear with the auton set, with the hook in the can.
 * 1) Lift up the can.
 * 2) Drive forward and acquire the tote.
 * 3) Turn 90° to the left.
 * 4) Drive forward to the auton zone.
 * 5) Drop the can onto the tote.
 */
public class AutonLiftContainerHerdTote extends CommandGroup {

    public  AutonLiftContainerHerdTote() {
        addSequential(new LiftUpInchesCommand(TOTE_HEIGHT + AUTON_TOTE_COLLISION_AVOIDANCE_OFFSET));
        addParallel(new AutonAcquirerAcquireCommand());
        addSequential(new AutonDriveForwardInchesCommand(TOTE_WIDTH , AUTON_DRIVETRAIN_TOTE_WIDTH_TIMEOUT));
        addSequential(new AutonAcquirerOffCommand());
        // Tote is on the right from the driver's point of view, so we turn 90 degrees
        addSequential(new DrivetrainRotateNoPIDCommand(-90) , 2.0);
        // Go to auton zone
        addSequential(new AutonDriveForwardInchesCommand(AUTON_DRIVE_FORWARD_DRIVER_SIDE_INCHES, AUTON_DRIVE_FORWARD_DRIVER_SIDE_TIMEOUT));
    }
}

@@ -419,16 +419,63 @@
 OFFSET;%0A
+import edu.stuy.commands.ArmsSetNarrowCommand;%0A
 import e
@@ -1170,35 +1170,40 @@
 AcquireCommand()
+, 5.0
 );%0A
-
         addSeque
@@ -1291,24 +1291,75 @@
 _TIMEOUT));%0A
+        addSequential(new ArmsSetNarrowCommand());%0A
         addS
@@ -1486,16 +1486,28 @@
  degrees
+ to the left
 %0A       
@@ -1562,14 +1562,8 @@
 -90)
- , 2.0
 );%0A 

Fix logic in AutonLiftContainerHerdTote