diff --git a/commands/Drivetrain/ReversableArcadeDrive.java b/commands/Drivetrain/ReversableArcadeDrive.java index f715039..b6579e2 100644 --- a/commands/Drivetrain/ReversableArcadeDrive.java +++ b/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); } } diff --git a/commands/camera/AutoCameraCommand.java b/commands/camera/AutoCameraCommand.java new file mode 100644 index 0000000..001bbc7 --- /dev/null +++ b/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 { +} diff --git a/commands/lightstrip/TeamColorLights.java b/commands/lightstrip/TeamColorLights.java index 0976ab2..b56c30b 100644 --- a/commands/lightstrip/TeamColorLights.java +++ b/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(); } - - } \ No newline at end of file diff --git a/subsystems/CappedSpeedControllerGroup.java b/subsystems/CappedSpeedControllerGroup.java new file mode 100644 index 0000000..9234b42 --- /dev/null +++ b/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; + } +} diff --git a/subsystems/DifferentialDriveTrain.java b/subsystems/DifferentialDriveTrain.java index 574ecf1..d25e1a4 100644 --- a/subsystems/DifferentialDriveTrain.java +++ b/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 diff --git a/subsystems/LightStrip.java b/subsystems/LightStrip.java index 07e73ea..ba950a7 100644 --- a/subsystems/LightStrip.java +++ b/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; + } + } } \ No newline at end of file