From e8396029e98a00544e437300a4143ff7b01e4102 Mon Sep 17 00:00:00 2001 From: Dimitri Date: Mon, 17 Feb 2020 21:50:44 -0800 Subject: [PATCH 1/7] Updated repo to include fixes for day before mock bag --- .../Drivetrain/ReversableArcadeDrive.java | 42 ++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) 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); } } From 726e422cc54edbc4288de096517d8b5bf445e91b Mon Sep 17 00:00:00 2001 From: Dimitri Date: Tue, 18 Feb 2020 12:57:16 -0800 Subject: [PATCH 2/7] Make lightstrip big brain --- commands/MacroMaker.java | 272 +++++++++++++++++++++ commands/lightstrip/TeamColorLights.java | 27 +- subsystems/CappedSpeedControllerGroup.java | 39 +++ subsystems/LightStrip.java | 96 +++++++- 4 files changed, 412 insertions(+), 22 deletions(-) create mode 100644 commands/MacroMaker.java create mode 100644 subsystems/CappedSpeedControllerGroup.java diff --git a/commands/MacroMaker.java b/commands/MacroMaker.java new file mode 100644 index 0000000..0f177ab --- /dev/null +++ b/commands/MacroMaker.java @@ -0,0 +1,272 @@ +package frc.robot.core751.commands; + +import java.io.File; +import java.io.PrintWriter; + +import java.util.LinkedList; +import java.util.Scanner; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.core751.Controllable; +import frc.robot.core751.Parseable; +import frc.robot.core751.wrappers.OverrideableJoystick; + +public class MacroMaker extends CommandBase { + private Scanner scanner; + private PrintWriter writer; + private String file;//Maybe make this a constaint? + + private Controllable[] subsystems; // For building macros + private Joystick joystick; // For joystick buttons + private Boolean[] SmartDashButtons; // For smartdashboard buttons + private Macro[] macros; + + public MacroMaker(Controllable[] subsystems, Joystick joystick) { + this(subsystems, joystick, null); + } + + public MacroMaker(Controllable[] subsystems, Boolean[] SmartDashButtons) { + this(subsystems, null, SmartDashButtons); + } + + public MacroMaker(Controllable[] subsystems, Joystick joystick, Boolean[] SmartDashButtons) { + this.subsystems = subsystems; + this.joystick = joystick; + this.SmartDashButtons = SmartDashButtons; + this.macros = new Macro[joystick.getButtonCount() + SmartDashButtons.length]; + + SmartDashboard.putBoolean("Editing", true); + SmartDashboard.putBoolean("Add", false); + SmartDashboard.putBoolean("Remove", false); + SmartDashboard.putNumber("Button", 0); + + for (Controllable subsystem : subsystems) { + + if (subsystem instanceof SubsystemBase) { + addRequirements((SubsystemBase) subsystem); + } + + } + try{ + File file = new File(this.file); + scanner = new Scanner(file); + writer = new PrintWriter(file); + + scanner.useDelimiter(",|\\n"); + }catch(Exception e){//Good coding practice trust me + System.out.println(e); + } + } + + @Override + public void execute() { + if (SmartDashboard.getBoolean("Editing", true)) { + + if (SmartDashboard.getBoolean("Add", false)) { + SmartDashboard.putBoolean("Add", false); + int button = (int) SmartDashboard.getNumber("Button", 0); + if (button > 0 && button < macros.length) { + if (macros[button - 1] != null) { + // macros[button - 1].add(subsystem, value, code, time); + } else { + macros[button - 1] = new Macro(); + // macros[button - 1].add(subsystem, value, code, time); + } + } + } else if (SmartDashboard.getBoolean("Remove", false)) { + SmartDashboard.putBoolean("Remove", false); + int button = (int) SmartDashboard.getNumber("Button", 0); + if (button > 0 && button < macros.length) { + if (macros[button - 1] != null) { + //macros.remove(subsystem); + } + } + } + + } else { + for (int i = 0; i < macros.length; i++) {// There has got to be a better way to do this. + if (i <= joystick.getButtonCount()) { + if (joystick.getRawButtonPressed(i)) { + macros[i - 1].execute(); + } + } else if (SmartDashButtons[i]) { + macros[i - 1].execute(); + } + } + } + + } + + public void autoRead() { + + } + + public void autoWrite(){ + + } + + public void read()throws FileFormatException{ //CHANGE ChANGE CHANGE CHANGE + int line = 0; + int count = 0; + String temp; + Controllable subsystem; + while(scanner.hasNext()){ + temp = scanner.next(); + if(temp != null){ + macros[line] = new Macro(); + subsystem = subsystemLookup(temp); + macros[line].add(subsystem, parseValue(subsystem, scanner.next()), Integer.parseInt(scanner.next()), Long.parseLong(scanner.next())); + } + line++; + + } + } + + private Controllable subsystemLookup(String string){ + for(Controllable subsystem : subsystems){ + if(subsystem.name().equals(string)){ + return subsystem; + } + } + return null; + } + + private Object parseValue(Controllable subsystem, String string){ + switch(subsystem.parseType()){ + case Int : + return Integer.parseInt(string); + + case Double : + return Double.parseDouble(string); + + case Bool : + return Boolean.parseBoolean(string); + + case Parseable: //Will create the interface later + + } + return null; + } + + public void write() {// To a text file(The easy one) CHAnGE CHAMNGE CHANGE CHANGE CHANGE CHANGE + String temp = ""; + for(Macro macro : macros){ + if(macro != null){ + temp += macro; + } else { + temp += "null"; + } + temp += "\\n"; + } + writer.print(temp); + } + + private abstract class Macro { public abstract void run(); @Override public abstract String toString();} //Just to have an abstract class to extend. + + + public static enum pressModel{//I can't put this in button Macro ReeeeEEEEeeEe + Press, + Hold, + Release + } + private class buttonMacro extends Macro{ + int button; + pressModel pModel; + OverrideableJoystick joystick; + + public buttonMacro(int button, pressModel pModel, OverrideableJoystick joystick){ + this.joystick = joystick; + this.pModel = pModel; + this.button = button; + } + + @Override + public void run() { + switch(pModel){ + case Press: + joystick.press(button); + break; + case Hold: + joystick.hold(button); + break; + case Release: + joystick.release(button); + break; + } + } + + @Override + public String toString() { + return "B" + button + "," + pModel +",@"; + } + } + + private class commandMacro extends Macro{ + + private Controllable subsystem; + private Object value;//Turn to Parsable when Wrappers are done + private int code; + + public commandMacro(Controllable subsystem, Object value) { + this(subsystem, value, 0); + } + + public commandMacro(Controllable subsystem, Object value, int code) { + this.subsystem = subsystem; + this.value = value; + this.code = code; + } + + @Override + public void run() { + subsystem.execute(value, code); + } + @Override + public String toString() { + return "C" + subsystem + "," + value +"," + code +",@"; + } + + } + private class MacroList extends CommandBase { + + private LinkedList macros = new LinkedList<>(); + private LinkedList timeList = new LinkedList<>(); + + public String toString() { + String returnValue = ""; + int index = 0; + for(Macro macro : macros){ + returnValue += macro.toString() + timeList.get(index) +";"; + index ++; + } + return returnValue; + } + public void add(Macro macro){ + add(macro,0L); + } + public void add(Macro macro,Long time){ + macros.add(macro); + timeList.add(time); + } + + public void remove() { + macros.removeLast(); + } + + @Override + public void execute() { + // TODO Auto-generated method stub + super.execute(); + } + + } + private class FileFormatException extends Exception{ + public FileFormatException(int line){ + super("" + line); + } + } + +} diff --git a/commands/lightstrip/TeamColorLights.java b/commands/lightstrip/TeamColorLights.java index 0976ab2..7d4e016 100644 --- a/commands/lightstrip/TeamColorLights.java +++ b/commands/lightstrip/TeamColorLights.java @@ -6,17 +6,21 @@ 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; + public TeamColorLights(LightStrip[] lightStrips) { + this.lightStrips = lightStrips; - addRequirements(lightStrip); + for (LightStrip l : lightStrips) { + addRequirements(l); + } + } @@ -35,19 +39,20 @@ public void initialize() { 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(); } diff --git a/subsystems/CappedSpeedControllerGroup.java b/subsystems/CappedSpeedControllerGroup.java new file mode 100644 index 0000000..0f0335a --- /dev/null +++ b/subsystems/CappedSpeedControllerGroup.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* 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 = Constants.DefaultSpeedCap; + + public CappedSpeedControllerGroup(SpeedController speedController, SpeedController... speedControllers) { + super(speedController, speedControllers); + } + + @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/LightStrip.java b/subsystems/LightStrip.java index 07e73ea..3c0203c 100644 --- a/subsystems/LightStrip.java +++ b/subsystems/LightStrip.java @@ -9,15 +9,38 @@ 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 = 2; + 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.start(); + this.orientation = orientation; } public void start() { @@ -34,26 +57,77 @@ public void advanceTic(){ } 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; - - v = (int) (v * ( (Math.cos( (((double)(ni))/this.length)*cycleCount*2 * Math.PI ) + 2 ) /3) ); - - this.setHSV(i, h, s, v); + public void clearEffects() { + for (int i = 0; i < effects.length; i++) { + effects[i] = false; + } } - public void setHSVWave(int i, int cycleCount, int[] c) { - this.setHSVWave(i, cycleCount, c[0], c[1], c[2]); + 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)); + break; + case BACKWARD: + this.postEffectLEDS[i][2] = (int)(this.preEffectLEDS[i][2] * this.getWaveMod(-i)); + 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]; + } + } } + private double getWaveMod(int i) { + return (Math.cos((i+(tic/length)%length)*cycleCount*2*Math.PI)+2/3); + } + + 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; + + } + } + + private void copyToBuffer() { + for (int i = 0; i < length; i++) { + 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; + } + + + } \ No newline at end of file From b40af3f3fea9cdfd8ab5f928700e9754a09c95c8 Mon Sep 17 00:00:00 2001 From: Dimitri Date: Tue, 18 Feb 2020 13:30:28 -0800 Subject: [PATCH 3/7] dont know why macro code was here --- commands/MacroMaker.java | 272 --------------------- subsystems/CappedSpeedControllerGroup.java | 5 +- 2 files changed, 3 insertions(+), 274 deletions(-) delete mode 100644 commands/MacroMaker.java diff --git a/commands/MacroMaker.java b/commands/MacroMaker.java deleted file mode 100644 index 0f177ab..0000000 --- a/commands/MacroMaker.java +++ /dev/null @@ -1,272 +0,0 @@ -package frc.robot.core751.commands; - -import java.io.File; -import java.io.PrintWriter; - -import java.util.LinkedList; -import java.util.Scanner; - -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.core751.Controllable; -import frc.robot.core751.Parseable; -import frc.robot.core751.wrappers.OverrideableJoystick; - -public class MacroMaker extends CommandBase { - private Scanner scanner; - private PrintWriter writer; - private String file;//Maybe make this a constaint? - - private Controllable[] subsystems; // For building macros - private Joystick joystick; // For joystick buttons - private Boolean[] SmartDashButtons; // For smartdashboard buttons - private Macro[] macros; - - public MacroMaker(Controllable[] subsystems, Joystick joystick) { - this(subsystems, joystick, null); - } - - public MacroMaker(Controllable[] subsystems, Boolean[] SmartDashButtons) { - this(subsystems, null, SmartDashButtons); - } - - public MacroMaker(Controllable[] subsystems, Joystick joystick, Boolean[] SmartDashButtons) { - this.subsystems = subsystems; - this.joystick = joystick; - this.SmartDashButtons = SmartDashButtons; - this.macros = new Macro[joystick.getButtonCount() + SmartDashButtons.length]; - - SmartDashboard.putBoolean("Editing", true); - SmartDashboard.putBoolean("Add", false); - SmartDashboard.putBoolean("Remove", false); - SmartDashboard.putNumber("Button", 0); - - for (Controllable subsystem : subsystems) { - - if (subsystem instanceof SubsystemBase) { - addRequirements((SubsystemBase) subsystem); - } - - } - try{ - File file = new File(this.file); - scanner = new Scanner(file); - writer = new PrintWriter(file); - - scanner.useDelimiter(",|\\n"); - }catch(Exception e){//Good coding practice trust me - System.out.println(e); - } - } - - @Override - public void execute() { - if (SmartDashboard.getBoolean("Editing", true)) { - - if (SmartDashboard.getBoolean("Add", false)) { - SmartDashboard.putBoolean("Add", false); - int button = (int) SmartDashboard.getNumber("Button", 0); - if (button > 0 && button < macros.length) { - if (macros[button - 1] != null) { - // macros[button - 1].add(subsystem, value, code, time); - } else { - macros[button - 1] = new Macro(); - // macros[button - 1].add(subsystem, value, code, time); - } - } - } else if (SmartDashboard.getBoolean("Remove", false)) { - SmartDashboard.putBoolean("Remove", false); - int button = (int) SmartDashboard.getNumber("Button", 0); - if (button > 0 && button < macros.length) { - if (macros[button - 1] != null) { - //macros.remove(subsystem); - } - } - } - - } else { - for (int i = 0; i < macros.length; i++) {// There has got to be a better way to do this. - if (i <= joystick.getButtonCount()) { - if (joystick.getRawButtonPressed(i)) { - macros[i - 1].execute(); - } - } else if (SmartDashButtons[i]) { - macros[i - 1].execute(); - } - } - } - - } - - public void autoRead() { - - } - - public void autoWrite(){ - - } - - public void read()throws FileFormatException{ //CHANGE ChANGE CHANGE CHANGE - int line = 0; - int count = 0; - String temp; - Controllable subsystem; - while(scanner.hasNext()){ - temp = scanner.next(); - if(temp != null){ - macros[line] = new Macro(); - subsystem = subsystemLookup(temp); - macros[line].add(subsystem, parseValue(subsystem, scanner.next()), Integer.parseInt(scanner.next()), Long.parseLong(scanner.next())); - } - line++; - - } - } - - private Controllable subsystemLookup(String string){ - for(Controllable subsystem : subsystems){ - if(subsystem.name().equals(string)){ - return subsystem; - } - } - return null; - } - - private Object parseValue(Controllable subsystem, String string){ - switch(subsystem.parseType()){ - case Int : - return Integer.parseInt(string); - - case Double : - return Double.parseDouble(string); - - case Bool : - return Boolean.parseBoolean(string); - - case Parseable: //Will create the interface later - - } - return null; - } - - public void write() {// To a text file(The easy one) CHAnGE CHAMNGE CHANGE CHANGE CHANGE CHANGE - String temp = ""; - for(Macro macro : macros){ - if(macro != null){ - temp += macro; - } else { - temp += "null"; - } - temp += "\\n"; - } - writer.print(temp); - } - - private abstract class Macro { public abstract void run(); @Override public abstract String toString();} //Just to have an abstract class to extend. - - - public static enum pressModel{//I can't put this in button Macro ReeeeEEEEeeEe - Press, - Hold, - Release - } - private class buttonMacro extends Macro{ - int button; - pressModel pModel; - OverrideableJoystick joystick; - - public buttonMacro(int button, pressModel pModel, OverrideableJoystick joystick){ - this.joystick = joystick; - this.pModel = pModel; - this.button = button; - } - - @Override - public void run() { - switch(pModel){ - case Press: - joystick.press(button); - break; - case Hold: - joystick.hold(button); - break; - case Release: - joystick.release(button); - break; - } - } - - @Override - public String toString() { - return "B" + button + "," + pModel +",@"; - } - } - - private class commandMacro extends Macro{ - - private Controllable subsystem; - private Object value;//Turn to Parsable when Wrappers are done - private int code; - - public commandMacro(Controllable subsystem, Object value) { - this(subsystem, value, 0); - } - - public commandMacro(Controllable subsystem, Object value, int code) { - this.subsystem = subsystem; - this.value = value; - this.code = code; - } - - @Override - public void run() { - subsystem.execute(value, code); - } - @Override - public String toString() { - return "C" + subsystem + "," + value +"," + code +",@"; - } - - } - private class MacroList extends CommandBase { - - private LinkedList macros = new LinkedList<>(); - private LinkedList timeList = new LinkedList<>(); - - public String toString() { - String returnValue = ""; - int index = 0; - for(Macro macro : macros){ - returnValue += macro.toString() + timeList.get(index) +";"; - index ++; - } - return returnValue; - } - public void add(Macro macro){ - add(macro,0L); - } - public void add(Macro macro,Long time){ - macros.add(macro); - timeList.add(time); - } - - public void remove() { - macros.removeLast(); - } - - @Override - public void execute() { - // TODO Auto-generated method stub - super.execute(); - } - - } - private class FileFormatException extends Exception{ - public FileFormatException(int line){ - super("" + line); - } - } - -} diff --git a/subsystems/CappedSpeedControllerGroup.java b/subsystems/CappedSpeedControllerGroup.java index 0f0335a..9234b42 100644 --- a/subsystems/CappedSpeedControllerGroup.java +++ b/subsystems/CappedSpeedControllerGroup.java @@ -13,10 +13,11 @@ public class CappedSpeedControllerGroup extends SpeedControllerGroup { - private double speedCap = Constants.DefaultSpeedCap; + private double speedCap; - public CappedSpeedControllerGroup(SpeedController speedController, SpeedController... speedControllers) { + public CappedSpeedControllerGroup(SpeedController speedController, double speedCap, SpeedController... speedControllers) { super(speedController, speedControllers); + this.speedCap = speedCap; } @Override From 32c842428b1408725448ba61b3c3ced5a4e098f6 Mon Sep 17 00:00:00 2001 From: Dimitri Date: Tue, 18 Feb 2020 13:37:41 -0800 Subject: [PATCH 4/7] LED strips witch direction when drivetrain does --- subsystems/DifferentialDriveTrain.java | 2 +- subsystems/LightStrip.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/subsystems/DifferentialDriveTrain.java b/subsystems/DifferentialDriveTrain.java index 574ecf1..cb16e6f 100644 --- a/subsystems/DifferentialDriveTrain.java +++ b/subsystems/DifferentialDriveTrain.java @@ -40,7 +40,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 3c0203c..31fe5f3 100644 --- a/subsystems/LightStrip.java +++ b/subsystems/LightStrip.java @@ -78,10 +78,10 @@ private void postProccessing() { if (effects[PostProccessingEffects.WAVE.ordinal()]) { switch(this.orientation) { case FORWARD: - this.postEffectLEDS[i][2] = (int)(this.preEffectLEDS[i][2] * this.getWaveMod(i)); + 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)); + this.postEffectLEDS[i][2] = (int)(this.preEffectLEDS[i][2] * this.getWaveMod(-i* DifferentialDriveTrain.direction.getMod())); break; case HORIZONTAL: if (i < length/2) { From c244a6dfc7a054d1ec56d840d22bdcf49130d5f6 Mon Sep 17 00:00:00 2001 From: Dimitri Saliba Date: Tue, 18 Feb 2020 15:13:43 -0800 Subject: [PATCH 5/7] Directional Camera Switching --- commands/camera/AutoCameraCommand.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 commands/camera/AutoCameraCommand.java 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 { +} From 95ab07ce79ca7f959f8870b0781d95810ce846d1 Mon Sep 17 00:00:00 2001 From: Dimitri Date: Tue, 25 Feb 2020 17:42:29 -0800 Subject: [PATCH 6/7] WIP --- commands/lightstrip/TeamColorLights.java | 23 ++--- subsystems/DifferentialDriveTrain.java | 2 +- subsystems/LightStrip.java | 104 ++++++++++++++++++++++- 3 files changed, 113 insertions(+), 16 deletions(-) diff --git a/commands/lightstrip/TeamColorLights.java b/commands/lightstrip/TeamColorLights.java index 0976ab2..3c3ffe7 100644 --- a/commands/lightstrip/TeamColorLights.java +++ b/commands/lightstrip/TeamColorLights.java @@ -6,17 +6,18 @@ 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; + public TeamColorLights(LightStrip[] lightStrips) { + this.lightStrips = lightStrips; - addRequirements(lightStrip); + addRequirements(lightStrips); } @@ -35,19 +36,19 @@ public void initialize() { break; } - for (int i = 0; i < lightStrip.length; i++) { - this.lightStrip.setHSVWave(i, 2, this.allianceColor); + for (LightStrip l : lightStrips) { + l.clearEffects(); + l.setEffect(PostProccessingEffects.WAVE); + l.fillHSV(this.allianceColor[0], this.allianceColor[0], this.allianceColor[0]); + 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(); } diff --git a/subsystems/DifferentialDriveTrain.java b/subsystems/DifferentialDriveTrain.java index 574ecf1..cb16e6f 100644 --- a/subsystems/DifferentialDriveTrain.java +++ b/subsystems/DifferentialDriveTrain.java @@ -40,7 +40,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..c37fea4 100644 --- a/subsystems/LightStrip.java +++ b/subsystems/LightStrip.java @@ -11,13 +11,35 @@ public class LightStrip extends SubsystemBase { public int length; public AddressableLEDBuffer buffer; public int tic; + private boolean[] effects; + private Orientation orientation; + public int cycleCount = 1; + public int hueShiftSpeed = 7; + public int [][] postEffectLEDS; + public int [][] preEffectLEDS; - 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.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,10 +52,12 @@ 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(); } @@ -51,9 +75,81 @@ public void setHSVWave(int i, int cycleCount, int h, int s, int v) { this.setHSV(i, h, s, v); } - public void setHSVWave(int i, int cycleCount, int[] c) { - this.setHSVWave(i, cycleCount, c[0], c[1], c[2]); + 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]; + } + + + } + + 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; + + 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; + + } + } + + private void copyToBuffer() { + for (int i = 0; i < length; i++) { + 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 From 6ff7292ec6ff4a857b1b8d67b4a0d09988e17cce Mon Sep 17 00:00:00 2001 From: Dimitri Date: Sat, 29 Feb 2020 12:21:12 -0800 Subject: [PATCH 7/7] Tweaked HSV colors to be correct --- commands/lightstrip/TeamColorLights.java | 6 +----- subsystems/DifferentialDriveTrain.java | 1 - subsystems/LightStrip.java | 13 +------------ 3 files changed, 2 insertions(+), 18 deletions(-) diff --git a/commands/lightstrip/TeamColorLights.java b/commands/lightstrip/TeamColorLights.java index 7d4e016..b56c30b 100644 --- a/commands/lightstrip/TeamColorLights.java +++ b/commands/lightstrip/TeamColorLights.java @@ -20,8 +20,6 @@ public TeamColorLights(LightStrip[] lightStrips) { for (LightStrip l : lightStrips) { addRequirements(l); } - - } @Override @@ -32,7 +30,7 @@ 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}; @@ -54,6 +52,4 @@ public void execute() { l.update(); } } - - } \ No newline at end of file diff --git a/subsystems/DifferentialDriveTrain.java b/subsystems/DifferentialDriveTrain.java index cb16e6f..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; diff --git a/subsystems/LightStrip.java b/subsystems/LightStrip.java index 8569d4b..ba950a7 100644 --- a/subsystems/LightStrip.java +++ b/subsystems/LightStrip.java @@ -17,8 +17,6 @@ public class LightStrip extends SubsystemBase { private Orientation orientation; public int cycleCount = 1; public int hueShiftSpeed = 7; - public int [][] postEffectLEDS; - public int [][] preEffectLEDS; public enum Orientation { FORWARD, @@ -72,12 +70,6 @@ public void setHSV(int i, int h, int s, int v) { this.buffer.setHSV(i, h, s, v); } - public void clearEffects() { - for (int i = 0; i < effects.length; i++) { - effects[i] = false; - } - } - private void postProccessing() { for (int i = 0; i < length; i++) { if (effects[PostProccessingEffects.WAVE.ordinal()]) { @@ -138,6 +130,7 @@ public void fillHSV(int h, int s, int v) { 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]); } } @@ -151,8 +144,4 @@ public void clearEffects() { this.effects[i] = false; } } - - - - } \ No newline at end of file