New component fixes

commands/Drivetrain/ReversableArcadeDrive.java

@@ -2,10 +2,17 @@
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
 import frc.robot.core751.subsystems.DifferentialDriveTrain;
 
 public class ReversableArcadeDrive extends CommandBase {
 
+    private double startYDistance;
+    private double previousYDistance = 0;
+    private double targetYDistance;
+    private boolean inDeaccel = false;
+    private double startStepDeaccelTime;
+
     private Joystick driveStick;
     private DifferentialDriveTrain differentialDriveTrain;
 
@@ -18,7 +25,40 @@ public ReversableArcadeDrive(Joystick driveStick, DifferentialDriveTrain differe
     @Override
     public void execute() {
         int mod = differentialDriveTrain.getDirection().getMod();
-        this.differentialDriveTrain.getDifferentialDrive().arcadeDrive(-driveStick.getRawAxis(4), driveStick.getRawAxis(5)*mod);
+        double x = -driveStick.getRawAxis(4);
+        double y = driveStick.getRawAxis(5)*mod;
+
+        if(inDeaccel && 
+            edu.wpi.first.wpilibj.Timer.getFPGATimestamp() - startStepDeaccelTime >= 
+            Constants.maxSparkDeccelPeriod / Constants.sparkDeccelSteps) {
+            startStepDeaccelTime = edu.wpi.first.wpilibj.Timer.getFPGATimestamp(); 
+
+            previousYDistance += (targetYDistance - startYDistance) / Constants.sparkDeccelSteps;
+
+            if(Math.abs(targetYDistance - y) >= Constants.minPauseDeaccelThreshold){
+                inDeaccel = false;
+                return;
+            }
+
+            if(previousYDistance == targetYDistance) {
+                inDeaccel = false;
+            }
+        } else {
+            if((previousYDistance >= 0 && previousYDistance - x >= Constants.sparkDeccelThreshold) ||
+                (previousYDistance <= -1 && y - previousYDistance >= Constants.sparkDeccelThreshold)) {
+                inDeaccel = true;
+                startYDistance = previousYDistance;
+                targetYDistance = y;
+
+                previousYDistance += (targetYDistance - startYDistance) / Constants.sparkDeccelSteps;
+
+                startStepDeaccelTime = edu.wpi.first.wpilibj.Timer.getFPGATimestamp(); //seconds
+            } else {
+                previousYDistance = y;
+            }
+        }
+
+        this.differentialDriveTrain.getDifferentialDrive().arcadeDrive(x, previousYDistance);
     }
 
 }

commands/camera/AutoCameraCommand.java

@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.core751.commands.camera;
+
+/**
+ * Add your docs here.
+ */
+public class AutoCameraCommand {
+}

commands/lightstrip/TeamColorLights.java

@@ -6,18 +6,20 @@
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.core751.subsystems.LightStrip;
+import frc.robot.core751.subsystems.LightStrip.PostProccessingEffects;
 
 public class TeamColorLights extends CommandBase{
 
-    private LightStrip lightStrip;
+    private LightStrip[] lightStrips;
     private Alliance alliance;
     private int[] allianceColor;
 
-    public TeamColorLights(LightStrip lightStrip) {
-        this.lightStrip = lightStrip;
-
-        addRequirements(lightStrip);
+    public TeamColorLights(LightStrip[] lightStrips) {
+        this.lightStrips = lightStrips;
 
+        for (LightStrip l : lightStrips) {
+            addRequirements(l);
+        }
     }
 
     @Override
@@ -28,27 +30,26 @@ public void initialize() {
                 this.allianceColor = new int[]{0, 255, 255};
             break;
             case Blue:
-                this.allianceColor = new int[]{240, 255, 255};
+                this.allianceColor = new int[]{100, 255, 255};
             break;
             default:
                 this.allianceColor = new int[]{300, 255, 255};
             break;
         }
 
-        for (int i = 0; i < lightStrip.length; i++) {
-            this.lightStrip.setHSVWave(i, 2, this.allianceColor);
+        for (LightStrip l : lightStrips) {
+            l.fillHSV(this.allianceColor[0], this.allianceColor[1], this.allianceColor[2]);
+            l.clearEffects();
+            l.setEffect(PostProccessingEffects.WAVE);
+            l.update();
         }
-        this.lightStrip.update();
-        this.lightStrip.start();
+
     }
 
     @Override
     public void execute() {
-        for (int i = 0; i < lightStrip.length; i++) {
-            this.lightStrip.setHSVWave(i, 2, this.allianceColor);
+        for (LightStrip l : lightStrips) {
+            l.update();
         }
-        this.lightStrip.update();
     }
-
-
 }

subsystems/CappedSpeedControllerGroup.java

@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.core751.subsystems;
+
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.SpeedController;
+import frc.robot.Constants;
+
+public class CappedSpeedControllerGroup extends SpeedControllerGroup {
+
+    private double speedCap;
+
+    public CappedSpeedControllerGroup(SpeedController speedController, double speedCap, SpeedController... speedControllers) {
+        super(speedController, speedControllers);
+        this.speedCap = speedCap;
+    }
+
+    @Override
+    public void set(double speed) {
+        if (speed < 0) {
+            speed = Math.max(speed, -speedCap);
+        } else if (speed > 0) {
+            speed = Math.min(speed, speedCap);
+        }
+        super.set(speed);
+    }
+
+    public double getSpeedCap() {
+        return speedCap;
+    }
+
+    public void setSpeedCap(double speedCap) {
+        this.speedCap = speedCap;
+    }
+}

subsystems/DifferentialDriveTrain.java

@@ -1,6 +1,5 @@
 package frc.robot.core751.subsystems;
 
-import com.revrobotics.SparkMax;
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
@@ -40,7 +39,7 @@ private DriveTrainDirection(int mod) {
 
     private SpeedController[] controllers;
 
-    private DriveTrainDirection direction = DriveTrainDirection.FORWARD;
+    public static DriveTrainDirection direction = DriveTrainDirection.FORWARD;
 
     private static SpeedControllerGroup arrayToGroup(SpeedController[] sp) {
         //There has to be a better way to do this

subsystems/LightStrip.java

@@ -9,15 +9,41 @@ public class LightStrip extends SubsystemBase {
 
     private AddressableLED LED;
     public int length;
+    private int[][] preEffectLEDS;
+    private int[][] postEffectLEDS;
     public AddressableLEDBuffer buffer;
     public int tic;
+    private boolean[] effects;
+    private Orientation orientation;
+    public int cycleCount = 1;
+    public int hueShiftSpeed = 7;
 
-    public LightStrip(int port, int length) {
+    public enum Orientation {
+        FORWARD,
+        BACKWARD,
+        HORIZONTAL;
+    }
+
+    public enum PostProccessingEffects {
+        WAVE,
+        HUE_SHIFT;
+    }
+
+    public LightStrip(int port, int length, Orientation orientation) {
         this.LED = new AddressableLED(port);
         this.LED.setLength(length);
         this.length = length;
         this.buffer = new AddressableLEDBuffer(length);
+        this.effects = new boolean[PostProccessingEffects.values().length];
+        this.preEffectLEDS = new int[length][3];
+        this.postEffectLEDS = new int[length][3];
+        this.cycleCount = length/5;
         this.tic = 0;
+        this.postEffectLEDS = new int[length][3];
+        this.preEffectLEDS = new int [length][3];
+        this.orientation = orientation;
+        this.effects = new boolean[PostProccessingEffects.values().length];
+        this.start();
     }
 
     public void start() {
@@ -30,30 +56,92 @@ public void stop() {
 
     public void advanceTic(){
         this.tic++;
-        this.tic%=2048;
+        this.tic%=(length*length);
     }
 
     public void update() {
+        this.postProccessing();
+        this.copyToBuffer();
         this.LED.setData(this.buffer);
         this.advanceTic();
     }
 
     public void setHSV(int i, int h, int s, int v) {
-        this.buffer.setHSV(i, h/2, s, v);
+        this.buffer.setHSV(i, h, s, v);
     }
 
-    public void setHSVWave(int i, int cycleCount, int h, int s, int v) {
-        int ni = i + tic/5;
-        ni %= this.length;
+    private void postProccessing() {
+        for (int i = 0; i < length; i++) {
+            if (effects[PostProccessingEffects.WAVE.ordinal()]) {
+                switch(this.orientation) {
+                    case FORWARD:
+                        this.postEffectLEDS[i][2] = (int)(this.preEffectLEDS[i][2] * this.getWaveMod(i * DifferentialDriveTrain.direction.getMod()));
+                    break;
+                    case BACKWARD:
+                        this.postEffectLEDS[i][2] = (int)(this.preEffectLEDS[i][2] * this.getWaveMod(-i* DifferentialDriveTrain.direction.getMod()));
+                    break;
+                    case HORIZONTAL:
+                        if (i < length/2) {
+                            this.postEffectLEDS[i][2] = (int)(this.preEffectLEDS[i][2] * this.getWaveMod(i));
+                        }else {
+                            this.postEffectLEDS[i][2] = (int)(this.preEffectLEDS[i][2] * this.getWaveMod(-i));
+                        }
+                    break;
+                }
+            }
+            if (effects[PostProccessingEffects.HUE_SHIFT.ordinal()]) {
+                if (Math.abs(this.postEffectLEDS[i][0] - this.preEffectLEDS[i][0]) >= hueShiftSpeed) {
+                    this.postEffectLEDS[i][0] = this.preEffectLEDS[i][0];
+                }else {
+                    this.postEffectLEDS[i][0] += hueShiftSpeed * this.postEffectLEDS[i][0]>this.preEffectLEDS[i][0]?-1:1;
+                }
+            } else {
+                this.postEffectLEDS[i][0] = this.preEffectLEDS[i][0];
+            }
+
+            this.postEffectLEDS[i][1] = this.preEffectLEDS[i][1];
+        }
+
 
-        v = (int) (v *   (  (Math.cos( (((double)(ni))/this.length)*cycleCount*2  * Math.PI )  + 2 )  /3)    );
+    }
+
+    private double getWaveMod(int i) {
+        //return ((Math.cos((i+(((float)tic/100f)/length)%length)/length)*cycleCount*2*Math.PI)+2/3);
+        float m = i +  tic/5f;
 
-        this.setHSV(i, h, s, v);
+        float cl = length/cycleCount;
+        float o =  m%cl;
+        float v = 0.25f;
+        if (o < (cl/2.0)) {
+            return 0.75+(v*o)/(cl/2);
+        }else {
+            return 0.75 + (v-(v/(cl/2)))*o;
+        }
+    }
+
+    public void fillHSV(int h, int s, int v) {
+        for (int i = 0; i < length; i++) {
+                this.preEffectLEDS[i][0] = h;
+                this.preEffectLEDS[i][1] = s;
+                this.preEffectLEDS[i][2] = v;
+
+        }
     }
 
-    public void setHSVWave(int i, int cycleCount, int[] c) {
-        this.setHSVWave(i, cycleCount, c[0], c[1], c[2]);
+    private void copyToBuffer() {
+        for (int i = 0; i < length; i++) {
+            //this.buffer.setHSV(i, this.preEffectLEDS[i][0], this.preEffectLEDS[i][0], this.preEffectLEDS[i][0]);
+            this.buffer.setHSV(i, this.postEffectLEDS[i][0], this.postEffectLEDS[i][1], this.postEffectLEDS[i][2]);
+        }
     }
 
+    public void setEffect(PostProccessingEffects e) {
+        this.effects[e.ordinal()] = true;
+    }
 
+    public void clearEffects() {
+        for (int i = 0; i < effects.length; i++) {
+            this.effects[i] = false;
+        }
+    }
 }