From 14286440753e3726ce6b8f5230a2af340c797765 Mon Sep 17 00:00:00 2001 From: NoamRosenberg08 Date: Mon, 24 Apr 2023 19:31:52 +0300 Subject: [PATCH 01/10] noam - added the base for better led i'll work some more after yom hazicaron --- .idea/misc.xml | 2 +- .../greenblitz/tobyDetermined/RobotMap.java | 18 ++++ .../LED/BetterLED/RunSingleSequenceNode.java | 24 +++++ .../LED/BetterLED/SimpleSetColor.java | 27 ++++++ .../tobyDetermined/subsystems/LED.java | 5 + .../utils/InterruptableWaitCommand.java | 29 ++++++ .../edu/greenblitz/utils/LEDSequence.java | 92 +++++++++++++++++++ 7 files changed, 196 insertions(+), 1 deletion(-) create mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSingleSequenceNode.java create mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/SimpleSetColor.java create mode 100644 src/main/java/edu/greenblitz/utils/InterruptableWaitCommand.java create mode 100644 src/main/java/edu/greenblitz/utils/LEDSequence.java diff --git a/.idea/misc.xml b/.idea/misc.xml index c90de3bd..0f221d8c 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -4,5 +4,5 @@ - + \ No newline at end of file diff --git a/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java b/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java index d3c0d999..980954e1 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java @@ -5,6 +5,7 @@ import com.pathplanner.lib.auto.PIDConstants; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMaxLimitSwitch; +import edu.greenblitz.tobyDetermined.subsystems.LED; import edu.greenblitz.tobyDetermined.subsystems.swerve.KazaSwerveModule; import edu.greenblitz.tobyDetermined.subsystems.swerve.SdsSwerveModule; import edu.greenblitz.utils.PIDObject; @@ -72,6 +73,23 @@ public static class Vision { public static class LED { + + public enum Sections{ + + + + normal(1,10), + errors(11,100), + all(0, LENGTH); + + public int start; + public int end; + Sections(int startIndex, int endIndex) { + end = endIndex; + start = startIndex; + } + } + public static final int LENGTH = 100; public static final int PORT = 0; public static final double BLINKING_TIME = 0.2; diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSingleSequenceNode.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSingleSequenceNode.java new file mode 100644 index 00000000..b946f35b --- /dev/null +++ b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSingleSequenceNode.java @@ -0,0 +1,24 @@ +package edu.greenblitz.tobyDetermined.commands.LED.BetterLED; + +import edu.greenblitz.tobyDetermined.commands.LED.LEDCommand; +import edu.greenblitz.utils.InterruptableWaitCommand; +import edu.greenblitz.utils.LEDSequence; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class RunSingleSequenceNode extends SequentialCommandGroup { + + private LEDSequence sequence; + public RunSingleSequenceNode(LEDSequence sequence){ + this.sequence = sequence; + addCommands( + new InterruptableWaitCommand(sequence.getWaitBeforeStartTime()), + new SimpleSetColor(sequence.getColor()), + new InterruptableWaitCommand(sequence.getDuration()) + ); + } + + + + +} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/SimpleSetColor.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/SimpleSetColor.java new file mode 100644 index 00000000..e4abd271 --- /dev/null +++ b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/SimpleSetColor.java @@ -0,0 +1,27 @@ +package edu.greenblitz.tobyDetermined.commands.LED.BetterLED; + +import edu.greenblitz.tobyDetermined.RobotMap; +import edu.greenblitz.tobyDetermined.commands.LED.LEDCommand; +import edu.wpi.first.wpilibj.util.Color; + +public class SimpleSetColor extends LEDCommand { + + private Color color; + + private int startingIndex; + private int endingIndex; + + public SimpleSetColor (int startingIndex,int endingIndex,Color color){ + led.setColor(startingIndex,endingIndex,color); + } + public SimpleSetColor (Color color){ + led.setColor(startingIndex,endingIndex,color); + } + public SimpleSetColor (RobotMap.LED.Sections section, Color color){ + led.setColor(section.start,section.end,color); + } + + + + +} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java index 10986305..72ef75ce 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java @@ -43,6 +43,11 @@ public void setColor(int i, Color color) { this.addressableLED.setData(ledBuffer); } + public void setColor (int startIndex,int endIndex, Color color){ + for (int i = startIndex; i < endIndex; i++) { + setColor(i,color); + } + } public void turnOff (){ setColor(new Color(0,0,0)); diff --git a/src/main/java/edu/greenblitz/utils/InterruptableWaitCommand.java b/src/main/java/edu/greenblitz/utils/InterruptableWaitCommand.java new file mode 100644 index 00000000..32d363f2 --- /dev/null +++ b/src/main/java/edu/greenblitz/utils/InterruptableWaitCommand.java @@ -0,0 +1,29 @@ +package edu.greenblitz.utils; + +import edu.wpi.first.wpilibj.Timer; + +public class InterruptableWaitCommand extends GBCommand{ + + private double time; + private Timer timer; + + public InterruptableWaitCommand (double time){ + this.time = time; + this.timer = new Timer(); + } + + @Override + public void initialize() { + timer.start(); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(this.time); + } + + @Override + public void end(boolean interrupted) { + timer.stop(); + } +} diff --git a/src/main/java/edu/greenblitz/utils/LEDSequence.java b/src/main/java/edu/greenblitz/utils/LEDSequence.java new file mode 100644 index 00000000..b4e7edc9 --- /dev/null +++ b/src/main/java/edu/greenblitz/utils/LEDSequence.java @@ -0,0 +1,92 @@ +package edu.greenblitz.utils; + +import edu.greenblitz.tobyDetermined.RobotMap; +import edu.wpi.first.wpilibj.util.Color; + +public class LEDSequence { + + private Color color; + private double duration; + private double waitBeforeStartTime; + + private RobotMap.LED.Sections section; + private LEDSequence next; + + public LEDSequence(Color color, double duration, double waitBeforeStartTime, RobotMap.LED.Sections section, LEDSequence next) { + this.color = color; + this.duration = duration; + this.waitBeforeStartTime = waitBeforeStartTime; + this.section = section; + this.next = next; + } + + public LEDSequence(Color color) { + this.color = color; + this.duration = 0; + this.waitBeforeStartTime = 0; + this.section = RobotMap.LED.Sections.all; + this.next = null; + } + + //with: + + public LEDSequence withDuration (double duration){ + this.duration = duration; + return this; + } + public LEDSequence withBeforeStartTime (double time){ + this.duration = time; + return this; + } + public LEDSequence withNext (LEDSequence next){ + this.next = next; + return this; + } + + + public LEDSequence setSection (RobotMap.LED.Sections section){ + this.section = section; + return this; + } + public LEDSequence setColor (Color color){ + this.color = color; + return this; + } + public LEDSequence setDuration (double duration){ + this.duration = duration; + return this; + } + public LEDSequence setBeforeStartTime (double time){ + this.waitBeforeStartTime = time; + return this; + } + + + public Color getColor() { + return color; + } + + public double getDuration() { + return duration; + } + + public double getWaitBeforeStartTime() { + return waitBeforeStartTime; + } + + public void setWaitBeforeStartTime(double waitBeforeStartTime) { + this.waitBeforeStartTime = waitBeforeStartTime; + } + + public RobotMap.LED.Sections getSection() { + return section; + } + + public LEDSequence getNext() { + return next; + } + + public void setNext(LEDSequence next) { + this.next = next; + } +} From d877fba40a8a1fc59a82f715fcd1f04a88403869 Mon Sep 17 00:00:00 2001 From: NoamRosenberg08 Date: Mon, 24 Apr 2023 20:57:52 +0300 Subject: [PATCH 02/10] noam - added sequence run command --- .../commands/LED/BetterLED/RunSequence.java | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSequence.java diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSequence.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSequence.java new file mode 100644 index 00000000..8b4018e7 --- /dev/null +++ b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSequence.java @@ -0,0 +1,28 @@ +package edu.greenblitz.tobyDetermined.commands.LED.BetterLED; + +import edu.greenblitz.tobyDetermined.RobotMap; +import edu.greenblitz.utils.GBCommand; +import edu.greenblitz.utils.LEDSequence; + +public class RunSequence extends GBCommand { + + private LEDSequence sequence; + + public RunSequence (LEDSequence sequence){ + this.sequence = sequence; + } + + @Override + public void execute() { + if (sequence != null){ + new RunSingleSequenceNode(sequence); + sequence = sequence.getNext(); + } + } + + @Override + public boolean isFinished() { + return sequence == null; + } + +} From de4881b3590bae867254353dc47d5f6a2cf29f4e Mon Sep 17 00:00:00 2001 From: NoamRosenberg08 Date: Sat, 10 Jun 2023 21:00:55 +0300 Subject: [PATCH 03/10] added sections and changed disabled way of showing errors --- .../edu/greenblitz/tobyDetermined/Robot.java | 26 +++++++++++-------- .../greenblitz/tobyDetermined/RobotMap.java | 8 ++++-- .../tobyDetermined/subsystems/LED.java | 16 ++++++++---- 3 files changed, 32 insertions(+), 18 deletions(-) diff --git a/src/main/java/edu/greenblitz/tobyDetermined/Robot.java b/src/main/java/edu/greenblitz/tobyDetermined/Robot.java index 0cf735da..1bd4cc8b 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/Robot.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/Robot.java @@ -45,6 +45,7 @@ public void robotInit() { SwerveChassis.getInstance().resetChassisPose(); SwerveChassis.getInstance().resetAllEncoders(); // SwerveChassis.getInstance().resetEncodersByCalibrationRod(); + LED.getInstance(); } @@ -84,6 +85,7 @@ public void robotPeriodic() { @Override public void disabledInit() { CommandScheduler.getInstance().cancelAll(); + LED.getInstance().turnOff(); } @@ -150,24 +152,26 @@ public void disabledPeriodic() { // }else{ // LED.getInstance().setColor(Color.kGreen); // } - if (SwerveChassis.getInstance().isEncoderBroken()){ - if (Extender.getInstance().DidReset()){ - LED.getInstance().setColor(new Color(136, 8 ,90)); //dark red - } else { - LED.getInstance().setColor(Color.kRed); - } - - } else if (!Extender.getInstance().DidReset()){ - LED.getInstance().setColor(Color.kOrangeRed); - } else { - LED.getInstance().setColor(Color.kGreen); + + if(Extender.getInstance().DidReset()){ + LED.getInstance().setColor(RobotMap.LED.Sections.ARM_ENCODER_BROKEN,Color.kGreen); + }else{ + LED.getInstance().setColor(RobotMap.LED.Sections.ARM_ENCODER_BROKEN,Color.kRed); } + + if(SwerveChassis.getInstance().isEncoderBroken()){ + LED.getInstance().setColor(RobotMap.LED.Sections.SWERVE_ENCODER_BROKEN,Color.kRed); + }else{ + LED.getInstance().setColor(RobotMap.LED.Sections.ARM_ENCODER_BROKEN,Color.kGreen); + } + if(Extender.getInstance().getLimitSwitch()){ if (Extender.getInstance().getLength() > 0 || !Extender.getInstance().DidReset()) { Extender.getInstance().resetLength(); } } + SwerveChassis.getInstance().isEncoderBroken(); Elbow.getInstance().resetEncoder(); } diff --git a/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java b/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java index 980954e1..7c70f311 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java @@ -77,9 +77,13 @@ public static class LED { public enum Sections{ + //sections + ARM_ENCODER_BROKEN (0,9), + SWERVE_ENCODER_BROKEN (11,20), + + + - normal(1,10), - errors(11,100), all(0, LENGTH); public int start; diff --git a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java index 72ef75ce..55359b48 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java @@ -31,10 +31,9 @@ public static void init(){ } } - public void setColor(Color color) { + public void setColor(Color color) { //to all for (int i = 0; i < this.ledBuffer.getLength(); i++) { - this.ledBuffer.setLED(i, color); - SmartDashboard.putNumber("led num", i); + setColor(i,color); } this.addressableLED.setData(ledBuffer); } @@ -49,6 +48,10 @@ public void setColor (int startIndex,int endIndex, Color color){ } } + public void setColor (RobotMap.LED.Sections section, Color color){ + setColor(section.start,section.end,color); + } + public void turnOff (){ setColor(new Color(0,0,0)); } @@ -56,11 +59,14 @@ public void turnOff (){ public void turnOff (int index){ setColor(index,new Color(0,0,0)); } - public void turnoff (int startIndex,int endIndex){ + public void turnOff (int startIndex,int endIndex){ for (int i = startIndex; i < endIndex; i++) { - setColor(i,new Color(0,0,0)); + turnOff(i); } } + public void turnOff (RobotMap.LED.Sections section){ + turnOff(section.start,section.end); + } From 32d87ffd3c6e33ed4a9c250f2f42718396d5a292 Mon Sep 17 00:00:00 2001 From: Raz Date: Wed, 14 Jun 2023 20:18:58 +0300 Subject: [PATCH 04/10] noam - removed sequance cuz no need for it --- .../commands/LED/BetterLED/RunSequence.java | 28 ------ .../LED/BetterLED/RunSingleSequenceNode.java | 24 ----- .../LED/BetterLED/SimpleSetColor.java | 27 ------ .../edu/greenblitz/utils/LEDSequence.java | 92 ------------------- 4 files changed, 171 deletions(-) delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSequence.java delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSingleSequenceNode.java delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/SimpleSetColor.java delete mode 100644 src/main/java/edu/greenblitz/utils/LEDSequence.java diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSequence.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSequence.java deleted file mode 100644 index 8b4018e7..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSequence.java +++ /dev/null @@ -1,28 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED.BetterLED; - -import edu.greenblitz.tobyDetermined.RobotMap; -import edu.greenblitz.utils.GBCommand; -import edu.greenblitz.utils.LEDSequence; - -public class RunSequence extends GBCommand { - - private LEDSequence sequence; - - public RunSequence (LEDSequence sequence){ - this.sequence = sequence; - } - - @Override - public void execute() { - if (sequence != null){ - new RunSingleSequenceNode(sequence); - sequence = sequence.getNext(); - } - } - - @Override - public boolean isFinished() { - return sequence == null; - } - -} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSingleSequenceNode.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSingleSequenceNode.java deleted file mode 100644 index b946f35b..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/RunSingleSequenceNode.java +++ /dev/null @@ -1,24 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED.BetterLED; - -import edu.greenblitz.tobyDetermined.commands.LED.LEDCommand; -import edu.greenblitz.utils.InterruptableWaitCommand; -import edu.greenblitz.utils.LEDSequence; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -public class RunSingleSequenceNode extends SequentialCommandGroup { - - private LEDSequence sequence; - public RunSingleSequenceNode(LEDSequence sequence){ - this.sequence = sequence; - addCommands( - new InterruptableWaitCommand(sequence.getWaitBeforeStartTime()), - new SimpleSetColor(sequence.getColor()), - new InterruptableWaitCommand(sequence.getDuration()) - ); - } - - - - -} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/SimpleSetColor.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/SimpleSetColor.java deleted file mode 100644 index e4abd271..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BetterLED/SimpleSetColor.java +++ /dev/null @@ -1,27 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED.BetterLED; - -import edu.greenblitz.tobyDetermined.RobotMap; -import edu.greenblitz.tobyDetermined.commands.LED.LEDCommand; -import edu.wpi.first.wpilibj.util.Color; - -public class SimpleSetColor extends LEDCommand { - - private Color color; - - private int startingIndex; - private int endingIndex; - - public SimpleSetColor (int startingIndex,int endingIndex,Color color){ - led.setColor(startingIndex,endingIndex,color); - } - public SimpleSetColor (Color color){ - led.setColor(startingIndex,endingIndex,color); - } - public SimpleSetColor (RobotMap.LED.Sections section, Color color){ - led.setColor(section.start,section.end,color); - } - - - - -} diff --git a/src/main/java/edu/greenblitz/utils/LEDSequence.java b/src/main/java/edu/greenblitz/utils/LEDSequence.java deleted file mode 100644 index b4e7edc9..00000000 --- a/src/main/java/edu/greenblitz/utils/LEDSequence.java +++ /dev/null @@ -1,92 +0,0 @@ -package edu.greenblitz.utils; - -import edu.greenblitz.tobyDetermined.RobotMap; -import edu.wpi.first.wpilibj.util.Color; - -public class LEDSequence { - - private Color color; - private double duration; - private double waitBeforeStartTime; - - private RobotMap.LED.Sections section; - private LEDSequence next; - - public LEDSequence(Color color, double duration, double waitBeforeStartTime, RobotMap.LED.Sections section, LEDSequence next) { - this.color = color; - this.duration = duration; - this.waitBeforeStartTime = waitBeforeStartTime; - this.section = section; - this.next = next; - } - - public LEDSequence(Color color) { - this.color = color; - this.duration = 0; - this.waitBeforeStartTime = 0; - this.section = RobotMap.LED.Sections.all; - this.next = null; - } - - //with: - - public LEDSequence withDuration (double duration){ - this.duration = duration; - return this; - } - public LEDSequence withBeforeStartTime (double time){ - this.duration = time; - return this; - } - public LEDSequence withNext (LEDSequence next){ - this.next = next; - return this; - } - - - public LEDSequence setSection (RobotMap.LED.Sections section){ - this.section = section; - return this; - } - public LEDSequence setColor (Color color){ - this.color = color; - return this; - } - public LEDSequence setDuration (double duration){ - this.duration = duration; - return this; - } - public LEDSequence setBeforeStartTime (double time){ - this.waitBeforeStartTime = time; - return this; - } - - - public Color getColor() { - return color; - } - - public double getDuration() { - return duration; - } - - public double getWaitBeforeStartTime() { - return waitBeforeStartTime; - } - - public void setWaitBeforeStartTime(double waitBeforeStartTime) { - this.waitBeforeStartTime = waitBeforeStartTime; - } - - public RobotMap.LED.Sections getSection() { - return section; - } - - public LEDSequence getNext() { - return next; - } - - public void setNext(LEDSequence next) { - this.next = next; - } -} From 89fb728bbd3898bb1b27bdb47c756a85e2f51216 Mon Sep 17 00:00:00 2001 From: NoamRosenberg08 Date: Sat, 24 Jun 2023 01:32:56 +0300 Subject: [PATCH 05/10] noam - started working on cr --- src/main/java/edu/greenblitz/tobyDetermined/Robot.java | 6 +++--- .../edu/greenblitz/tobyDetermined/subsystems/LED.java | 9 +++++---- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/edu/greenblitz/tobyDetermined/Robot.java b/src/main/java/edu/greenblitz/tobyDetermined/Robot.java index 1bd4cc8b..5167965e 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/Robot.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/Robot.java @@ -45,8 +45,8 @@ public void robotInit() { SwerveChassis.getInstance().resetChassisPose(); SwerveChassis.getInstance().resetAllEncoders(); // SwerveChassis.getInstance().resetEncodersByCalibrationRod(); - LED.getInstance(); - + LED.getInstance(); //todo might make error + } @Override @@ -162,7 +162,7 @@ public void disabledPeriodic() { if(SwerveChassis.getInstance().isEncoderBroken()){ LED.getInstance().setColor(RobotMap.LED.Sections.SWERVE_ENCODER_BROKEN,Color.kRed); }else{ - LED.getInstance().setColor(RobotMap.LED.Sections.ARM_ENCODER_BROKEN,Color.kGreen); + LED.getInstance().setColor(RobotMap.LED.Sections.SWERVE_ENCODER_BROKEN,Color.kGreen); } diff --git a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java index 55359b48..f78a9013 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.WaitCommand; public class LED extends GBSubsystem { @@ -35,11 +36,9 @@ public void setColor(Color color) { //to all for (int i = 0; i < this.ledBuffer.getLength(); i++) { setColor(i,color); } - this.addressableLED.setData(ledBuffer); } public void setColor(int i, Color color) { this.ledBuffer.setLED(i, color); - this.addressableLED.setData(ledBuffer); } public void setColor (int startIndex,int endIndex, Color color){ @@ -74,13 +73,15 @@ public void setHSV(int h, int s, int v) { for (int i = 0; i < this.ledBuffer.getLength(); i++) { this.ledBuffer.setHSV(i, h, s, v); } - this.addressableLED.setData(ledBuffer); } public void setHSV(int index, int h, int s, int v) { this.ledBuffer.setHSV(index, h, s, v); - this.addressableLED.setData(ledBuffer); } + @Override + public void periodic() { + this.addressableLED.setData(ledBuffer); + } } \ No newline at end of file From 07f63d8b0a09c3f6713c35a0325413eb193b6fee Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Mon, 26 Jun 2023 10:57:30 +0300 Subject: [PATCH 06/10] noam - did cr --- .../edu/greenblitz/tobyDetermined/Robot.java | 4 +- .../greenblitz/tobyDetermined/RobotMap.java | 718 +++++++++--------- .../commands/LED/BalanceOnRampLED.java | 12 - .../commands/LED/DrivingSidewaysLED.java | 11 - .../commands/LED/EncoderBrokenLED.java | 10 - .../LED/HumanPlayerObjectIndicator.java | 42 - .../commands/LED/LEDCommand.java | 15 - .../commands/LED/LedBlinking.java | 22 - .../commands/LED/ObjectInClawLED.java | 13 - .../commands/LED/ObjectInIntakeLED.java | 14 - .../commands/LED/SetLEDColor.java | 22 - .../rotateAutomation/FullAlign.java | 4 +- .../telescopicArm/extender/ResetExtender.java | 2 +- .../tobyDetermined/subsystems/LED.java | 9 +- .../telescopicArm/ObjectSelector.java | 5 +- 15 files changed, 356 insertions(+), 547 deletions(-) delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BalanceOnRampLED.java delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/DrivingSidewaysLED.java delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/EncoderBrokenLED.java delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/HumanPlayerObjectIndicator.java delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/LEDCommand.java delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/LedBlinking.java delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/ObjectInClawLED.java delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/ObjectInIntakeLED.java delete mode 100644 src/main/java/edu/greenblitz/tobyDetermined/commands/LED/SetLEDColor.java diff --git a/src/main/java/edu/greenblitz/tobyDetermined/Robot.java b/src/main/java/edu/greenblitz/tobyDetermined/Robot.java index 5167965e..4a15e535 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/Robot.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/Robot.java @@ -45,7 +45,6 @@ public void robotInit() { SwerveChassis.getInstance().resetChassisPose(); SwerveChassis.getInstance().resetAllEncoders(); // SwerveChassis.getInstance().resetEncodersByCalibrationRod(); - LED.getInstance(); //todo might make error } @@ -66,6 +65,9 @@ private static void initSubsystems() { IntakeExtender.init(); IntakeRoller.init(); OI.init(); + +// LED.getInstance(); //todo might make error + LED.init(); //todo might make error } private static void initPortForwarding() { diff --git a/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java b/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java index 7c70f311..42cdbf01 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java @@ -5,7 +5,6 @@ import com.pathplanner.lib.auto.PIDConstants; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMaxLimitSwitch; -import edu.greenblitz.tobyDetermined.subsystems.LED; import edu.greenblitz.tobyDetermined.subsystems.swerve.KazaSwerveModule; import edu.greenblitz.tobyDetermined.subsystems.swerve.SdsSwerveModule; import edu.greenblitz.utils.PIDObject; @@ -19,382 +18,359 @@ import static edu.greenblitz.tobyDetermined.RobotMap.TelescopicArm.Elbow.STARTING_ANGLE_RELATIVE_TO_GROUND; public class RobotMap { - public static final Robot.robotName ROBOT_NAME = Robot.robotName.Frankenstein; + public static final Robot.robotName ROBOT_NAME = Robot.robotName.Frankenstein; - public static class General { + public static class General { + + public final static double MIN_VOLTAGE_BATTERY = 11.97; + public final static double RAMP_RATE_VAL = 0.4; + + public static class Motors { + + public final static double SPARKMAX_TICKS_PER_RADIAN = Math.PI * 2; + public final static double SPARKMAX_VELOCITY_UNITS_PER_RPM = 1; + public static final double NEO_PHYSICAL_TICKS_TO_RADIANS = SPARKMAX_TICKS_PER_RADIAN / 42; //do not use unless you understand the meaning + public final static double FALCON_TICKS_PER_RADIAN = 2 * Math.PI / 2048.0; - public final static double MIN_VOLTAGE_BATTERY = 11.97; - public final static double RAMP_RATE_VAL = 0.4; + public final static double FALCON_VELOCITY_UNITS_PER_RPM = 600.0 / 2048; + } + } - public static class Motors { - public final static double SPARKMAX_TICKS_PER_RADIAN = Math.PI * 2; - public final static double SPARKMAX_VELOCITY_UNITS_PER_RPM = 1; - public static final double NEO_PHYSICAL_TICKS_TO_RADIANS = SPARKMAX_TICKS_PER_RADIAN / 42; //do not use unless you understand the meaning - public final static double FALCON_TICKS_PER_RADIAN = 2 * Math.PI / 2048.0; + public static class gyro { //Yum + public static final int pigeonID = 12; + } - public final static double FALCON_VELOCITY_UNITS_PER_RPM = 600.0 / 2048; - } - } + public static class Joystick { + public static final int MAIN = 0; + public static final int SECOND = 1; + } + public static class Pneumatics { + public static class PCM { + public static final int PCM_ID = 22; + public static final PneumaticsModuleType PCM_TYPE = PneumaticsModuleType.CTREPCM; + } - public static class gyro { //Yum - public static final int pigeonID = 12; - } + public static class PressureSensor { + public static final int PRESSURE = 3; + } + } - public static class Joystick { - public static final int MAIN = 0; - public static final int SECOND = 1; - } - - public static class Pneumatics { - public static class PCM { - public static final int PCM_ID = 22; - public static final PneumaticsModuleType PCM_TYPE = PneumaticsModuleType.CTREPCM; - } - - public static class PressureSensor { - public static final int PRESSURE = 3; - } - } - - public static class Vision { - public static final String[] LIMELIGHT_NAMES = new String[]{"limelight-front"}; - public static double STANDARD_DEVIATION_ODOMETRY = 0.001; - public static double STANDARD_DEVIATION_VISION2D = 0.3; - public static double STANDARD_DEVIATION_VISION_ANGLE = 0.6; - public static final int[] PORT_NUMBERS = {5800, 5801, 5802, 5803, 5804, 5805}; - public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(new Translation3d(), new Rotation3d()); - public final static double MIN_DISTANCE_TO_FILTER_OUT = 1; - public final static double VISION_CONSTANT = 0.2; - - - } - - public static class LED { - - - public enum Sections{ - - - //sections - ARM_ENCODER_BROKEN (0,9), - SWERVE_ENCODER_BROKEN (11,20), - - - - - all(0, LENGTH); - - public int start; - public int end; - Sections(int startIndex, int endIndex) { - end = endIndex; - start = startIndex; - } - } - - public static final int LENGTH = 100; - public static final int PORT = 0; - public static final double BLINKING_TIME = 0.2; - } - - public static class Ultrasonic { - public static final int PING_DIO_PORT = 5; - public static final int ECHO_DIO_PORT = 6; - public static final double DISTANCE_FROM_FLOOR_TO_STOP_IN_MM = 120; - } - - public static class Swerve { - - public static class Frankenstein { - - - public static final double ROBOT_LENGTH_IN_METERS = 0.69; - public static final double ROBOT_WIDTH_IN_METERS = 0.79; - public static final double BUMPER_WIDTH = 0.08; - - public static final double MAX_VELOCITY = 2; - public static final double MAX_ACCELERATION = 1.75; - public static final double MAX_ANGULAR_SPEED = 8; - public static final double MAX_ANGULAR_ACCELERATION = 20; //todo calibrate - public static final int LAMPREY_AVERAGE_BITS = 2; - - - public static final PIDConstants TRANSLATION_PID = new PIDConstants(2, 0, 0); - - public static final PIDConstants ROTATION_PID = new PIDConstants(2, 0, 0); - - public static final Translation2d[] SwerveLocationsInSwerveKinematicsCoordinates = new Translation2d[]{ - //the WPILib coordinate system is stupid. (x is forwards, y is leftwards) - //the translations are given rotated by 90 degrees clockwise to avoid coordinates system conversion at output - new Translation2d(0.3020647, 0.25265), /*fl*/ - new Translation2d(0.3020647, -0.25265),/*fr*/ - new Translation2d(-0.3020647, 0.25265),/*bl*/ - new Translation2d(-0.3020647, -0.25265)/*br*/ - }; - } - - - static final Pose2d initialRobotPosition = new Pose2d(0, 0, new Rotation2d(0)); - public static final Translation2d[] SwerveLocationsInSwerveKinematicsCoordinates = new Translation2d[]{ - //the WPILib coordinate system is stupid. (x is forwards, y is leftwards) - //the translations are given rotated by 90 degrees clockwise to avoid coordinates system conversion at output - new Translation2d(0.3020647, 0.25265), /*fl*/ - new Translation2d(0.3020647, -0.25265),/*fr*/ - new Translation2d(-0.3020647, 0.25265),/*bl*/ - new Translation2d(-0.3020647, -0.25265)/*br*/}; - - public static final double MAX_VELOCITY = 4.1818320981472068; - public static final double MAX_ACCELERATION = 14.37979171376739; - public static final double MAX_ANGULAR_SPEED = 10.454580245368017; - - - public static final PIDObject rotationPID = new PIDObject().withKp(0.5).withKi(0).withKd(0).withFF(0.1); - public static final double ks = 0.14876; - public static final double kv = 3.3055; - - public static final double ka = 0.11023; - - public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleFrontLeft = new KazaSwerveModule.KazaSwerveModuleConfigObject(1, 10, 0, false); //front left - - - public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleFrontRight = new KazaSwerveModule.KazaSwerveModuleConfigObject(3, 11, 2, true); //front right - - public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleBackLeft = new KazaSwerveModule.KazaSwerveModuleConfigObject(2, 8, 1, false); //back left - - public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleBackRight = new KazaSwerveModule.KazaSwerveModuleConfigObject(12, 5, 3, true); //back right - - public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleFrontLeft = new SdsSwerveModule.SdsSwerveModuleConfigObject(1, 0, 1, 0.8486328125,false); //front left - - public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleFrontRight = new SdsSwerveModule.SdsSwerveModuleConfigObject(3, 2, 2,0.2939453125 ,true); //front right - - public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleBackLeft = new SdsSwerveModule.SdsSwerveModuleConfigObject(5, 4, 3, 0.5524,false); //back left - - public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleBackRight = new SdsSwerveModule.SdsSwerveModuleConfigObject(7, 6, 4, 0.8718,true); //back right - - - public static class KazaSwerve { - public static final double ANG_GEAR_RATIO = 6.0; - public static final double LIN_GEAR_RATIO = 8.0; - - public static final double ks = 0.14876; - public static final double kv = 3.3055; - public static final double ka = 0.11023; - - public static final double WHEEL_CIRC = 0.0517 * 2 * Math.PI; - public static final double linTicksToMeters = RobotMap.General.Motors.SPARKMAX_TICKS_PER_RADIAN * WHEEL_CIRC / LIN_GEAR_RATIO / (2 * Math.PI); - public static final double angleTicksToWheelToRPM = RobotMap.General.Motors.SPARKMAX_VELOCITY_UNITS_PER_RPM / ANG_GEAR_RATIO; - public static final double linTicksToMetersPerSecond = RobotMap.General.Motors.SPARKMAX_VELOCITY_UNITS_PER_RPM * WHEEL_CIRC / 60 / LIN_GEAR_RATIO; - public static final PIDObject linPID = new PIDObject().withKp(0.0003).withMaxPower(0.5); - public static final GBSparkMax.SparkMaxConfObject baseLinConfObj = new GBSparkMax.SparkMaxConfObject().withIdleMode(CANSparkMax.IdleMode.kBrake).withCurrentLimit(40).withRampRate(RobotMap.General.RAMP_RATE_VAL).withPID(linPID).withPositionConversionFactor(linTicksToMeters).withVelocityConversionFactor(linTicksToMetersPerSecond); - public static final double angleTicksToRadians = RobotMap.General.Motors.SPARKMAX_TICKS_PER_RADIAN / ANG_GEAR_RATIO; - public static final PIDObject angPID = new PIDObject().withKp(0.5).withMaxPower(1.0); - public static final GBSparkMax.SparkMaxConfObject baseAngConfObj = new GBSparkMax.SparkMaxConfObject().withIdleMode(CANSparkMax.IdleMode.kBrake).withCurrentLimit(30).withRampRate(RobotMap.General.RAMP_RATE_VAL).withInverted(true).withPID(angPID).withPositionConversionFactor(angleTicksToRadians).withVelocityConversionFactor(angleTicksToWheelToRPM); - } - - public static class SdsSwerve { - public static final double ANG_GEAR_RATIO = (150.0 / 7); - public static final double LIN_GEAR_RATIO = 8.14; - - public static final double ks = 0.16411; - public static final double kv = 2.6824; - public static final double ka = 0.25968; - - public static final double WHEEL_CIRC = 0.0517 * 2 * Math.PI; - public static final double linTicksToMeters = RobotMap.General.Motors.FALCON_TICKS_PER_RADIAN * WHEEL_CIRC / 2 / Math.PI / LIN_GEAR_RATIO; - public static final double angleTicksToWheelToRPM = RobotMap.General.Motors.FALCON_VELOCITY_UNITS_PER_RPM / ANG_GEAR_RATIO; - public static final double linTicksToMetersPerSecond = RobotMap.General.Motors.FALCON_VELOCITY_UNITS_PER_RPM / LIN_GEAR_RATIO * WHEEL_CIRC / 60; - public static final double angleTicksToRadians = RobotMap.General.Motors.FALCON_TICKS_PER_RADIAN / ANG_GEAR_RATIO; - public static final double magEncoderTicksToFalconTicks = 2 * Math.PI / angleTicksToRadians; - - public static final PIDObject angPID = new PIDObject().withKp(0.05).withMaxPower(1.0).withFF(0);//.withKd(10).withMaxPower(0.8); - public static final GBFalcon.FalconConfObject baseAngConfObj = new GBFalcon.FalconConfObject().withNeutralMode(NeutralMode.Brake).withCurrentLimit(30).withRampRate(RobotMap.General.RAMP_RATE_VAL).withInverted(true).withPID(angPID); - - public static final PIDObject linPID = new PIDObject().withKp(0.0003).withMaxPower(0.5); - public static final GBFalcon.FalconConfObject baseLinConfObj = new GBFalcon.FalconConfObject().withNeutralMode(NeutralMode.Brake).withCurrentLimit(40).withRampRate(RobotMap.General.RAMP_RATE_VAL).withPID(linPID); - } - - public static class Autonomus { - public static final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(MAX_VELOCITY, MAX_ACCELERATION); - } - - public static class Balance { - public static final double GET_ON_SPEED_FACTOR = 0.9; - public static final double GET_TO_FLOOR_SPEED_FACTOR = 0.5; - public static final double BANG_BANG_SPEED_FACTOR = 0.5; - } - } - - public static class RotatingBelly { - public static final int MOTOR_ID = 15; - public static final double ROTATING_TIME = 0.8; - public static final double ROTATING_POWER = 0.8; - public static final int MACRO_SWITCH_DIO_PORT = 7; - - public static final double ROTATE_FROM_SWITCH_TO_STOP_TIME = 0.2; - public static final double ROTATE_FROM_STOP_TO_SWITCH_TIME = 0.4; - public static double ROTATE_TO_DOOR_TIME = 3; - - public static final int CLOSE_PISTON_ID = 3; - public static final int OPEN_PISTON_ID = 7; - - } - - public static class TelescopicArm { - - - public enum PresetPositions { - //height in meters - //input angle degrees output angle radians - CONE_HIGH(0.71, Math.toRadians(25.1) - STARTING_ANGLE_RELATIVE_TO_GROUND), // originally 0.775 - CONE_MID(0.31, /*1.94*/ Math.toRadians(107)), - CUBE_HIGH(0.450, Math.toRadians(15.46) - STARTING_ANGLE_RELATIVE_TO_GROUND), - CUBE_MID(0.29, 1.85), - LOW(0.35, Math.toRadians(60)), - - POST_CONE_DROP(0.089,0.1), - PRE_CONE_DROP(0.089,0.667), - - COMMUNITY_PRE_GRID(Extender.MAX_ENTRANCE_LENGTH - Extender.LENGTH_TOLERANCE, CONE_HIGH.angleInRadians), - - INTAKE_GRAB_CONE_POSITION(0.34, 0.12), - INTAKE_GRAB_CUBE_POSITION(0.25, INTAKE_GRAB_CONE_POSITION.angleInRadians), - PRE_INTAKE_GRAB_POSITION(0.02,0.35), //0.28), - - ZIG_HAIL(0, Math.toRadians(20.7) - STARTING_ANGLE_RELATIVE_TO_GROUND), - FEEDER(0.663, 1.949); - - - public double distance; - public double angleInRadians; - - PresetPositions(double distance, double angleInRads) { - this.distance = distance; - this.angleInRadians = angleInRads; - - } - } - - public static class Extender { - public static final int MOTOR_ID = 2; - public static final double GEAR_RATIO = 1 / 5.0; - public static final double STARTING_LENGTH = 0.3; - public static final int BACKWARDS_LIMIT = 0; - public static final double FORWARD_LIMIT = 0.78; - public static final double EXTENDED_LENGTH = FORWARD_LIMIT - 0.05; - public static final SparkMaxLimitSwitch.Type SWITCH_TYPE = SparkMaxLimitSwitch.Type.kNormallyClosed; - public static final double MAX_LENGTH_IN_ROBOT = 0.37; - public static double MAX_ENTRANCE_LENGTH = 0.054; - public static final PIDObject PID = new PIDObject().withKp(60).withKd(2).withMaxPower(1); - public static final double SETPOINT_D = 10; - public static final double DEBOUNCE_TIME_FOR_LIMIT_SWITCH = 0.05; - - - public static final double EXTENDER_EXTENDING_GEAR_CIRC = 0.0165 * (2 * Math.PI); - public static final double POSITION_CONVERSION_FACTOR = GEAR_RATIO * EXTENDER_EXTENDING_GEAR_CIRC; - public static final double VELOCITY_CONVERSION_FACTOR = POSITION_CONVERSION_FACTOR / 60; - public static final double LENGTH_TOLERANCE = 0.045; //in meters - public static final double FORWARDS_LENGTH_TOLERANCE = 0.01; //in meters - public static final double VELOCITY_TOLERANCE = 0.02; - - public static final double kS = 0.3; - public static final double kG = 0.67 - kS; - - public static final double MAX_ACCELERATION = 3.5; //4.2 - public static final double MAX_VELOCITY = 1.75; - public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_VELOCITY, MAX_ACCELERATION); - - public static final GBSparkMax.SparkMaxConfObject EXTENDER_CONFIG_OBJECT = new GBSparkMax.SparkMaxConfObject() - .withPID(RobotMap.TelescopicArm.Extender.PID) - .withPositionConversionFactor(RobotMap.TelescopicArm.Extender.POSITION_CONVERSION_FACTOR) - .withVelocityConversionFactor(RobotMap.TelescopicArm.Extender.VELOCITY_CONVERSION_FACTOR) - .withIdleMode(CANSparkMax.IdleMode.kBrake) - .withRampRate(General.RAMP_RATE_VAL) - .withCurrentLimit(40); - } - - public static class Claw { - public static final int MOTOR_ID = 3; - public static final int SOLENOID_OPEN_CLAW_ID = 6; - public static final int SOLENOID_CLOSED_CLAW_ID = 2; - public static final double MOTOR_POWER_GRIP = 0.3; - - public static final double MOTOR_POWER_CONE = 0.6; - public static final double MOTOR_POWER_RELEASE = -0.15; - - public static final double TIME_OF_GRIP_CONSTANT = 2; - - public static final GBSparkMax.SparkMaxConfObject CLAW_CONFIG_OBJECT = new GBSparkMax.SparkMaxConfObject() - .withPID(RobotMap.TelescopicArm.Extender.PID) - .withPositionConversionFactor(RobotMap.TelescopicArm.Extender.POSITION_CONVERSION_FACTOR) - .withVelocityConversionFactor(RobotMap.TelescopicArm.Extender.VELOCITY_CONVERSION_FACTOR) - .withIdleMode(CANSparkMax.IdleMode.kBrake) - .withRampRate(General.RAMP_RATE_VAL) - .withCurrentLimit(30) - .withInverted(true); - } - - public static class Elbow { - public static final int MOTOR_ID = 1; - public static final int RESET_MEDIAN_SIZE = 7; - public static final double GEAR_RATIO = 1; - - public static final double kS = 0.133000000000002; - public static final double MIN_Kg = 0.155; - public static final double MAX_Kg = 0.62; - public static final double MAX_KG_MEASUREMENT_LENGTH = 0.822964668273926; - public static final double STARTING_ANGLE_RELATIVE_TO_GROUND = -1.765; //this is most easily measured using the encoder, so it is already radians - public static final double MAX_ACCELERATION = 10; - public static final double MAX_VELOCITY = 4; - public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_VELOCITY, MAX_ACCELERATION); - public static final PIDObject PID = new PIDObject().withKp(4).withKd(0.4).withMaxPower(1); - public static final double STARTING_WALL_ZONE_ANGLE = Units.degreesToRadians(11); - public static final double END_WALL_ZONE_ANGLE = Units.degreesToRadians(35.5); - - - public static final double ABSOLUTE_POSITION_CONVERSION_FACTOR = General.Motors.SPARKMAX_TICKS_PER_RADIAN / GEAR_RATIO; - public static final double ABSOLUTE_VELOCITY_CONVERSION_FACTOR = ABSOLUTE_POSITION_CONVERSION_FACTOR; - public static final double RELATIVE_POSITION_CONVERSION_FACTOR = 0.0328; //you know of calibrating pid but have you heard of calibrating the gear ratio - public static final double RELATIVE_VELOCITY_CONVERSION_FACTOR = 0.0328/ 60; - public static final double FORWARD_ANGLE_LIMIT = 2.13 + Math.toRadians(10); - public static final double BACKWARD_ANGLE_LIMIT = Units.degreesToRadians(4); - - public static final double ANGLE_TOLERANCE = Units.degreesToRadians(3.5); - public static final double ANGULAR_VELOCITY_TOLERANCE = Units.degreesToRadians(3); - - - public static final GBSparkMax.SparkMaxConfObject ELBOW_CONFIG_OBJECT = new GBSparkMax.SparkMaxConfObject() - .withPID(PID) - .withIdleMode(CANSparkMax.IdleMode.kBrake) - .withRampRate(General.RAMP_RATE_VAL) - .withCurrentLimit(RobotMap.TelescopicArm.Elbow.CURRENT_LIMIT); - - public static final int CURRENT_LIMIT = 40; - - } - } - - public static class ThirdLeg { - public static final int FORWARD_PORT = 2; - public static final int REVERSE_PORT = 3; - } - - public static class Intake { - public static final int ROLLER_ID = 4; - public static final double DEFAULT_POWER = 1; - public static final int BEAM_BREAKER_ID = 20; - public static final GBSparkMax.SparkMaxConfObject INTAKE_CONFIG_OBJECT = new GBSparkMax.SparkMaxConfObject() - .withPID(RobotMap.TelescopicArm.Extender.PID) - .withPositionConversionFactor(RobotMap.TelescopicArm.Extender.POSITION_CONVERSION_FACTOR) - .withVelocityConversionFactor(RobotMap.TelescopicArm.Extender.VELOCITY_CONVERSION_FACTOR) - .withIdleMode(CANSparkMax.IdleMode.kBrake) - .withRampRate(General.RAMP_RATE_VAL) - .withCurrentLimit(30) - .withInverted(true); - public static class Solenoid { - public static final int FORWARD_PORT = 4; - public static final int REVERSE_PORT = 0; - } - } + public static class Vision { + public static final String[] LIMELIGHT_NAMES = new String[]{"limelight-front"}; + public static final int[] PORT_NUMBERS = {5800, 5801, 5802, 5803, 5804, 5805}; + public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(new Translation3d(), new Rotation3d()); + public final static double MIN_DISTANCE_TO_FILTER_OUT = 1; + public final static double VISION_CONSTANT = 0.2; + public static double STANDARD_DEVIATION_ODOMETRY = 0.001; + public static double STANDARD_DEVIATION_VISION2D = 0.3; + public static double STANDARD_DEVIATION_VISION_ANGLE = 0.6; + + + } + + public static class LED { + + + public static final int LENGTH = 100; + public static final int PORT = 0; + public static final double BLINKING_TIME = 0.2; + public enum Sections { + + + //sections + ARM_ENCODER_BROKEN(0, 9), + SWERVE_ENCODER_BROKEN(11, 20), + + + ALL(0, LENGTH); + + public int start; + public int end; + + Sections(int startIndex, int endIndex) { + end = endIndex; + start = startIndex; + } + } + } + + public static class Ultrasonic { + public static final int PING_DIO_PORT = 5; + public static final int ECHO_DIO_PORT = 6; + public static final double DISTANCE_FROM_FLOOR_TO_STOP_IN_MM = 120; + } + + public static class Swerve { + + public static final Translation2d[] SwerveLocationsInSwerveKinematicsCoordinates = new Translation2d[]{ + //the WPILib coordinate system is stupid. (x is forwards, y is leftwards) + //the translations are given rotated by 90 degrees clockwise to avoid coordinates system conversion at output + new Translation2d(0.3020647, 0.25265), /*fl*/ + new Translation2d(0.3020647, -0.25265),/*fr*/ + new Translation2d(-0.3020647, 0.25265),/*bl*/ + new Translation2d(-0.3020647, -0.25265)/*br*/}; + public static final double MAX_VELOCITY = 4.1818320981472068; + public static final double MAX_ACCELERATION = 14.37979171376739; + public static final double MAX_ANGULAR_SPEED = 10.454580245368017; + public static final PIDObject rotationPID = new PIDObject().withKp(0.5).withKi(0).withKd(0).withFF(0.1); + public static final double ks = 0.14876; + public static final double kv = 3.3055; + public static final double ka = 0.11023; + static final Pose2d initialRobotPosition = new Pose2d(0, 0, new Rotation2d(0)); + public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleFrontLeft = new KazaSwerveModule.KazaSwerveModuleConfigObject(1, 10, 0, false); //front left + public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleFrontRight = new KazaSwerveModule.KazaSwerveModuleConfigObject(3, 11, 2, true); //front right + public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleBackLeft = new KazaSwerveModule.KazaSwerveModuleConfigObject(2, 8, 1, false); //back left + public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleBackRight = new KazaSwerveModule.KazaSwerveModuleConfigObject(12, 5, 3, true); //back right + public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleFrontLeft = new SdsSwerveModule.SdsSwerveModuleConfigObject(1, 0, 1, 0.8486328125, false); //front left + public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleFrontRight = new SdsSwerveModule.SdsSwerveModuleConfigObject(3, 2, 2, 0.2939453125, true); //front right + public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleBackLeft = new SdsSwerveModule.SdsSwerveModuleConfigObject(5, 4, 3, 0.5524, false); //back left + public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleBackRight = new SdsSwerveModule.SdsSwerveModuleConfigObject(7, 6, 4, 0.8718, true); //back right + + public static class Frankenstein { + + + public static final double ROBOT_LENGTH_IN_METERS = 0.69; + public static final double ROBOT_WIDTH_IN_METERS = 0.79; + public static final double BUMPER_WIDTH = 0.08; + + public static final double MAX_VELOCITY = 2; + public static final double MAX_ACCELERATION = 1.75; + public static final double MAX_ANGULAR_SPEED = 8; + public static final double MAX_ANGULAR_ACCELERATION = 20; //todo calibrate + public static final int LAMPREY_AVERAGE_BITS = 2; + + + public static final PIDConstants TRANSLATION_PID = new PIDConstants(2, 0, 0); + + public static final PIDConstants ROTATION_PID = new PIDConstants(2, 0, 0); + + public static final Translation2d[] SwerveLocationsInSwerveKinematicsCoordinates = new Translation2d[]{ + //the WPILib coordinate system is stupid. (x is forwards, y is leftwards) + //the translations are given rotated by 90 degrees clockwise to avoid coordinates system conversion at output + new Translation2d(0.3020647, 0.25265), /*fl*/ + new Translation2d(0.3020647, -0.25265),/*fr*/ + new Translation2d(-0.3020647, 0.25265),/*bl*/ + new Translation2d(-0.3020647, -0.25265)/*br*/ + }; + } + + public static class KazaSwerve { + public static final double ANG_GEAR_RATIO = 6.0; + public static final double LIN_GEAR_RATIO = 8.0; + + public static final double ks = 0.14876; + public static final double kv = 3.3055; + public static final double ka = 0.11023; + + public static final double WHEEL_CIRC = 0.0517 * 2 * Math.PI; + public static final double linTicksToMeters = RobotMap.General.Motors.SPARKMAX_TICKS_PER_RADIAN * WHEEL_CIRC / LIN_GEAR_RATIO / (2 * Math.PI); + public static final double angleTicksToWheelToRPM = RobotMap.General.Motors.SPARKMAX_VELOCITY_UNITS_PER_RPM / ANG_GEAR_RATIO; + public static final double linTicksToMetersPerSecond = RobotMap.General.Motors.SPARKMAX_VELOCITY_UNITS_PER_RPM * WHEEL_CIRC / 60 / LIN_GEAR_RATIO; + public static final GBSparkMax.SparkMaxConfObject baseLinConfObj = new GBSparkMax.SparkMaxConfObject().withIdleMode(CANSparkMax.IdleMode.kBrake).withCurrentLimit(40).withRampRate(RobotMap.General.RAMP_RATE_VAL).withPID(linPID).withPositionConversionFactor(linTicksToMeters).withVelocityConversionFactor(linTicksToMetersPerSecond); + public static final PIDObject linPID = new PIDObject().withKp(0.0003).withMaxPower(0.5); + public static final double angleTicksToRadians = RobotMap.General.Motors.SPARKMAX_TICKS_PER_RADIAN / ANG_GEAR_RATIO; + public static final GBSparkMax.SparkMaxConfObject baseAngConfObj = new GBSparkMax.SparkMaxConfObject().withIdleMode(CANSparkMax.IdleMode.kBrake).withCurrentLimit(30).withRampRate(RobotMap.General.RAMP_RATE_VAL).withInverted(true).withPID(angPID).withPositionConversionFactor(angleTicksToRadians).withVelocityConversionFactor(angleTicksToWheelToRPM); + public static final PIDObject angPID = new PIDObject().withKp(0.5).withMaxPower(1.0); + } + + public static class SdsSwerve { + public static final double ANG_GEAR_RATIO = (150.0 / 7); + public static final double LIN_GEAR_RATIO = 8.14; + + public static final double ks = 0.16411; + public static final double kv = 2.6824; + public static final double ka = 0.25968; + + public static final double WHEEL_CIRC = 0.0517 * 2 * Math.PI; + public static final double linTicksToMeters = RobotMap.General.Motors.FALCON_TICKS_PER_RADIAN * WHEEL_CIRC / 2 / Math.PI / LIN_GEAR_RATIO; + public static final double angleTicksToWheelToRPM = RobotMap.General.Motors.FALCON_VELOCITY_UNITS_PER_RPM / ANG_GEAR_RATIO; + public static final double linTicksToMetersPerSecond = RobotMap.General.Motors.FALCON_VELOCITY_UNITS_PER_RPM / LIN_GEAR_RATIO * WHEEL_CIRC / 60; + public static final double angleTicksToRadians = RobotMap.General.Motors.FALCON_TICKS_PER_RADIAN / ANG_GEAR_RATIO; + public static final double magEncoderTicksToFalconTicks = 2 * Math.PI / angleTicksToRadians; + + public static final PIDObject angPID = new PIDObject().withKp(0.05).withMaxPower(1.0).withFF(0);//.withKd(10).withMaxPower(0.8); + public static final GBFalcon.FalconConfObject baseAngConfObj = new GBFalcon.FalconConfObject().withNeutralMode(NeutralMode.Brake).withCurrentLimit(30).withRampRate(RobotMap.General.RAMP_RATE_VAL).withInverted(true).withPID(angPID); + + public static final PIDObject linPID = new PIDObject().withKp(0.0003).withMaxPower(0.5); + public static final GBFalcon.FalconConfObject baseLinConfObj = new GBFalcon.FalconConfObject().withNeutralMode(NeutralMode.Brake).withCurrentLimit(40).withRampRate(RobotMap.General.RAMP_RATE_VAL).withPID(linPID); + } + + public static class Autonomus { + public static final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(MAX_VELOCITY, MAX_ACCELERATION); + } + + public static class Balance { + public static final double GET_ON_SPEED_FACTOR = 0.9; + public static final double GET_TO_FLOOR_SPEED_FACTOR = 0.5; + public static final double BANG_BANG_SPEED_FACTOR = 0.5; + } + } + + public static class RotatingBelly { + public static final int MOTOR_ID = 15; + public static final double ROTATING_TIME = 0.8; + public static final double ROTATING_POWER = 0.8; + public static final int MACRO_SWITCH_DIO_PORT = 7; + + public static final double ROTATE_FROM_SWITCH_TO_STOP_TIME = 0.2; + public static final double ROTATE_FROM_STOP_TO_SWITCH_TIME = 0.4; + public static final int CLOSE_PISTON_ID = 3; + public static final int OPEN_PISTON_ID = 7; + public static double ROTATE_TO_DOOR_TIME = 3; + + } + + public static class TelescopicArm { + + + public enum PresetPositions { + //height in meters + //input angle degrees output angle radians + CONE_HIGH(0.71, Math.toRadians(25.1) - STARTING_ANGLE_RELATIVE_TO_GROUND), // originally 0.775 + CONE_MID(0.31, /*1.94*/ Math.toRadians(107)), + CUBE_HIGH(0.450, Math.toRadians(15.46) - STARTING_ANGLE_RELATIVE_TO_GROUND), + CUBE_MID(0.29, 1.85), + LOW(0.35, Math.toRadians(60)), + + POST_CONE_DROP(0.089, 0.1), + PRE_CONE_DROP(0.089, 0.667), + + COMMUNITY_PRE_GRID(Extender.MAX_ENTRANCE_LENGTH - Extender.LENGTH_TOLERANCE, CONE_HIGH.angleInRadians), + + INTAKE_GRAB_CONE_POSITION(0.34, 0.12), + INTAKE_GRAB_CUBE_POSITION(0.25, INTAKE_GRAB_CONE_POSITION.angleInRadians), + PRE_INTAKE_GRAB_POSITION(0.02, 0.35), //0.28), + + ZIG_HAIL(0, Math.toRadians(20.7) - STARTING_ANGLE_RELATIVE_TO_GROUND), + FEEDER(0.663, 1.949); + + + public double distance; + public double angleInRadians; + + PresetPositions(double distance, double angleInRads) { + this.distance = distance; + this.angleInRadians = angleInRads; + + } + } + + public static class Extender { + public static final int MOTOR_ID = 2; + public static final double GEAR_RATIO = 1 / 5.0; + public static final double STARTING_LENGTH = 0.3; + public static final int BACKWARDS_LIMIT = 0; + public static final double FORWARD_LIMIT = 0.78; + public static final double EXTENDED_LENGTH = FORWARD_LIMIT - 0.05; + public static final SparkMaxLimitSwitch.Type SWITCH_TYPE = SparkMaxLimitSwitch.Type.kNormallyClosed; + public static final double MAX_LENGTH_IN_ROBOT = 0.37; + public static final PIDObject PID = new PIDObject().withKp(60).withKd(2).withMaxPower(1); + public static final double SETPOINT_D = 10; + public static final double DEBOUNCE_TIME_FOR_LIMIT_SWITCH = 0.05; + public static final double EXTENDER_EXTENDING_GEAR_CIRC = 0.0165 * (2 * Math.PI); + public static final double POSITION_CONVERSION_FACTOR = GEAR_RATIO * EXTENDER_EXTENDING_GEAR_CIRC; + public static final double VELOCITY_CONVERSION_FACTOR = POSITION_CONVERSION_FACTOR / 60; + public static final GBSparkMax.SparkMaxConfObject EXTENDER_CONFIG_OBJECT = new GBSparkMax.SparkMaxConfObject() + .withPID(RobotMap.TelescopicArm.Extender.PID) + .withPositionConversionFactor(RobotMap.TelescopicArm.Extender.POSITION_CONVERSION_FACTOR) + .withVelocityConversionFactor(RobotMap.TelescopicArm.Extender.VELOCITY_CONVERSION_FACTOR) + .withIdleMode(CANSparkMax.IdleMode.kBrake) + .withRampRate(General.RAMP_RATE_VAL) + .withCurrentLimit(40); + public static final double LENGTH_TOLERANCE = 0.045; //in meters + public static final double FORWARDS_LENGTH_TOLERANCE = 0.01; //in meters + public static final double VELOCITY_TOLERANCE = 0.02; + + public static final double kS = 0.3; + public static final double kG = 0.67 - kS; + + public static final double MAX_ACCELERATION = 3.5; //4.2 + public static final double MAX_VELOCITY = 1.75; + public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_VELOCITY, MAX_ACCELERATION); + public static double MAX_ENTRANCE_LENGTH = 0.054; + } + + public static class Claw { + public static final int MOTOR_ID = 3; + public static final int SOLENOID_OPEN_CLAW_ID = 6; + public static final int SOLENOID_CLOSED_CLAW_ID = 2; + public static final double MOTOR_POWER_GRIP = 0.3; + + public static final double MOTOR_POWER_CONE = 0.6; + public static final double MOTOR_POWER_RELEASE = -0.15; + + public static final double TIME_OF_GRIP_CONSTANT = 2; + + public static final GBSparkMax.SparkMaxConfObject CLAW_CONFIG_OBJECT = new GBSparkMax.SparkMaxConfObject() + .withPID(RobotMap.TelescopicArm.Extender.PID) + .withPositionConversionFactor(RobotMap.TelescopicArm.Extender.POSITION_CONVERSION_FACTOR) + .withVelocityConversionFactor(RobotMap.TelescopicArm.Extender.VELOCITY_CONVERSION_FACTOR) + .withIdleMode(CANSparkMax.IdleMode.kBrake) + .withRampRate(General.RAMP_RATE_VAL) + .withCurrentLimit(30) + .withInverted(true); + } + + public static class Elbow { + public static final int MOTOR_ID = 1; + public static final int RESET_MEDIAN_SIZE = 7; + public static final double GEAR_RATIO = 1; + + public static final double kS = 0.133000000000002; + public static final double MIN_Kg = 0.155; + public static final double MAX_Kg = 0.62; + public static final double MAX_KG_MEASUREMENT_LENGTH = 0.822964668273926; + public static final double STARTING_ANGLE_RELATIVE_TO_GROUND = -1.765; //this is most easily measured using the encoder, so it is already radians + public static final double MAX_ACCELERATION = 10; + public static final double MAX_VELOCITY = 4; + public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_VELOCITY, MAX_ACCELERATION); + public static final PIDObject PID = new PIDObject().withKp(4).withKd(0.4).withMaxPower(1); + public static final double STARTING_WALL_ZONE_ANGLE = Units.degreesToRadians(11); + public static final double END_WALL_ZONE_ANGLE = Units.degreesToRadians(35.5); + + + public static final double ABSOLUTE_POSITION_CONVERSION_FACTOR = General.Motors.SPARKMAX_TICKS_PER_RADIAN / GEAR_RATIO; + public static final double ABSOLUTE_VELOCITY_CONVERSION_FACTOR = ABSOLUTE_POSITION_CONVERSION_FACTOR; + public static final double RELATIVE_POSITION_CONVERSION_FACTOR = 0.0328; //you know of calibrating pid but have you heard of calibrating the gear ratio + public static final double RELATIVE_VELOCITY_CONVERSION_FACTOR = 0.0328 / 60; + public static final double FORWARD_ANGLE_LIMIT = 2.13 + Math.toRadians(10); + public static final double BACKWARD_ANGLE_LIMIT = Units.degreesToRadians(4); + + public static final double ANGLE_TOLERANCE = Units.degreesToRadians(3.5); + public static final double ANGULAR_VELOCITY_TOLERANCE = Units.degreesToRadians(3); + public static final int CURRENT_LIMIT = 40; + public static final GBSparkMax.SparkMaxConfObject ELBOW_CONFIG_OBJECT = new GBSparkMax.SparkMaxConfObject() + .withPID(PID) + .withIdleMode(CANSparkMax.IdleMode.kBrake) + .withRampRate(General.RAMP_RATE_VAL) + .withCurrentLimit(RobotMap.TelescopicArm.Elbow.CURRENT_LIMIT); + + } + } + + public static class ThirdLeg { + public static final int FORWARD_PORT = 2; + public static final int REVERSE_PORT = 3; + } + + public static class Intake { + public static final int ROLLER_ID = 4; + public static final double DEFAULT_POWER = 1; + public static final int BEAM_BREAKER_ID = 20; + public static final GBSparkMax.SparkMaxConfObject INTAKE_CONFIG_OBJECT = new GBSparkMax.SparkMaxConfObject() + .withPID(RobotMap.TelescopicArm.Extender.PID) + .withPositionConversionFactor(RobotMap.TelescopicArm.Extender.POSITION_CONVERSION_FACTOR) + .withVelocityConversionFactor(RobotMap.TelescopicArm.Extender.VELOCITY_CONVERSION_FACTOR) + .withIdleMode(CANSparkMax.IdleMode.kBrake) + .withRampRate(General.RAMP_RATE_VAL) + .withCurrentLimit(30) + .withInverted(true); + + public static class Solenoid { + public static final int FORWARD_PORT = 4; + public static final int REVERSE_PORT = 0; + } + } } \ No newline at end of file diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BalanceOnRampLED.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BalanceOnRampLED.java deleted file mode 100644 index c05d10c4..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/BalanceOnRampLED.java +++ /dev/null @@ -1,12 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED; - -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.RepeatCommand; - -public class BalanceOnRampLED extends SetLEDColor { - - public BalanceOnRampLED(){ - super(Color.kRed); - } -} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/DrivingSidewaysLED.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/DrivingSidewaysLED.java deleted file mode 100644 index bb47d34f..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/DrivingSidewaysLED.java +++ /dev/null @@ -1,11 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED; - -import edu.wpi.first.wpilibj.util.Color; - -public class DrivingSidewaysLED extends SetLEDColor{ - - public DrivingSidewaysLED(){ - super(Color.kHotPink); - } - -} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/EncoderBrokenLED.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/EncoderBrokenLED.java deleted file mode 100644 index da7cdf13..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/EncoderBrokenLED.java +++ /dev/null @@ -1,10 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED; - -import edu.greenblitz.tobyDetermined.subsystems.swerve.SwerveChassis; -import edu.wpi.first.wpilibj.util.Color; - -public class EncoderBrokenLED extends LedBlinking{ - public EncoderBrokenLED() { - super(Color.kRed); - } -} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/HumanPlayerObjectIndicator.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/HumanPlayerObjectIndicator.java deleted file mode 100644 index 4b07b059..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/HumanPlayerObjectIndicator.java +++ /dev/null @@ -1,42 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED; - - -import edu.greenblitz.tobyDetermined.RobotMap; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.util.Color; - -/** - * @author noam - a command to indicate to the human player what object he wants (he - the robot) - * */ -public class HumanPlayerObjectIndicator extends LEDCommand{ - - private final wantedObject wantedGameObject; - - public HumanPlayerObjectIndicator(wantedObject object){ - this.wantedGameObject = object; - } - - public enum wantedObject{ - CUBE, - CONE - } - - @Override - public void execute() { - switch (this.wantedGameObject){ - - case CONE: - new LedBlinking(Color.kYellow).repeatedly(); - break; - - case CUBE: - new LedBlinking(Color.kMagenta).repeatedly(); - break; - } - } - - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/LEDCommand.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/LEDCommand.java deleted file mode 100644 index ec96adc6..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/LEDCommand.java +++ /dev/null @@ -1,15 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED; - -import edu.greenblitz.tobyDetermined.RobotMap; -import edu.greenblitz.tobyDetermined.subsystems.LED; -import edu.greenblitz.utils.GBCommand; - -public abstract class LEDCommand extends GBCommand { - protected LED led; - - public LEDCommand(){ - led = LED.getInstance(); - require(led); - } - -} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/LedBlinking.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/LedBlinking.java deleted file mode 100644 index c8048052..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/LedBlinking.java +++ /dev/null @@ -1,22 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED; - -import edu.greenblitz.tobyDetermined.RobotMap; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -public class LedBlinking extends SequentialCommandGroup { - - private Color color; - - public LedBlinking(Color firstColor, Color secondColor){ - super( - new SetLEDColor(firstColor),new WaitCommand(RobotMap.LED.BLINKING_TIME), - new SetLEDColor(secondColor),new WaitCommand(RobotMap.LED.BLINKING_TIME) - ); - } - - public LedBlinking(Color color){ - this(color, Color.kBlack); - } -} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/ObjectInClawLED.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/ObjectInClawLED.java deleted file mode 100644 index 1e45e390..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/ObjectInClawLED.java +++ /dev/null @@ -1,13 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED; - -import edu.greenblitz.tobyDetermined.RobotMap; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.util.Color; - - -public class ObjectInClawLED extends LedBlinking{ - - public ObjectInClawLED() { - super(Color.kOrange); - } -} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/ObjectInIntakeLED.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/ObjectInIntakeLED.java deleted file mode 100644 index eec7691b..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/ObjectInIntakeLED.java +++ /dev/null @@ -1,14 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED; - -import edu.greenblitz.tobyDetermined.OI; -import edu.greenblitz.tobyDetermined.RobotMap; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.util.Color; - -public class ObjectInIntakeLED extends LedBlinking{ - - - public ObjectInIntakeLED() { - super(Color.kAqua); - } -} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/SetLEDColor.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/SetLEDColor.java deleted file mode 100644 index 72b33ed1..00000000 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/LED/SetLEDColor.java +++ /dev/null @@ -1,22 +0,0 @@ -package edu.greenblitz.tobyDetermined.commands.LED; - -import edu.wpi.first.wpilibj.util.Color; - -public class SetLEDColor extends LEDCommand{ - - private Color color; - public SetLEDColor(Color color){ - this.color = color; - } - - - @Override - public void execute(){ - led.setColor(color); - } - - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java index 80c08fbe..6b253070 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java @@ -5,8 +5,6 @@ import edu.greenblitz.tobyDetermined.commands.rotatingBelly.RotateOutDoorDirection; import edu.greenblitz.tobyDetermined.subsystems.LED; import edu.greenblitz.tobyDetermined.subsystems.RotatingBelly.RotatingBelly; -import edu.greenblitz.tobyDetermined.subsystems.telescopicArm.ObjectSelector; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.*; @@ -26,7 +24,7 @@ public FullAlign(){ new RotateOutDoorDirection().until(()-> RotatingBelly.getInstance().isLimitSwitchPressed()), new RotateOutDoorDirection().raceWith(new WaitCommand(EXTRA_TIME)), - new InstantCommand(()-> LED.getInstance().setColor(Color.kOrchid)) + new InstantCommand(()-> LED.getInstance().setColor(RobotMap.LED.Sections.ALL,Color.kOrchid)) ).raceWith(new WaitCommand(FINAL_TIME))); } diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java index af4cb6e0..24020a27 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java @@ -36,6 +36,6 @@ public void end(boolean interrupted) { super.end(interrupted); extender.resetLength(); extender.stop(); - LED.getInstance().setColor(Color.kGreen); + LED.getInstance().setColor(RobotMap.LED.Sections.ALL,Color.kGreen); } } diff --git a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java index f78a9013..1c19e70f 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java @@ -3,9 +3,7 @@ import edu.greenblitz.tobyDetermined.RobotMap; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.WaitCommand; public class LED extends GBSubsystem { @@ -32,11 +30,6 @@ public static void init(){ } } - public void setColor(Color color) { //to all - for (int i = 0; i < this.ledBuffer.getLength(); i++) { - setColor(i,color); - } - } public void setColor(int i, Color color) { this.ledBuffer.setLED(i, color); } @@ -52,7 +45,7 @@ public void setColor (RobotMap.LED.Sections section, Color color){ } public void turnOff (){ - setColor(new Color(0,0,0)); + setColor(RobotMap.LED.Sections.ALL,new Color(0,0,0)); } public void turnOff (int index){ diff --git a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/telescopicArm/ObjectSelector.java b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/telescopicArm/ObjectSelector.java index ff4302fa..b7e1306f 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/telescopicArm/ObjectSelector.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/telescopicArm/ObjectSelector.java @@ -1,5 +1,6 @@ package edu.greenblitz.tobyDetermined.subsystems.telescopicArm; +import edu.greenblitz.tobyDetermined.RobotMap; import edu.greenblitz.tobyDetermined.subsystems.LED; import edu.wpi.first.wpilibj.util.Color; @@ -8,13 +9,13 @@ public class ObjectSelector { public static void selectCone(){ isCone = true; - LED.getInstance().setColor(Color.kYellow); + LED.getInstance().setColor(RobotMap.LED.Sections.ALL,Color.kYellow); Claw.getInstance().coneCatchMode(); } public static void selectCube(){ isCone = false; - LED.getInstance().setColor(Color.kMagenta); + LED.getInstance().setColor(RobotMap.LED.Sections.ALL,Color.kMagenta); Claw.getInstance().cubeCatchMode(); } From b09de713beddd32f8aef348e0f6006e09cc6a2a3 Mon Sep 17 00:00:00 2001 From: NoamRosenberg08 Date: Sun, 16 Jul 2023 00:29:59 +0300 Subject: [PATCH 07/10] noam - did more of the CR --- src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java b/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java index 42cdbf01..e3e7422e 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java @@ -71,8 +71,6 @@ public static class Vision { } public static class LED { - - public static final int LENGTH = 100; public static final int PORT = 0; public static final double BLINKING_TIME = 0.2; @@ -168,11 +166,11 @@ public static class KazaSwerve { public static final double linTicksToMeters = RobotMap.General.Motors.SPARKMAX_TICKS_PER_RADIAN * WHEEL_CIRC / LIN_GEAR_RATIO / (2 * Math.PI); public static final double angleTicksToWheelToRPM = RobotMap.General.Motors.SPARKMAX_VELOCITY_UNITS_PER_RPM / ANG_GEAR_RATIO; public static final double linTicksToMetersPerSecond = RobotMap.General.Motors.SPARKMAX_VELOCITY_UNITS_PER_RPM * WHEEL_CIRC / 60 / LIN_GEAR_RATIO; - public static final GBSparkMax.SparkMaxConfObject baseLinConfObj = new GBSparkMax.SparkMaxConfObject().withIdleMode(CANSparkMax.IdleMode.kBrake).withCurrentLimit(40).withRampRate(RobotMap.General.RAMP_RATE_VAL).withPID(linPID).withPositionConversionFactor(linTicksToMeters).withVelocityConversionFactor(linTicksToMetersPerSecond); public static final PIDObject linPID = new PIDObject().withKp(0.0003).withMaxPower(0.5); + public static final GBSparkMax.SparkMaxConfObject baseLinConfObj = new GBSparkMax.SparkMaxConfObject().withIdleMode(CANSparkMax.IdleMode.kBrake).withCurrentLimit(40).withRampRate(RobotMap.General.RAMP_RATE_VAL).withPID(linPID).withPositionConversionFactor(linTicksToMeters).withVelocityConversionFactor(linTicksToMetersPerSecond); public static final double angleTicksToRadians = RobotMap.General.Motors.SPARKMAX_TICKS_PER_RADIAN / ANG_GEAR_RATIO; - public static final GBSparkMax.SparkMaxConfObject baseAngConfObj = new GBSparkMax.SparkMaxConfObject().withIdleMode(CANSparkMax.IdleMode.kBrake).withCurrentLimit(30).withRampRate(RobotMap.General.RAMP_RATE_VAL).withInverted(true).withPID(angPID).withPositionConversionFactor(angleTicksToRadians).withVelocityConversionFactor(angleTicksToWheelToRPM); public static final PIDObject angPID = new PIDObject().withKp(0.5).withMaxPower(1.0); + public static final GBSparkMax.SparkMaxConfObject baseAngConfObj = new GBSparkMax.SparkMaxConfObject().withIdleMode(CANSparkMax.IdleMode.kBrake).withCurrentLimit(30).withRampRate(RobotMap.General.RAMP_RATE_VAL).withInverted(true).withPID(angPID).withPositionConversionFactor(angleTicksToRadians).withVelocityConversionFactor(angleTicksToWheelToRPM); } public static class SdsSwerve { From 2cb7c098a31daa13d56ba07785654cdc1db77153 Mon Sep 17 00:00:00 2001 From: NoamRosenberg08 Date: Mon, 17 Jul 2023 23:14:20 +0300 Subject: [PATCH 08/10] noam - finished cr --- .../edu/greenblitz/tobyDetermined/Robot.java | 11 ++++--- .../rotateAutomation/FullAlign.java | 2 +- .../telescopicArm/extender/ResetExtender.java | 2 +- .../tobyDetermined/subsystems/LED.java | 20 ++++++++----- .../telescopicArm/ObjectSelector.java | 4 +-- .../utils/InterruptableWaitCommand.java | 29 ------------------- 6 files changed, 21 insertions(+), 47 deletions(-) delete mode 100644 src/main/java/edu/greenblitz/utils/InterruptableWaitCommand.java diff --git a/src/main/java/edu/greenblitz/tobyDetermined/Robot.java b/src/main/java/edu/greenblitz/tobyDetermined/Robot.java index 4a15e535..d790baa7 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/Robot.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/Robot.java @@ -65,9 +65,8 @@ private static void initSubsystems() { IntakeExtender.init(); IntakeRoller.init(); OI.init(); + LED.init(); -// LED.getInstance(); //todo might make error - LED.init(); //todo might make error } private static void initPortForwarding() { @@ -156,15 +155,15 @@ public void disabledPeriodic() { // } if(Extender.getInstance().DidReset()){ - LED.getInstance().setColor(RobotMap.LED.Sections.ARM_ENCODER_BROKEN,Color.kGreen); + LED.getInstance().setColor(Color.kGreen, RobotMap.LED.Sections.ARM_ENCODER_BROKEN); }else{ - LED.getInstance().setColor(RobotMap.LED.Sections.ARM_ENCODER_BROKEN,Color.kRed); + LED.getInstance().setColor(Color.kRed, RobotMap.LED.Sections.ARM_ENCODER_BROKEN); } if(SwerveChassis.getInstance().isEncoderBroken()){ - LED.getInstance().setColor(RobotMap.LED.Sections.SWERVE_ENCODER_BROKEN,Color.kRed); + LED.getInstance().setColor(Color.kRed, RobotMap.LED.Sections.SWERVE_ENCODER_BROKEN); }else{ - LED.getInstance().setColor(RobotMap.LED.Sections.SWERVE_ENCODER_BROKEN,Color.kGreen); + LED.getInstance().setColor(Color.kGreen, RobotMap.LED.Sections.SWERVE_ENCODER_BROKEN); } diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java index 6b253070..73cc033e 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java @@ -24,7 +24,7 @@ public FullAlign(){ new RotateOutDoorDirection().until(()-> RotatingBelly.getInstance().isLimitSwitchPressed()), new RotateOutDoorDirection().raceWith(new WaitCommand(EXTRA_TIME)), - new InstantCommand(()-> LED.getInstance().setColor(RobotMap.LED.Sections.ALL,Color.kOrchid)) + new InstantCommand(()-> LED.getInstance().setColor(Color.kOrchid, RobotMap.LED.Sections.ALL)) ).raceWith(new WaitCommand(FINAL_TIME))); } diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java index 24020a27..a833749a 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java @@ -36,6 +36,6 @@ public void end(boolean interrupted) { super.end(interrupted); extender.resetLength(); extender.stop(); - LED.getInstance().setColor(RobotMap.LED.Sections.ALL,Color.kGreen); + LED.getInstance().setColor(Color.kGreen, RobotMap.LED.Sections.ALL); } } diff --git a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java index 1c19e70f..9370f1ca 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/LED.java @@ -30,26 +30,30 @@ public static void init(){ } } - public void setColor(int i, Color color) { - this.ledBuffer.setLED(i, color); + + public void setColor (Color color){ + setColor(color, RobotMap.LED.Sections.ALL); + } + public void setColor(Color color, int index) { + this.ledBuffer.setLED(index, color); } - public void setColor (int startIndex,int endIndex, Color color){ + public void setColor ( Color color,int startIndex,int endIndex){ for (int i = startIndex; i < endIndex; i++) { - setColor(i,color); + setColor(color,i); } } - public void setColor (RobotMap.LED.Sections section, Color color){ - setColor(section.start,section.end,color); + public void setColor (Color color, RobotMap.LED.Sections section){ + setColor(color,section.start,section.end); } public void turnOff (){ - setColor(RobotMap.LED.Sections.ALL,new Color(0,0,0)); + setColor(new Color(0,0,0),RobotMap.LED.Sections.ALL); } public void turnOff (int index){ - setColor(index,new Color(0,0,0)); + setColor(new Color(0,0,0),index); } public void turnOff (int startIndex,int endIndex){ for (int i = startIndex; i < endIndex; i++) { diff --git a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/telescopicArm/ObjectSelector.java b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/telescopicArm/ObjectSelector.java index b7e1306f..26a0424c 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/subsystems/telescopicArm/ObjectSelector.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/subsystems/telescopicArm/ObjectSelector.java @@ -9,13 +9,13 @@ public class ObjectSelector { public static void selectCone(){ isCone = true; - LED.getInstance().setColor(RobotMap.LED.Sections.ALL,Color.kYellow); + LED.getInstance().setColor(Color.kYellow, RobotMap.LED.Sections.ALL); Claw.getInstance().coneCatchMode(); } public static void selectCube(){ isCone = false; - LED.getInstance().setColor(RobotMap.LED.Sections.ALL,Color.kMagenta); + LED.getInstance().setColor(Color.kYellow, RobotMap.LED.Sections.ALL); Claw.getInstance().cubeCatchMode(); } diff --git a/src/main/java/edu/greenblitz/utils/InterruptableWaitCommand.java b/src/main/java/edu/greenblitz/utils/InterruptableWaitCommand.java deleted file mode 100644 index 32d363f2..00000000 --- a/src/main/java/edu/greenblitz/utils/InterruptableWaitCommand.java +++ /dev/null @@ -1,29 +0,0 @@ -package edu.greenblitz.utils; - -import edu.wpi.first.wpilibj.Timer; - -public class InterruptableWaitCommand extends GBCommand{ - - private double time; - private Timer timer; - - public InterruptableWaitCommand (double time){ - this.time = time; - this.timer = new Timer(); - } - - @Override - public void initialize() { - timer.start(); - } - - @Override - public boolean isFinished() { - return timer.hasElapsed(this.time); - } - - @Override - public void end(boolean interrupted) { - timer.stop(); - } -} From 3371b10e03ae779da6c9303bc0ca1e1c94236405 Mon Sep 17 00:00:00 2001 From: Raz Date: Sun, 3 Sep 2023 17:20:43 +0300 Subject: [PATCH 09/10] noam - working --- .idea/misc.xml | 2 +- .../greenblitz/tobyDetermined/RobotMap.java | 138 +++++++++++------- 2 files changed, 84 insertions(+), 56 deletions(-) diff --git a/.idea/misc.xml b/.idea/misc.xml index 0f221d8c..c90de3bd 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -4,5 +4,5 @@ - + \ No newline at end of file diff --git a/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java b/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java index e3e7422e..b2df9f70 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/RobotMap.java @@ -20,6 +20,13 @@ public class RobotMap { public static final Robot.robotName ROBOT_NAME = Robot.robotName.Frankenstein; + + + public static class BreakCoastSwitch{ + public static final int BREAK_COAST_SWITCH_DIO_PORT = 0; + + } + public static class General { public final static double MIN_VOLTAGE_BATTERY = 11.97; @@ -47,9 +54,9 @@ public static class Joystick { } public static class Pneumatics { - public static class PCM { - public static final int PCM_ID = 22; - public static final PneumaticsModuleType PCM_TYPE = PneumaticsModuleType.CTREPCM; + public static class PneumaticsController { + public static final int ID = 1; + public static final PneumaticsModuleType PCM_TYPE = PneumaticsModuleType.REVPH; } public static class PressureSensor { @@ -59,13 +66,13 @@ public static class PressureSensor { public static class Vision { public static final String[] LIMELIGHT_NAMES = new String[]{"limelight-front"}; + public static double STANDARD_DEVIATION_ODOMETRY = 0.001; + public static double STANDARD_DEVIATION_VISION2D = 0.3; + public static double STANDARD_DEVIATION_VISION_ANGLE = 0.6; public static final int[] PORT_NUMBERS = {5800, 5801, 5802, 5803, 5804, 5805}; public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(new Translation3d(), new Rotation3d()); public final static double MIN_DISTANCE_TO_FILTER_OUT = 1; public final static double VISION_CONSTANT = 0.2; - public static double STANDARD_DEVIATION_ODOMETRY = 0.001; - public static double STANDARD_DEVIATION_VISION2D = 0.3; - public static double STANDARD_DEVIATION_VISION_ANGLE = 0.6; } @@ -78,8 +85,8 @@ public enum Sections { //sections - ARM_ENCODER_BROKEN(0, 9), - SWERVE_ENCODER_BROKEN(11, 20), + ARM_ENCODER_BROKEN(0, 3), + SWERVE_ENCODER_BROKEN(4, 8), ALL(0, LENGTH); @@ -102,30 +109,6 @@ public static class Ultrasonic { public static class Swerve { - public static final Translation2d[] SwerveLocationsInSwerveKinematicsCoordinates = new Translation2d[]{ - //the WPILib coordinate system is stupid. (x is forwards, y is leftwards) - //the translations are given rotated by 90 degrees clockwise to avoid coordinates system conversion at output - new Translation2d(0.3020647, 0.25265), /*fl*/ - new Translation2d(0.3020647, -0.25265),/*fr*/ - new Translation2d(-0.3020647, 0.25265),/*bl*/ - new Translation2d(-0.3020647, -0.25265)/*br*/}; - public static final double MAX_VELOCITY = 4.1818320981472068; - public static final double MAX_ACCELERATION = 14.37979171376739; - public static final double MAX_ANGULAR_SPEED = 10.454580245368017; - public static final PIDObject rotationPID = new PIDObject().withKp(0.5).withKi(0).withKd(0).withFF(0.1); - public static final double ks = 0.14876; - public static final double kv = 3.3055; - public static final double ka = 0.11023; - static final Pose2d initialRobotPosition = new Pose2d(0, 0, new Rotation2d(0)); - public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleFrontLeft = new KazaSwerveModule.KazaSwerveModuleConfigObject(1, 10, 0, false); //front left - public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleFrontRight = new KazaSwerveModule.KazaSwerveModuleConfigObject(3, 11, 2, true); //front right - public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleBackLeft = new KazaSwerveModule.KazaSwerveModuleConfigObject(2, 8, 1, false); //back left - public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleBackRight = new KazaSwerveModule.KazaSwerveModuleConfigObject(12, 5, 3, true); //back right - public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleFrontLeft = new SdsSwerveModule.SdsSwerveModuleConfigObject(1, 0, 1, 0.8486328125, false); //front left - public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleFrontRight = new SdsSwerveModule.SdsSwerveModuleConfigObject(3, 2, 2, 0.2939453125, true); //front right - public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleBackLeft = new SdsSwerveModule.SdsSwerveModuleConfigObject(5, 4, 3, 0.5524, false); //back left - public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleBackRight = new SdsSwerveModule.SdsSwerveModuleConfigObject(7, 6, 4, 0.8718, true); //back right - public static class Frankenstein { @@ -133,8 +116,8 @@ public static class Frankenstein { public static final double ROBOT_WIDTH_IN_METERS = 0.79; public static final double BUMPER_WIDTH = 0.08; - public static final double MAX_VELOCITY = 2; - public static final double MAX_ACCELERATION = 1.75; + public static final double CAPPED_MAX_VELOCITY = 2; + public static final double CAPPED_MAX_ACCELERATION = 1.75; public static final double MAX_ANGULAR_SPEED = 8; public static final double MAX_ANGULAR_ACCELERATION = 20; //todo calibrate public static final int LAMPREY_AVERAGE_BITS = 2; @@ -154,6 +137,45 @@ public static class Frankenstein { }; } + + static final Pose2d initialRobotPosition = new Pose2d(0, 0, new Rotation2d(0)); + public static final Translation2d[] SwerveLocationsInSwerveKinematicsCoordinates = new Translation2d[]{ + //the WPILib coordinate system is stupid. (x is forwards, y is leftwards) + //the translations are given rotated by 90 degrees clockwise to avoid coordinates system conversion at output + new Translation2d(0.3020647, 0.25265), /*fl*/ + new Translation2d(0.3020647, -0.25265),/*fr*/ + new Translation2d(-0.3020647, 0.25265),/*bl*/ + new Translation2d(-0.3020647, -0.25265)/*br*/}; + + public static final double MAX_VELOCITY = 4.1818320981472068; + public static final double MAX_ACCELERATION = 14.37979171376739; + public static final double MAX_ANGULAR_SPEED = 10.454580245368017; + + + public static final PIDObject rotationPID = new PIDObject().withKp(0.5).withKi(0).withKd(0).withFF(0.1); + public static final double ks = 0.14876; + public static final double kv = 3.3055; + + public static final double ka = 0.11023; + + public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleFrontLeft = new KazaSwerveModule.KazaSwerveModuleConfigObject(1, 10, 0, false); //front left + + + public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleFrontRight = new KazaSwerveModule.KazaSwerveModuleConfigObject(3, 11, 2, true); //front right + + public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleBackLeft = new KazaSwerveModule.KazaSwerveModuleConfigObject(2, 8, 1, false); //back left + + public static KazaSwerveModule.KazaSwerveModuleConfigObject KazaModuleBackRight = new KazaSwerveModule.KazaSwerveModuleConfigObject(12, 5, 3, true); //back right + + public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleFrontLeft = new SdsSwerveModule.SdsSwerveModuleConfigObject(1, 0, 1, 0.8486328125,false); //front left + + public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleFrontRight = new SdsSwerveModule.SdsSwerveModuleConfigObject(3, 2, 2,0.2939453125 ,true); //front right + + public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleBackLeft = new SdsSwerveModule.SdsSwerveModuleConfigObject(5, 4, 3, 0.5524,false); //back left + + public static SdsSwerveModule.SdsSwerveModuleConfigObject SdsModuleBackRight = new SdsSwerveModule.SdsSwerveModuleConfigObject(7, 6, 4, 0.8718,true); //back right + + public static class KazaSwerve { public static final double ANG_GEAR_RATIO = 6.0; public static final double LIN_GEAR_RATIO = 8.0; @@ -214,10 +236,11 @@ public static class RotatingBelly { public static final double ROTATE_FROM_SWITCH_TO_STOP_TIME = 0.2; public static final double ROTATE_FROM_STOP_TO_SWITCH_TIME = 0.4; - public static final int CLOSE_PISTON_ID = 3; - public static final int OPEN_PISTON_ID = 7; public static double ROTATE_TO_DOOR_TIME = 3; + public static final int CLOSE_PISTON_ID = 7; + public static final int OPEN_PISTON_ID = 9; + } public static class TelescopicArm { @@ -232,14 +255,14 @@ public enum PresetPositions { CUBE_MID(0.29, 1.85), LOW(0.35, Math.toRadians(60)), - POST_CONE_DROP(0.089, 0.1), - PRE_CONE_DROP(0.089, 0.667), + POST_CONE_DROP(0.089,0.1), + PRE_CONE_DROP(0.089,0.667), COMMUNITY_PRE_GRID(Extender.MAX_ENTRANCE_LENGTH - Extender.LENGTH_TOLERANCE, CONE_HIGH.angleInRadians), - INTAKE_GRAB_CONE_POSITION(0.34, 0.12), + INTAKE_GRAB_CONE_POSITION(0.34, 0.123), INTAKE_GRAB_CUBE_POSITION(0.25, INTAKE_GRAB_CONE_POSITION.angleInRadians), - PRE_INTAKE_GRAB_POSITION(0.02, 0.35), //0.28), + PRE_INTAKE_GRAB_POSITION(0.02,0.35), //0.28), ZIG_HAIL(0, Math.toRadians(20.7) - STARTING_ANGLE_RELATIVE_TO_GROUND), FEEDER(0.663, 1.949); @@ -264,19 +287,15 @@ public static class Extender { public static final double EXTENDED_LENGTH = FORWARD_LIMIT - 0.05; public static final SparkMaxLimitSwitch.Type SWITCH_TYPE = SparkMaxLimitSwitch.Type.kNormallyClosed; public static final double MAX_LENGTH_IN_ROBOT = 0.37; + public static double MAX_ENTRANCE_LENGTH = 0.054; public static final PIDObject PID = new PIDObject().withKp(60).withKd(2).withMaxPower(1); public static final double SETPOINT_D = 10; public static final double DEBOUNCE_TIME_FOR_LIMIT_SWITCH = 0.05; + + public static final double EXTENDER_EXTENDING_GEAR_CIRC = 0.0165 * (2 * Math.PI); public static final double POSITION_CONVERSION_FACTOR = GEAR_RATIO * EXTENDER_EXTENDING_GEAR_CIRC; public static final double VELOCITY_CONVERSION_FACTOR = POSITION_CONVERSION_FACTOR / 60; - public static final GBSparkMax.SparkMaxConfObject EXTENDER_CONFIG_OBJECT = new GBSparkMax.SparkMaxConfObject() - .withPID(RobotMap.TelescopicArm.Extender.PID) - .withPositionConversionFactor(RobotMap.TelescopicArm.Extender.POSITION_CONVERSION_FACTOR) - .withVelocityConversionFactor(RobotMap.TelescopicArm.Extender.VELOCITY_CONVERSION_FACTOR) - .withIdleMode(CANSparkMax.IdleMode.kBrake) - .withRampRate(General.RAMP_RATE_VAL) - .withCurrentLimit(40); public static final double LENGTH_TOLERANCE = 0.045; //in meters public static final double FORWARDS_LENGTH_TOLERANCE = 0.01; //in meters public static final double VELOCITY_TOLERANCE = 0.02; @@ -287,13 +306,20 @@ public static class Extender { public static final double MAX_ACCELERATION = 3.5; //4.2 public static final double MAX_VELOCITY = 1.75; public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_VELOCITY, MAX_ACCELERATION); - public static double MAX_ENTRANCE_LENGTH = 0.054; + + public static final GBSparkMax.SparkMaxConfObject EXTENDER_CONFIG_OBJECT = new GBSparkMax.SparkMaxConfObject() + .withPID(RobotMap.TelescopicArm.Extender.PID) + .withPositionConversionFactor(RobotMap.TelescopicArm.Extender.POSITION_CONVERSION_FACTOR) + .withVelocityConversionFactor(RobotMap.TelescopicArm.Extender.VELOCITY_CONVERSION_FACTOR) + .withIdleMode(CANSparkMax.IdleMode.kBrake) + .withRampRate(General.RAMP_RATE_VAL) + .withCurrentLimit(40); } public static class Claw { public static final int MOTOR_ID = 3; - public static final int SOLENOID_OPEN_CLAW_ID = 6; - public static final int SOLENOID_CLOSED_CLAW_ID = 2; + public static final int SOLENOID_OPEN_CLAW_ID = 8; + public static final int SOLENOID_CLOSED_CLAW_ID = 5; public static final double MOTOR_POWER_GRIP = 0.3; public static final double MOTOR_POWER_CONE = 0.6; @@ -332,19 +358,22 @@ public static class Elbow { public static final double ABSOLUTE_POSITION_CONVERSION_FACTOR = General.Motors.SPARKMAX_TICKS_PER_RADIAN / GEAR_RATIO; public static final double ABSOLUTE_VELOCITY_CONVERSION_FACTOR = ABSOLUTE_POSITION_CONVERSION_FACTOR; public static final double RELATIVE_POSITION_CONVERSION_FACTOR = 0.0328; //you know of calibrating pid but have you heard of calibrating the gear ratio - public static final double RELATIVE_VELOCITY_CONVERSION_FACTOR = 0.0328 / 60; + public static final double RELATIVE_VELOCITY_CONVERSION_FACTOR = 0.0328/ 60; public static final double FORWARD_ANGLE_LIMIT = 2.13 + Math.toRadians(10); public static final double BACKWARD_ANGLE_LIMIT = Units.degreesToRadians(4); public static final double ANGLE_TOLERANCE = Units.degreesToRadians(3.5); public static final double ANGULAR_VELOCITY_TOLERANCE = Units.degreesToRadians(3); - public static final int CURRENT_LIMIT = 40; + + public static final GBSparkMax.SparkMaxConfObject ELBOW_CONFIG_OBJECT = new GBSparkMax.SparkMaxConfObject() .withPID(PID) .withIdleMode(CANSparkMax.IdleMode.kBrake) .withRampRate(General.RAMP_RATE_VAL) .withCurrentLimit(RobotMap.TelescopicArm.Elbow.CURRENT_LIMIT); + public static final int CURRENT_LIMIT = 40; + } } @@ -365,10 +394,9 @@ public static class Intake { .withRampRate(General.RAMP_RATE_VAL) .withCurrentLimit(30) .withInverted(true); - public static class Solenoid { - public static final int FORWARD_PORT = 4; - public static final int REVERSE_PORT = 0; + public static final int FORWARD_PORT = 10; + public static final int REVERSE_PORT = 6; } } } \ No newline at end of file From ad2e0594330572684a163de3009ab4852227b331 Mon Sep 17 00:00:00 2001 From: NoamRosenberg08 Date: Sat, 23 Sep 2023 21:10:26 +0300 Subject: [PATCH 10/10] noam - finished current resolevs, ready to merge --- .../commands/rotatingBelly/rotateAutomation/FullAlign.java | 2 +- .../commands/telescopicArm/extender/ResetExtender.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java index 73cc033e..93a81e99 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/commands/rotatingBelly/rotateAutomation/FullAlign.java @@ -24,7 +24,7 @@ public FullAlign(){ new RotateOutDoorDirection().until(()-> RotatingBelly.getInstance().isLimitSwitchPressed()), new RotateOutDoorDirection().raceWith(new WaitCommand(EXTRA_TIME)), - new InstantCommand(()-> LED.getInstance().setColor(Color.kOrchid, RobotMap.LED.Sections.ALL)) + new InstantCommand(()-> LED.getInstance().setColor(Color.kOrchid)) ).raceWith(new WaitCommand(FINAL_TIME))); } diff --git a/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java b/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java index a833749a..af4cb6e0 100644 --- a/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java +++ b/src/main/java/edu/greenblitz/tobyDetermined/commands/telescopicArm/extender/ResetExtender.java @@ -36,6 +36,6 @@ public void end(boolean interrupted) { super.end(interrupted); extender.resetLength(); extender.stop(); - LED.getInstance().setColor(Color.kGreen, RobotMap.LED.Sections.ALL); + LED.getInstance().setColor(Color.kGreen); } }