added flashyflash

Author: abbbbbby

core/src/main/java/org/teamtators/rotator/commands/AutonFlashyFlash.java

@@ -0,0 +1,62 @@
+package org.teamtators.rotator.commands;
+
+import org.teamtators.rotator.CommandBase;
+import org.teamtators.rotator.CoreRobot;
+import org.teamtators.rotator.config.Configurable;
+import org.teamtators.rotator.control.Timer;
+import org.teamtators.rotator.subsystems.AbstractVision;
+
+public class AutonFlashyFlash extends CommandBase implements Configurable<AutonFlashyFlash.Config> {
+
+    private Config config;
+    private int times;
+    private Timer timer;
+    private AbstractVision vision;
+
+    public AutonFlashyFlash(CoreRobot robot) {
+        super("AutonFlashyFlash");
+        vision = robot.vision();
+        requires(vision);
+
+    }
+
+    @Override
+    public void configure(AutonFlashyFlash.Config config) {
+        this.config = config;
+    }
+
+    @Override
+    protected void initialize() {
+        logger.info("Flashing light {} times", config.maxTimes);
+        times = 0;
+        vision.setLedState(true);
+        timer.reset();
+        timer.start();
+    }
+
+    @Override
+    protected boolean step() {
+        boolean blinding = vision.getLedState();
+        if(blinding && timer.hasPeriodElapsed(config.onTime)) {
+            vision.setLedState(false);
+            timer.reset();
+            timer.start();
+            times++;
+        } else if(!blinding && timer.hasPeriodElapsed(config.offTime)) {
+            vision.setLedState(true);
+            timer.reset();
+            timer.start();
+        }
+        return times >= config.maxTimes;
+    }
+
+    protected void finish() {
+        vision.setLedState(false);
+    }
+
+    static class Config {
+        public int maxTimes;
+        public float onTime;
+        public float offTime;
+    }
+}