package com.edinarobotics.zephyr.autonomous;

import com.edinarobotics.utils.autonomous.*;
import com.edinarobotics.zephyr.Zephyr;

/**
 *
 */
public class AutonomousStepFactory {
    private Zephyr robot;
    
    public AutonomousStepFactory(Zephyr robot){
        this.robot = robot;
    }
    
    public AutonomousStep getShooterFireStep(double shooterSpeed, int numShots){
        final double SHOOTER_WARMUP_DELAY = 3.5;
        final double PISTON_SET_DELAY = 1;
        final double BALL_LOAD_DELAY = 2.5;
        //Steps to bring the piston up and down
        AutonomousStep[] fireSteps = new AutonomousStep[7];
        fireSteps[0] = new ShooterPistonControlStep(true, robot);
        fireSteps[1] = new IdleWaitStep(PISTON_SET_DELAY, robot);
        fireSteps[2] = new ShooterPistonControlStep(false, robot);
        fireSteps[3] = new IdleWaitStep(PISTON_SET_DELAY, robot);
        fireSteps[4] = new SetConveyorStep(true, robot);
        fireSteps[5] = new IdleWaitStep(BALL_LOAD_DELAY, robot);
        fireSteps[6] = new SetConveyorStep(false, robot);
        
        AutonomousStep[] steps = new AutonomousStep[4];
        //Start the shooter
        steps[0] = new ShooterVoltagePWMStep(shooterSpeed, robot);
        //Wait for the shooter to warm up
        steps[1] = new IdleWaitStep(SHOOTER_WARMUP_DELAY, robot);
        //Fire the piston several times
        steps[2] = new ForLoopStep(new SequentialAutonomousStepGroup(fireSteps), numShots);
        //Stop the shooter
        steps[3] = new SetShooterSpeedStep(0, robot);
        
        return new SequentialAutonomousStepGroup(steps);
    }
}

@@ -420,11 +420,9 @@
 Y = 
-3.5
+4
 ;%0A  
@@ -1167,29 +1167,25 @@
  steps%5B0%5D = 
-new S
+s
 hooterVoltag
@@ -1189,11 +1189,16 @@
 tage
-PWM
+Estimate
 Step
@@ -1212,21 +1212,36 @@
 rSpeed, 
-robot
+SHOOTER_WARMUP_DELAY
 );%0A     
@@ -1557,32 +1557,32 @@
 obot);%0A        %0A
-
         return n
@@ -1628,10 +1628,597 @@
 ;%0A    %7D%0A
+    %0A    public AutonomousStep shooterVoltageEstimateStep(double targetVelocity, double warmupTime)%7B%0A        final double ITER_1_FRACTION = 0.75;%0A        AutonomousStep%5B%5D warmupSteps  = new AutonomousStep%5B4%5D;%0A        warmupSteps%5B0%5D = new ShooterVoltagePWMStep(targetVelocity, robot);%0A        warmupSteps%5B1%5D = new IdleWaitStep(warmupTime*ITER_1_FRACTION, robot);%0A        warmupSteps%5B2%5D = new ShooterVoltagePWMStep(targetVelocity, robot);%0A        warmupSteps%5B3%5D = new IdleWaitStep(warmupTime*(1-ITER_1_FRACTION),robot);%0A        return new SequentialAutonomousStepGroup(warmupSteps);%0A    %7D%0A
 %7D%0A

Add iterative shooter power control to autonomous.