diff --git a/ThriftyTest/autons/auto-builder.py b/ThriftyTest/autons/auto-builder.py new file mode 100644 index 00000000..7fb246c5 --- /dev/null +++ b/ThriftyTest/autons/auto-builder.py @@ -0,0 +1,314 @@ +# A script to generate pathplanner .auto files from a much more simple auto file. +""" +syntax: + Command names are written without quotes. Nontrivial spaces (between regular characters) will be + respected. + To join commands in a sequential command group, use the + operator. + To join commands in a parallel command group, use the & operator. + To join commands in a race group, use the * operator + To join commands in a deadline group, use the ? operator + To make sure that any nesting is done properly, use parens to explicity + define order of operations + Multiple lines in a file are different commands running in sequence + + You can leave comments with the # sign. Anything after the # sign is ignored. + Comments MUST BE ON THEIR OWN LINE + + Here's a command that runs A and B in parallel, then runs C: + + (A & B) + C + + This command runs A, B, and C in sequence, in a race group with D: + + (A + B + C) * D + + The outputted json is pathplanner-ready and can be saved to src/main/deploy/pathplanner/autos/.auto +""" + +from enum import Enum +from io import UnsupportedOperation +import sys + +if len(sys.argv) != 2: + print(f"Usage: {sys.argv[0]} [filename]") + sys.exit(1) + +filename = sys.argv[1] + +file = open(filename, "r") +text = list(map(lambda s: s.strip(), file.readlines())) +file.close() + +class Token(Enum): + OPEN_PAREN = "(" + CLOSE_PAREN = ")" + PLUS = "+" + AND = "&" + DEADLINE = "?" + RACE = "*" + + def __repr__(self): + return f"('{self.value}')" + +TOKENS = [Token.OPEN_PAREN.value, Token.CLOSE_PAREN.value, Token.PLUS.value, Token.AND.value, Token.DEADLINE.value, Token.RACE.value] + +class Scanner: + def __init__(self, source): + self.start = 0 + self.current = 0 + self.tokens = [] + self.source = source + + def scan(self): + while self.start < len(self.source): + self.scan_token() + self.start = self.current + + def scan_token(self): + c = self.source[self.current] + self.current += 1 + match c: + case Token.OPEN_PAREN.value: + self.tokens.append(Token.OPEN_PAREN) + case Token.CLOSE_PAREN.value: + self.tokens.append(Token.CLOSE_PAREN) + case Token.PLUS.value: + self.tokens.append(Token.PLUS) + case Token.AND.value: + self.tokens.append(Token.AND) + case Token.DEADLINE.value: + self.tokens.append(Token.DEADLINE) + case Token.RACE.value: + self.tokens.append(Token.RACE) + case '#': + self.comment() + case _: + # assume it's an identifier, so run ident + self.ident() + + def comment(self): + # ignore the rest of the thing + self.current = len(self.source) + + def ident(self): + while self.current < len(self.source) and self.source[self.current] not in TOKENS: + self.current += 1 + # consume the last character + s = self.source[self.start:self.current].strip() + if s: + self.tokens.append(Named(s)) + +class Command: + def __init__(self, kind): + self.kind = kind + + def andThen(self, other): + if other.kind == Sequential.kind: + return other.andThen(self) + return Sequential([self, other]) + + def alongWith(self, other): + if other.kind == Parallel.kind: + return other.alongWith(self) + return Parallel([self, other]) + + def deadlineFor(self, other): + if other.kind == Parallel.kind: + return Deadline([self] + other.inner) + return Deadline([self, other]) + + def raceWith(self, other): + if other.kind == Race.kind: + return other.raceWith(self) + cmds = [] + if other.kind == Parallel.kind: + cmds += other.inner + else: + cmds.append(other) + cmds.append(self) + return Race(cmds) + + def json(self): + raise NotImplementedError("json() method not implemented on general class type") + + def __repr__(self): + return f"<{self.kind}>"; + +class GroupCommand(Command): + def __init__(self, cmds=[]): + self.inner = cmds + + def json(self): + return f'{{"type":"{self.kind}","data":{{"commands":[{",".join(map(lambda c: c.json(), self.inner))}]}}}}' + + def __repr__(self): + return f"[{self.kind[:3].upper()} {','.join(map(lambda c: str(c), self.inner))}]" + +class Sequential(GroupCommand): + kind = "sequential" + + def andThen(self, other): + if other.kind == Sequential.kind: + self.inner += other.inner + return self + self.inner.append(other) + return self + +class Parallel(GroupCommand): + kind = "parallel" + + def alongWith(self, other): + if other.kind == Parallel.kind: + self.inner += other.inner + return self + self.inner.append(other) + return self + + def raceWith(self, other): + if other.kind == Race.kind: + return other.raceWith(self) + if other.kind == Parallel.kind: + return Race(self.inner + other.inner) + return Race([other] + self.inner) + +class Race(GroupCommand): + kind = "race" + + def raceWith(self, other): + if other.kind == Parallel.kind or other.kind == Race.kind: + self.inner += other.inner + self.inner.append(other) + return self + +class Deadline(GroupCommand): + kind = "deadline" + + def deadlineFor(self, other): + if other.kind == Deadline.kind: + raise UnsupportedOperation("Deadline cannot have two deadlines") + if other.kind == Parallel.kind: + self.inner += other.inner + else: + self.inner.append(other) + return self + +class Named(Command): + kind = "named" + + def __init__(self, name): + self.name = name + + def __repr__(self): + return f"<{self.name}>" + + def json(self): + return f'{{"type":"named","data":{{"name":"{self.name}"}}}}' + +class Parser: + def __init__(self, tokens): + self.tokens = tokens + self.current = 0 + self.tree = Sequential() + + def find_groups(self, tokens): + # look for first parenthesis in the group. + i = 0 + while i < len(tokens): + if tokens[i] == Token.OPEN_PAREN: + break; + i += 1 + else: + # never left the loop, no parenthesis found + return self.condense(tokens) + # we now know that a parenthesis is found at index i. + j = i + 1 + offset = 1 # +1 for open, -1 for close + while j < len(tokens): + if tokens[j] == Token.OPEN_PAREN: + offset += 1 + elif tokens[j] == Token.CLOSE_PAREN: + offset -= 1 + if offset < 0: + print("parenthesis parse error!") + print(tokens) + sys.exit(1) + elif offset == 0: + break + j += 1 + else: + print("did not close parenthesis!") + print(tokens) + sys.exit(1) + inner = self.find_groups(tokens[i+1:j]) + return self.condense(tokens[:i] + inner + self.find_groups(tokens[j + 1:])) + + def condense(self, tokens): + # assumes tokens is a flat list. This will only work if tokens is a flat list. + return self.condenseRaces(self.condenseDeadline(self.condenseParallels(self.condenseSequentials(tokens)))) + + def condenseSequentials(self, tokens): + i = 1 + while i < len(tokens) - 1: + if tokens[i] == Token.PLUS: + break + i += 1 + else: + return tokens + prev = tokens[i - 1] + next = tokens[i + 1] + return self.condenseSequentials(tokens[:i-1] + [prev.andThen(next)] + tokens[i + 2:]) + + def condenseParallels(self, tokens): + i = 1 + while i < len(tokens) - 1: + if tokens[i] == Token.AND: + break + i += 1 + else: + return tokens + prev = tokens[i - 1] + next = tokens[i + 1] + return self.condenseParallels(tokens[:i-1] + [prev.alongWith(next)] + tokens[i + 2:]) + + def condenseDeadline(self, tokens): + i = 1 + while i < len(tokens) - 1: + if tokens[i] == Token.DEADLINE: + break + i += 1 + else: + return tokens + prev = tokens[i - 1] + next = tokens[i + 1] + return self.condenseDeadline(tokens[:i-1] + [prev.deadlineFor(next)] + tokens[i + 2:]) + + def condenseRaces(self, tokens): + i = 1 + while i < len(tokens) - 1: + if tokens[i] == Token.RACE: + break + i += 1 + else: + return tokens + prev = tokens[i - 1] + next = tokens[i + 1] + return self.condenseRaces(tokens[:i-1] + [prev.raceWith(next)] + tokens[i + 2:]) + + def parse(self): + return self.find_groups(self.tokens)[0] + +def pp_json(json): + return f'{{"version":"2025.0","command":{json},"resetOdom":false,"folder":null,"choreoAuto":false}}' + +lines = [] +for line in text: + if not line: + continue + scanner = Scanner(line) + scanner.scan() + # now to group, but how? + parser = Parser(scanner.tokens) + lines.append(parser.parse()) +cmd = Sequential(lines) + +print(pp_json(cmd.json())) diff --git a/ThriftyTest/autons/center.txt b/ThriftyTest/autons/center.txt new file mode 100644 index 00000000..36056a5c --- /dev/null +++ b/ThriftyTest/autons/center.txt @@ -0,0 +1,9 @@ +Align H +L4 +Align GH & Lower Algae +Align Barge Center +Net +Upper Algae & Align IJ +Align Barge Left +Net +Align LIntake diff --git a/ThriftyTest/autons/center2.txt b/ThriftyTest/autons/center2.txt new file mode 100644 index 00000000..b8dc1060 --- /dev/null +++ b/ThriftyTest/autons/center2.txt @@ -0,0 +1,12 @@ +Align H +L4 +Align Inside Lollipop & High Ground Algae +Beeline Barge Center +Net +Align Outside Lollipop & High Ground Algae +Beeline Barge Left +Net +Align IJ & High Ground Algae +Align Barge Left +Net +Beeline LIntake diff --git a/ThriftyTest/autons/left4.txt b/ThriftyTest/autons/left4.txt new file mode 100644 index 00000000..5b1ee6f5 --- /dev/null +++ b/ThriftyTest/autons/left4.txt @@ -0,0 +1,12 @@ +Align J +L4 +Recalibrate & ((Coral Wait ? Align LIntake) + Align K) +Intake +L4 +Recalibrate & ((Coral Wait ? Beeline LIntake) + Align L) +Intake +L4 +Recalibrate & ((Coral Wait ? Beeline LIntake) + Align A) +Intake +L4 +Recalibrate diff --git a/ThriftyTest/autons/right4.txt b/ThriftyTest/autons/right4.txt new file mode 100644 index 00000000..d62c62ec --- /dev/null +++ b/ThriftyTest/autons/right4.txt @@ -0,0 +1,12 @@ +Align E +L4 +Recalibrate & ((Coral Wait ? Align RIntake) + Align D) +Intake +L4 +Recalibrate & ((Coral Wait ? Beeline RIntake) + Align C) +Intake +L4 +Recalibrate & ((Coral Wait ? Beeline RIntake) + Align B) +Intake +L4 +Recalibrate diff --git a/ThriftyTest/build.gradle b/ThriftyTest/build.gradle index 6d83e956..0cd61c32 100644 --- a/ThriftyTest/build.gradle +++ b/ThriftyTest/build.gradle @@ -27,6 +27,17 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + jvmArgs.add("-XX:+UnlockExperimentalVMOptions") + jvmArgs.add("-XX:GCTimeRatio=5") + jvmArgs.add("-XX:+UseSerialGC") + jvmArgs.add("-XX:MaxGCPauseMillis=50") + + // The options below may improve performance, but should only be enabled on the RIO 2 + // + final MAX_JAVA_HEAP_SIZE_MB = 100; + jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-XX:+AlwaysPreTouch") } // Static files artifact diff --git a/ThriftyTest/glass.json b/ThriftyTest/glass.json index d66340f8..513cea31 100644 --- a/ThriftyTest/glass.json +++ b/ThriftyTest/glass.json @@ -1,35 +1,117 @@ { "NetworkTables": { - "Persistent Values": { - "open": false - }, "Retained Values": { "open": false }, - "Transitory Values": { - "open": false + "transitory": { + "SmartDashboard": { + "Coral": { + "open": true + }, + "Elevator": { + "open": true + }, + "Vision": { + "open": true + }, + "open": true + } }, "types": { "/FMSInfo": "FMSInfo", - "/SmartDashboard/Alerts": "Alerts", - "/SmartDashboard/Auto Chooser": "String Chooser", - "/SmartDashboard/Elevator Visualization": "Mechanism2d", - "/SmartDashboard/Go To B": "Command", - "/SmartDashboard/LIFT CLIMB": "Command", - "/SmartDashboard/LOWER CLIMB": "Command", - "/SmartDashboard/Pivot Arm Visualization": "Mechanism2d", - "/SmartDashboard/Reef mode off": "Command", - "/SmartDashboard/Reef mode on": "Command", + "/SmartDashboard/Auton Chooser": "String Chooser", + "/SmartDashboard/Command Scheduler": "Scheduler", + "/SmartDashboard/Elevator/Lazy Zero": "Command", + "/SmartDashboard/Elevator/Subsystem": "Subsystem", + "/SmartDashboard/PathPlanner": "Alerts", + "/SmartDashboard/PhotonAlerts": "Alerts", + "/SmartDashboard/Pivot/Visualization": "Mechanism2d", + "/SmartDashboard/Prep/Set Center": "Command", + "/SmartDashboard/Prep/Set Left": "Command", + "/SmartDashboard/Prep/Set Right": "Command", "/SmartDashboard/Super Field": "Field2d", - "/SmartDashboard/camera testing spot": "Command", - "/SmartDashboard/camera testing spot 2": "Command", - "/SmartDashboard/open funnel": "Command" + "/SmartDashboard/Test/Drive To Center": "Command", + "/SmartDashboard/Test/Enter test state": "Command", + "/SmartDashboard/Test/L4": "Command", + "/SmartDashboard/Test?": "Alerts", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" }, "windows": { + "/FMSInfo": { + "window": { + "visible": true + } + }, + "/SmartDashboard/Auton Chooser": { + "window": { + "visible": true + } + }, + "/SmartDashboard/Command Scheduler": { + "window": { + "visible": true + } + }, + "/SmartDashboard/Elevator/Lazy Zero": { + "window": { + "visible": true + } + }, "/SmartDashboard/Super Field": { + "Blue Alliance Left Intake": { + "arrows": false, + "selectable": false, + "style": "" + }, + "Blue Alliance Net": { + "arrows": false, + "selectable": false, + "style": "" + }, + "Blue Alliance Processor": { + "arrows": false, + "selectable": false, + "style": "" + }, + "Blue Alliance Reef": { + "arrows": false, + "selectable": false, + "style": "" + }, + "Blue Alliance Right Intake": { + "arrows": false, + "selectable": false, + "style": "" + }, + "Pathfind Trajectory": { + "arrows": false, + "selectable": false, + "style": "" + }, + "Red Alliance Left Intake": { + "arrows": false, + "selectable": false, + "style": "" + }, + "Red Alliance Net": { + "arrows": false, + "selectable": false, + "style": "" + }, + "Red Alliance Processor": { + "arrows": false, + "selectable": false, + "style": "" + }, + "Red Alliance Reef": { + "arrows": false, + "selectable": false, + "style": "" + }, "Red Alliance Right Intake": { "arrows": false, - "style": "Hidden" + "selectable": false, + "style": "" }, "Robot": { "arrowColor": [ @@ -39,35 +121,103 @@ 255.0 ], "color": [ - 0.45930102467536926, - 0.6751054525375366, - 0.3760080933570862, + 0.6306552290916443, + 0.27503088116645813, + 0.7388888597488403, 255.0 ], "length": 0.9399999976158142, "width": 0.9399999976158142 }, + "alt": { + "arrows": false, + "selectable": false, + "style": "" + }, "apriltag": { + "arrowColor": [], "arrows": false, - "style": "Hidden" + "color": [], + "length": 0.0, + "selectable": false, + "style": "Hidden", + "width": 0.0 + }, + "best": { + "arrows": false, + "selectable": false, + "style": "" }, "bottom": 1347, "builtin": "", - "estimation": { + "cam1": { "arrows": false, + "color": [], + "length": 0.0, + "style": "", + "width": 0.0 + }, + "cam2": { + "arrows": false, + "style": "" + }, + "cam3": { + "arrows": false, + "style": "" + }, + "cam4": { + "arrows": false, + "color": [], + "length": 0.0, + "style": "", + "width": 0.0 + }, + "cameras": { + "arrows": false, + "selectable": false, + "style": "Hidden" + }, + "height": 8.051901817321777, + "image": "/Users/rekrab/hackbots/2025_Reefscape/ThriftyTest/2025-field-cropped.png", + "reference": { + "arrowColor": [ + 0.7722222208976746, + 0.666685163974762, + 0.24453704059123993, + 255.0 + ], + "arrowSize": 24, "color": [ - 0.3948085606098175, - 0.4464501738548279, - 0.649789035320282, + 0.800000011920929, + 0.6802963018417358, + 0.35111114382743835, 255.0 ], - "length": 0.9399999976158142, + "length": 1.0, "selectable": false, - "width": 0.9399999976158142 + "width": 1.0 }, - "height": 8.051901817321777, - "image": "C:\\Users\\hackbots\\code\\2025_Reefscape\\ThriftyTest\\2025-field-cropped.png", "right": 2932, + "target": { + "arrows": false, + "color": [], + "length": 0.0, + "selectable": false, + "width": 0.0 + }, + "visibleTargetPoses": { + "arrows": false, + "color": [ + 0.35231274366378784, + 0.7555555701255798, + 0.2056790143251419, + 255.0 + ], + "length": 0.5, + "selectable": false, + "style": "", + "width": 0.5 + }, "width": 17.54825210571289, "window": { "visible": true @@ -76,13 +226,72 @@ } }, "NetworkTables Info": { - "Server": { - "open": true - }, "visible": true }, "NetworkTables Settings": { + "dsClient": false, "mode": "Client (NT4)", - "serverTeam": "3414" + "serverTeam": "localhost" + }, + "Plots": { + "Plot <0>": { + "plots": [ + { + "axis": [ + { + "max": 6.0, + "min": -1.0 + }, + { + "max": 12.0 + } + ], + "backgroundColor": [ + 0.12481480836868286, + 0.11629628390073776, + 0.13333332538604736, + 0.9399999976158142 + ], + "height": 0, + "name": "Stats", + "series": [ + { + "color": [ + 0.5887756943702698, + 0.30293208360671997, + 0.8388888835906982, + 1.0 + ], + "digital": "Analog", + "id": "NT:/SmartDashboard/Drivetrain/velo", + "weight": 3.0 + }, + { + "color": [ + 0.17543210089206696, + 0.27222222089767456, + 0.20769546926021576, + 1.0 + ], + "id": "NT:/SmartDashboard/Vision/Loop time (ms)" + }, + { + "color": [ + 0.3333333432674408, + 0.658823549747467, + 0.4078431725502014, + 1.0 + ], + "id": "NT:/SmartDashboard/Elevator/Position", + "yAxis": 1 + } + ], + "yaxis2": true + } + ], + "window": { + "name": "Stats" + } + } } } diff --git a/ThriftyTest/simgui-ds.json b/ThriftyTest/simgui-ds.json index 8f6d7994..4b36b755 100644 --- a/ThriftyTest/simgui-ds.json +++ b/ThriftyTest/simgui-ds.json @@ -1,9 +1,4 @@ { - "FMS": { - "window": { - "visible": false - } - }, "Joysticks": { "window": { "visible": false diff --git a/ThriftyTest/simgui.json b/ThriftyTest/simgui.json index 0d387c38..041fd9fa 100644 --- a/ThriftyTest/simgui.json +++ b/ThriftyTest/simgui.json @@ -1,21 +1,6 @@ { "HALProvider": { "Other Devices": { - "CANdi (v6)[59]": { - "header": { - "open": true - } - }, - "Talon FX (v6)[55]": { - "header": { - "open": true - } - }, - "Talon FX (v6)[56]": { - "header": { - "open": true - } - }, "window": { "visible": false } @@ -29,530 +14,180 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", - "/SmartDashboard/AUTOALIGN SLOWDOWN (LAST RESTORT)!!!!!!!!!!!!!!!!!!!!": "Command", "/SmartDashboard/Alerts": "Alerts", - "/SmartDashboard/AlignLEFT ": "Command", - "/SmartDashboard/AlignRight": "Command", - "/SmartDashboard/Auto Chooser": "String Chooser", - "/SmartDashboard/Drive To Center Command": "Command", - "/SmartDashboard/Elevator Visualization": "Mechanism2d", - "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/Go To B": "Command", - "/SmartDashboard/Height to Score #0": "String Chooser", - "/SmartDashboard/Height to Score #1": "String Chooser", - "/SmartDashboard/Height to Score #2": "String Chooser", - "/SmartDashboard/Height to Score #3": "String Chooser", - "/SmartDashboard/Height to Score #4": "String Chooser", - "/SmartDashboard/LIFT CLIMB": "Command", - "/SmartDashboard/LOWER CLIMB": "Command", - "/SmartDashboard/Lazy Zero Elevator": "Command", - "/SmartDashboard/ManualClimberCommand": "Command", - "/SmartDashboard/Module 0": "Mechanism2d", - "/SmartDashboard/Module 1": "Mechanism2d", - "/SmartDashboard/Module 2": "Mechanism2d", - "/SmartDashboard/Module 3": "Mechanism2d", - "/SmartDashboard/Multi Tag": "Command", + "/SmartDashboard/Auton Chooser": "String Chooser", + "/SmartDashboard/Camera Status": "Alerts", + "/SmartDashboard/Climber/Stow": "Command", + "/SmartDashboard/Command Scheduler": "Scheduler", + "/SmartDashboard/Coral": "Subsystem", + "/SmartDashboard/Coral/Subsystem": "Subsystem", + "/SmartDashboard/Elevator/Lazy Zero": "Command", + "/SmartDashboard/Elevator/Subsystem": "Subsystem", "/SmartDashboard/PathPlanner": "Alerts", - "/SmartDashboard/Pivot Arm Visualization": "Mechanism2d", - "/SmartDashboard/Place to Score #0": "String Chooser", - "/SmartDashboard/Place to Score #1": "String Chooser", - "/SmartDashboard/Place to Score #2": "String Chooser", - "/SmartDashboard/Place to Score #3": "String Chooser", - "/SmartDashboard/Place to Score #4": "String Chooser", - "/SmartDashboard/Raise Elevator": "Command", - "/SmartDashboard/Raise Pivot": "Command", - "/SmartDashboard/Reef mode off": "Command", - "/SmartDashboard/Reef mode on": "Command", - "/SmartDashboard/Reset Pose": "Command", - "/SmartDashboard/SendableChooser[0]": "String Chooser", - "/SmartDashboard/Set Center": "Command", - "/SmartDashboard/Set pose manually": "Command", - "/SmartDashboard/Single Tag": "Command", - "/SmartDashboard/Stow Elevator": "Command", + "/SmartDashboard/PhotonAlerts": "Alerts", + "/SmartDashboard/Prep/Set Center": "Command", + "/SmartDashboard/Prep/Set Left": "Command", + "/SmartDashboard/Prep/Set Right": "Command", "/SmartDashboard/Super Field": "Field2d", - "/SmartDashboard/Testing Command": "Command", + "/SmartDashboard/Test vision": "Command", + "/SmartDashboard/Test/Algae Intake (ground)": "Command", + "/SmartDashboard/Test/Drive To Center": "Command", + "/SmartDashboard/Test/Elevator Up": "Command", + "/SmartDashboard/Test/Enter test state": "Command", + "/SmartDashboard/Test/Follow Algae": "Command", + "/SmartDashboard/Test/L4": "Command", + "/SmartDashboard/Test/LowReefAlgaeIntake": "Command", + "/SmartDashboard/Test/Stow": "Command", + "/SmartDashboard/Test/Upper algae reef": "Command", + "/SmartDashboard/Test?": "Alerts", + "/SmartDashboard/Vision": "Alerts", + "/SmartDashboard/Vision/Camera Status": "Alerts", + "/SmartDashboard/VisionSystemSim-localization/Sim Field": "Field2d", "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d", - "/SmartDashboard/Zero Elevator": "Command", - "/SmartDashboard/a": "Command", - "/SmartDashboard/b": "Command", - "/SmartDashboard/c": "Command", - "/SmartDashboard/camera testing spot": "Command", - "/SmartDashboard/camera testing spot 2": "Command", - "/SmartDashboard/d": "Command", - "/SmartDashboard/drive to other pose": "Command", - "/SmartDashboard/drive to reef": "Command", - "/SmartDashboard/dynamic forward steer": "Command", - "/SmartDashboard/dynamic forward translation": "Command", - "/SmartDashboard/dynamic reverse steer": "Command", - "/SmartDashboard/dynamic reverse translation": "Command", - "/SmartDashboard/e": "Command", - "/SmartDashboard/elevator up": "Command", - "/SmartDashboard/f": "Command", - "/SmartDashboard/g": "Command", - "/SmartDashboard/h": "Command", - "/SmartDashboard/open funnel": "Command", - "/SmartDashboard/quasistatic forward steer": "Command", - "/SmartDashboard/quasistatic forward translation": "Command", - "/SmartDashboard/quasistatic reverse steer": "Command", - "/SmartDashboard/quasistatic reverse translation": "Command", - "/SmartDashboard/runme hehe": "Command", - "/SmartDashboard/teleport": "Command" + "/SmartDashboard/VisionSystemSim-test/Sim Field": "Field2d", + "/SmartDashboard/VisionSystemSim-tracking/Sim Field": "Field2d", + "/SmartDashboard/pro": "Command" }, "windows": { - "/SmartDashboard/April Tag Debug Field": { - "Robot": { - "arrowColor": [ - 0.7735829949378967, - 0.6149299144744873, - 0.9282700419425964, - 255.0 - ], - "color": [ - 0.8979749083518982, - 0.8026491403579712, - 0.9324894547462463, - 255.0 - ], - "length": 0.9440000057220459, - "selectable": false, - "weight": 3.0, - "width": 0.9440000057220459 - }, - "apriltag": { - "arrows": false, - "color": [ - 0.2275988608598709, - 0.23360605537891388, - 0.28691983222961426, - 255.0 - ], - "length": 0.4000000059604645, - "style": "Box/Image", - "width": 0.4000000059604645 - }, - "bottom": 1638, - "builtin": "2025 Reefscape", - "cameras": { - "arrowColor": [ - 1.0, - 1.0, - 1.0, - 255.0 - ], - "arrowSize": 100, - "arrowWeight": 2.0, - "color": [ - 0.21145951747894287, - 0.7805907130241394, - 0.1646815836429596, - 255.0 - ], - "length": 0.20000000298023224, - "style": "Hidden", - "width": 0.20000000298023224 - }, - "estimation": { - "arrows": false, - "color": [ - 0.8665041923522949, - 0.9240506291389465, - 0.5107621550559998, - 255.0 - ], - "length": 0.8999999761581421, - "style": "Box/Image", - "width": 0.8999999761581421 - }, - "height": 8.051901817321777, - "left": 534, - "right": 3466, - "top": 291, - "visibleTargetPoses": { - "arrowColor": [ - 1.0, - 1.0, - 1.0, - 255.0 - ], - "arrowWeight": 2.0, - "color": [ - 0.6858251690864563, - 0.3888266086578369, - 0.8776371479034424, - 255.0 - ], - "length": 0.4000000059604645, - "width": 0.4000000059604645 - }, - "width": 17.54825210571289, - "window": { - "visible": false - } - }, - "/SmartDashboard/Auto Chooser": { + "/SmartDashboard/Auton Chooser": { "window": { "visible": true } }, "/SmartDashboard/Super Field": { - "Blue Alliance Left Intake": { - "arrows": false, - "color": [ - 0.15189886093139648, - 1.0, - 0.0, - 255.0 - ], - "style": "Line (Closed)" - }, - "Blue Alliance Net": { - "arrows": false, - "color": [ - 0.0, - 0.025316476821899414, - 1.0, - 255.0 - ], - "style": "Line (Closed)" - }, - "Blue Alliance Processor": { - "arrows": false, - "color": [ - 0.025316238403320313, - 0.0, - 1.0, - 255.0 - ], - "style": "Line (Closed)" - }, - "Blue Alliance Reef": { - "arrows": false, - "color": [ - 1.0, - 0.0, - 0.9620256423950195, - 255.0 - ], - "style": "Line (Closed)" - }, - "Blue Alliance Right Intake": { - "arrows": false, - "color": [ - 0.05063295364379883, - 1.0, - 0.0, - 255.0 - ], - "style": "Line (Closed)" - }, - "Blue Left Human Player Bounds": { - "arrows": false, - "color": [ - 0.0, - 1.0, - 0.08928561210632324, - 255.0 - ], - "style": "Line (Closed)" - }, - "Blue Net Bounds": { - "arrows": false, - "color": [ - 0.035714149475097656, - 0.0, - 1.0, - 255.0 - ], - "style": "Line (Closed)" - }, - "Blue Processor Bounds": { - "arrows": false, - "color": [ - 0.0, - 0.01785731315612793, - 1.0, - 255.0 - ], - "style": "Line (Closed)" - }, - "Blue Reef Bounds": { - "arrows": false, - "color": [ - 1.0, - 0.0, - 0.9642858505249023, - 255.0 - ], - "style": "Line (Closed)" - }, - "Blue Right Human Player Bounds": { - "arrows": false, - "color": [ - 0.0, - 1.0, - 0.08928561210632324, - 255.0 - ], - "style": "Line (Closed)" - }, - "Drive Target": { - "arrows": false, - "color": [ - 0.37852850556373596, - 0.36489877104759216, - 0.47257381677627563, - 255.0 - ], - "length": 0.7599999904632568, - "selectable": false, - "width": 0.7599999904632568 + "Algae": { + "image": "../resources/algae.jpg", + "length": 0.800000011920929, + "width": 0.800000011920929 }, "FF": { "arrows": false, - "color": [ - 0.7843137383460999, - 0.030757399275898933, - 0.5626797080039978, - 255.0 - ], - "length": 2.5, + "length": 1.5, "selectable": false, - "weight": 9.0, - "width": 6.0 + "style": "Box/Image", + "width": 2.0 }, "Pathfind Trajectory": { "arrows": false, - "color": [ - 238.86077880859375, - 255.0, - 0.0, - 255.0 - ], - "style": "Line" - }, - "Red Alliance Left Intake": { - "arrows": false, - "color": [ - 0.05063295364379883, - 1.0, - 0.0, - 255.0 - ], - "style": "Line (Closed)" - }, - "Red Alliance Net": { - "arrows": false, - "style": "Line (Closed)" - }, - "Red Alliance Processor": { - "arrows": false, - "style": "Line (Closed)" - }, - "Red Alliance Reef": { - "arrows": false, - "color": [ - 0.7848100662231445, - 0.0, - 1.0, - 255.0 - ], - "style": "Line (Closed)" - }, - "Red Alliance Right Intake": { - "arrows": false, - "color": [ - 0.0, - 1.0, - 0.0, - 255.0 - ], - "style": "Line (Closed)" - }, - "Red Left Human Player Bounds": { - "arrows": false, - "color": [ - 0.0, - 1.0, - 0.017857074737548828, - 255.0 - ], - "style": "Line (Closed)" - }, - "Red Net Bounds": { - "arrows": false, - "style": "Line (Closed)" - }, - "Red Processor Bounds": { - "arrows": false, - "color": [ - 1.0, - 0.0, - 0.0, - 255.0 - ], - "style": "Line (Closed)" - }, - "Red Reef Bounds": { - "arrows": false, - "color": [ - 1.0, - 0.0, - 0.9107146263122559, - 255.0 - ], - "style": "Line (Closed)" - }, - "Red Right Human Player Bounds": { - "arrows": false, - "color": [ - 0.0, - 1.0, - 0.0, - 255.0 - ], - "style": "Line (Closed)" + "selectable": false, + "style": "Hidden" }, "Robot": { "arrowColor": [ - 1.0, - 1.0, - 1.0, + 0.9656862616539001, + 0.9656862616539001, + 0.9656862616539001, 255.0 ], - "arrowSize": 25, - "arrowWeight": 2.0, "color": [ - 0.7045847773551941, - 0.7914843559265137, - 0.7941176295280457, + 0.6150517463684082, + 0.32271242141723633, + 0.7745097875595093, 255.0 ], "length": 0.9399999976158142, "style": "Box/Image", "width": 0.9399999976158142 }, - "alt": { + "Tracked Object": { "arrows": false, - "color": [ - 0.0, - 0.14705896377563477, - 1.0, - 255.0 - ], - "length": 0.9399999976158142, - "style": "Hidden", - "width": 0.9399999976158142 + "image": "/Users/rekrab/hackbots/2025_Reefscape/resources/algae.jpg", + "length": 0.800000011920929, + "width": 0.800000011920929 }, "apriltag": { "arrows": false, "selectable": false, "style": "Hidden" }, - "best": { + "bottom": 1638, + "cameras": { "arrows": false, "color": [ - 0.0, - 1.0, - 0.11764693260192871, + 0.7850345969200134, + 0.8088235259056091, + 0.40441176295280457, 255.0 ], - "length": 0.9399999976158142, "selectable": false, - "style": "Hidden", - "width": 0.9399999976158142 + "style": "Hidden" }, - "bottom": 1347, - "builtin": "", - "cameras": { + "estimation": { "arrowColor": [ - 232.4050750732422, - 255.0, - 0.0, + 0.3990052044391632, + 0.6029411554336548, + 0.5009731650352478, 255.0 ], - "arrowSize": 100, - "arrowWeight": 2.0, + "arrowSize": 14, "color": [ - 255.0, - 167.8480987548828, - 0.0, + 0.45415228605270386, + 0.686274528503418, + 0.5702133178710938, 255.0 ], - "length": 0.30000001192092896, + "length": 0.9399999976158142, "selectable": false, - "style": "Hidden", - "weight": 2.0, - "width": 0.30000001192092896 + "width": 0.9399999976158142 }, - "chosen": { - "arrows": false, - "color": [ - 1.0, - 1.0, - 1.0, + "height": 8.051901817321777, + "left": 534, + "reference": { + "arrowColor": [ + 0.8480392098426819, + 0.6382302045822144, + 0.19953861832618713, 255.0 - ] - }, - "chsoen": { + ], + "arrowSize": 25, "color": [ - 0.9313725233078003, - 0.49764513969421387, - 0.06391774863004684, + 0.8186274766921997, + 0.5862345099449158, + 0.10032197833061218, 255.0 - ] + ], + "length": 1.0, + "selectable": false, + "width": 1.0 }, - "estimation": { - "color": [ - 0.9588443040847778, - 0.970588207244873, - 0.042820051312446594, - 255.0 - ] + "rej": { + "arrows": false, + "length": 2.0, + "selectable": false, + "width": 2.0 }, - "height": 8.051901817321777, - "image": "C:\\Users\\hackbots\\code\\2025_Reefscape\\ThriftyTest\\2025-field-cropped.png", "rejected": { - "arrows": false, - "color": [ - 0.8480392098426819, - 0.3034650385379791, - 0.3835495412349701, + "arrowColor": [ + 0.44117647409439087, + 0.30276817083358765, + 0.3841848075389862, 255.0 ], - "length": 0.9399999976158142, - "selectable": false, - "width": 0.9399999976158142 - }, - "right": 2932, - "target": { "arrows": false, "color": [ - 0.8649789094924927, - 0.31752389669418335, - 0.31752389669418335, + 0.4803921580314636, + 0.2401960790157318, + 0.33203572034835815, 255.0 ], "length": 0.9399999976158142, "selectable": false, - "style": "Box/Image", + "style": "Hidden", "width": 0.9399999976158142 }, + "right": 3466, + "top": 291, "visibleTargetPoses": { - "arrowColor": [ - 1.0, - 1.0, - 1.0, - 255.0 - ], + "arrows": false, "color": [ - 0.676780104637146, - 0.20238924026489258, - 0.8270041942596436, + 0.4533917307853699, + 0.8284313678741455, + 0.19086408615112305, 255.0 ], - "length": 0.4000000059604645, + "length": 0.5, "selectable": false, - "width": 0.4000000059604645 + "width": 0.5 }, "width": 17.54825210571289, "window": { @@ -562,46 +197,87 @@ } }, "NetworkTables": { + "Retained Values": { + "open": false + }, "transitory": { - "DriveState": { - "ChassisSpeeds##v_/DriveState/Speeds": { + "SmartDashboard": { + "Algae": { + "open": true + }, + "Algae Tracking": { + "open": true + }, + "Super Field": { + "double[]##v_/SmartDashboard/Super Field/tag relative": { + "open": true + } + }, + "Vision": { "open": true }, "open": true }, - "SmartDashboard": { - "open": true + "photonvision": { + "cam1": { + "open": true + } } } }, - "NetworkTables Info": { - "Server": { - "Subscribers": { - "open": true - }, - "open": true - }, - "visible": true - }, "Plot": { "Plot <0>": { "plots": [ { "axis": [ { - "label": "Axis", + "lockMax": true, + "lockMin": true, + "max": 6.0, + "min": -1.0 + }, + { + "lockMax": true, "lockMin": true, - "max": 5.0 + "max": 12.0 + }, + { + "lockMax": true, + "lockMin": true, + "max": 1.2 } ], "backgroundColor": [ - 0.0, - 0.0, - 0.0, + 0.1342865526676178, + 0.11985772848129272, + 0.14215683937072754, 0.8500000238418579 ], + "height": 288, + "yaxis2": true, + "yaxis3": true + } + ], + "window": { + "name": "Stats", + "visible": false + } + }, + "Plot <1>": { + "plots": [ + { + "axis": [ + { + "lockMin": true + } + ], + "backgroundColor": [ + 0.05999999865889549, + 0.05999999865889549, + 0.05999999865889549, + 0.9399999976158142 + ], "height": 0, - "name": "Plot", "series": [ { "color": [ @@ -610,7 +286,7 @@ 0.6901960968971252, 1.0 ], - "id": "NT:/SmartDashboard/ri" + "id": "NT:/SmartDashboard/*est. distance" }, { "color": [ @@ -619,7 +295,7 @@ 0.32156863808631897, 1.0 ], - "id": "NT:/SmartDashboard/ru" + "id": "NT:/SmartDashboard/*real dist" } ] } diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceCenterLeft.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceCenterLeft.auto deleted file mode 100644 index 3984b521..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceCenterLeft.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "StartH" - } - }, - { - "type": "named", - "data": { - "name": "Align H" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "path", - "data": { - "pathName": "HWall" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceCenterRight.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceCenterRight.auto deleted file mode 100644 index 990a2200..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceCenterRight.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "StartG" - } - }, - { - "type": "named", - "data": { - "name": "Align G" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "path", - "data": { - "pathName": "GWall" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceLeft.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceLeft.auto deleted file mode 100644 index 14e4845c..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceLeft.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "StartI" - } - }, - { - "type": "named", - "data": { - "name": "Align I" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceRight.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceRight.auto deleted file mode 100644 index 6e79e8a1..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/1PieceRight.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "StartF" - } - }, - { - "type": "named", - "data": { - "name": "Align F" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceLeft.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceLeft.auto index a6bd4dd7..bf2049f9 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceLeft.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceLeft.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "StartJ" - } - }, { "type": "named", "data": { @@ -29,7 +23,7 @@ { "type": "named", "data": { - "name": "Intake" + "name": "Recalibrate" } }, { @@ -37,27 +31,22 @@ "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "JIntake" - } - }, - { - "type": "named", + "type": "deadline", "data": { - "name": "LIntake Align" - } - }, - { - "type": "named", - "data": { - "name": "Intake Wait" - } - }, - { - "type": "path", - "data": { - "pathName": "IntakeK" + "commands": [ + { + "type": "named", + "data": { + "name": "Coral Wait" + } + }, + { + "type": "named", + "data": { + "name": "Align LIntake" + } + } + ] } }, { @@ -72,6 +61,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, { "type": "named", "data": { @@ -85,7 +80,7 @@ { "type": "named", "data": { - "name": "Intake" + "name": "Recalibrate" } }, { @@ -93,27 +88,22 @@ "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "KIntake" - } - }, - { - "type": "named", - "data": { - "name": "LIntake Align" - } - }, - { - "type": "named", - "data": { - "name": "Intake Wait" - } - }, - { - "type": "path", + "type": "deadline", "data": { - "pathName": "IntakeL" + "commands": [ + { + "type": "named", + "data": { + "name": "Coral Wait" + } + }, + { + "type": "named", + "data": { + "name": "Beeline LIntake" + } + } + ] } }, { @@ -128,6 +118,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, { "type": "named", "data": { @@ -141,7 +137,7 @@ { "type": "named", "data": { - "name": "Intake" + "name": "Recalibrate" } }, { @@ -149,27 +145,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "LIntake" - } - }, - { - "type": "named", - "data": { - "name": "LIntake Align" - } - }, - { - "type": "named", - "data": { - "name": "Intake Wait" - } - }, - { - "type": "path", - "data": { - "pathName": "IntakeA" + "commands": [ + { + "type": "named", + "data": { + "name": "Coral Wait" + } + }, + { + "type": "named", + "data": { + "name": "Beeline LIntake" + } + } + ] } }, { @@ -184,16 +175,28 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, { "type": "named", "data": { "name": "L4" } + }, + { + "type": "named", + "data": { + "name": "Recalibrate" + } } ] } }, - "resetOdom": true, + "resetOdom": false, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceRight.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceRight.auto index e8d65d5f..62534586 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceRight.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceRight.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "StartE" - } - }, { "type": "named", "data": { @@ -29,7 +23,7 @@ { "type": "named", "data": { - "name": "Intake" + "name": "Recalibrate" } }, { @@ -37,27 +31,22 @@ "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "FIntake" - } - }, - { - "type": "named", + "type": "deadline", "data": { - "name": "RIntake Align" - } - }, - { - "type": "named", - "data": { - "name": "Intake Wait" - } - }, - { - "type": "path", - "data": { - "pathName": "IntakeD" + "commands": [ + { + "type": "named", + "data": { + "name": "Coral Wait" + } + }, + { + "type": "named", + "data": { + "name": "Align RIntake" + } + } + ] } }, { @@ -72,6 +61,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, { "type": "named", "data": { @@ -85,7 +80,7 @@ { "type": "named", "data": { - "name": "Intake" + "name": "Recalibrate" } }, { @@ -93,27 +88,22 @@ "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "DIntake" - } - }, - { - "type": "named", - "data": { - "name": "RIntake Align" - } - }, - { - "type": "named", - "data": { - "name": "Intake Wait" - } - }, - { - "type": "path", + "type": "deadline", "data": { - "pathName": "IntakeC" + "commands": [ + { + "type": "named", + "data": { + "name": "Coral Wait" + } + }, + { + "type": "named", + "data": { + "name": "Beeline RIntake" + } + } + ] } }, { @@ -128,6 +118,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, { "type": "named", "data": { @@ -141,7 +137,7 @@ { "type": "named", "data": { - "name": "Intake" + "name": "Recalibrate" } }, { @@ -149,27 +145,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "CIntake" - } - }, - { - "type": "named", - "data": { - "name": "RIntake Align" - } - }, - { - "type": "named", - "data": { - "name": "Intake Wait" - } - }, - { - "type": "path", - "data": { - "pathName": "IntakeB" + "commands": [ + { + "type": "named", + "data": { + "name": "Coral Wait" + } + }, + { + "type": "named", + "data": { + "name": "Beeline RIntake" + } + } + ] } }, { @@ -184,16 +175,28 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, { "type": "named", "data": { "name": "L4" } + }, + { + "type": "named", + "data": { + "name": "Recalibrate" + } } ] } }, - "resetOdom": true, + "resetOdom": false, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto new file mode 100644 index 00000000..589978e2 --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto @@ -0,0 +1 @@ +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align H"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Align GH"}},{"type":"named","data":{"name":"Lower Algae"}}]}},{"type":"named","data":{"name":"Align Barge Center"}},{"type":"named","data":{"name":"Net"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Upper Algae"}},{"type":"named","data":{"name":"Align IJ"}}]}},{"type":"named","data":{"name":"Align Barge Left"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Align LIntake"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/CenterBarge.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/CenterBarge.auto index ebdd1ebf..7cd04d27 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/CenterBarge.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/CenterBarge.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "StartH" - } - }, { "type": "named", "data": { @@ -35,7 +29,7 @@ { "type": "named", "data": { - "name": "AlgaeLower" + "name": "Lower Algae" } } ] @@ -50,7 +44,7 @@ { "type": "named", "data": { - "name": "Align Barge" + "name": "Beeline Barge Center" } }, { @@ -72,7 +66,7 @@ { "type": "named", "data": { - "name": "AlgaeUpper" + "name": "Upper Algae" } }, { @@ -93,7 +87,7 @@ { "type": "named", "data": { - "name": "Align Barge" + "name": "Beeline Barge Center" } }, { @@ -111,7 +105,7 @@ ] } }, - "resetOdom": true, + "resetOdom": false, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Processor Center.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Processor Center.auto deleted file mode 100644 index ee26b21a..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Processor Center.auto +++ /dev/null @@ -1,79 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "StartG" - } - }, - { - "type": "named", - "data": { - "name": "Align G" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "named", - "data": { - "name": "Align GH" - } - }, - { - "type": "named", - "data": { - "name": "AlgaeLower" - } - }, - { - "type": "path", - "data": { - "pathName": "GHProcessor" - } - }, - { - "type": "named", - "data": { - "name": "Process" - } - }, - { - "type": "path", - "data": { - "pathName": "ProcessorEF" - } - }, - { - "type": "named", - "data": { - "name": "AlgaeUpper" - } - }, - { - "type": "path", - "data": { - "pathName": "EFProcessor" - } - }, - { - "type": "named", - "data": { - "name": "Process" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Test Path.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Test Path.auto deleted file mode 100644 index b28cdfb8..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Test Path.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "IntakeE" - } - }, - { - "type": "named", - "data": { - "name": "Align E" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Test.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Test.auto deleted file mode 100644 index 8a9fbe1a..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Test.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "StartG" - } - }, - { - "type": "named", - "data": { - "name": "Algae End" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/paths/EIntake.path b/ThriftyTest/src/main/deploy/pathplanner/paths/EIntake.path index d05fd8d9..a236df5b 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/paths/EIntake.path +++ b/ThriftyTest/src/main/deploy/pathplanner/paths/EIntake.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.8290983606557374, - "y": 1.5495389344262287 + "x": 2.687902565502183, + "y": 1.6449947973526196 }, "prevControl": { - "x": 3.367437281948422, - "y": 1.9230241926455585 + "x": 3.2262414867948674, + "y": 2.01848005557195 }, "nextControl": null, "isLocked": false, @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 3.0, + "velocity": 100000.0, "rotation": 55.0 }, "reversed": false, diff --git a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeA.path b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeA.path index b679953d..f72059d5 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeA.path +++ b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeA.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.558878327546296, + "x": 1.5588783275462958, "y": 5.461553385416666 }, "isLocked": false, diff --git a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeB.path b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeB.path index 93cf61ad..0e466cd8 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeB.path +++ b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeB.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4505122950819673, - "y": 1.1899077868852452 + "x": 1.211, + "y": 0.9501 }, "prevControl": null, "nextControl": { - "x": 1.8445104142949305, - "y": 2.071017089547282 + "x": 1.604998119212963, + "y": 1.8312093026620369 }, "isLocked": false, "linkedName": "RightIntake" diff --git a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeC.path b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeC.path index e8c56752..424671d3 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeC.path +++ b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeC.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4505122950819673, - "y": 1.1899077868852452 + "x": 1.211, + "y": 0.9501 }, "prevControl": null, "nextControl": { - "x": 2.1717297418351507, - "y": 1.8750655757464543 + "x": 1.9322174467531832, + "y": 1.6352577888612092 }, "isLocked": false, "linkedName": "RightIntake" diff --git a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeD.path b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeD.path index c175ebc2..a32e883e 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeD.path +++ b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeD.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4505122950819673, - "y": 1.1899077868852452 + "x": 1.211, + "y": 0.9501 }, "prevControl": null, "nextControl": { - "x": 2.40952868852459, - "y": 1.8132684426229502 + "x": 3.167310836226851, + "y": 1.7636744068287031 }, "isLocked": false, "linkedName": "RightIntake" @@ -47,7 +47,7 @@ "reversed": false, "folder": "IntakeX", "idealStartingState": { - "velocity": 0, + "velocity": 0.0, "rotation": 55.0 }, "useDefaultConstraints": true diff --git a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeE.path b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeE.path index e2fcbdf4..fb4c9b3a 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeE.path +++ b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeE.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.4505122950819673, - "y": 1.1899077868852452 + "x": 1.211, + "y": 0.9501 }, "prevControl": null, "nextControl": { - "x": 1.8034428772883508, - "y": 1.3620003337907018 + "x": 1.6737711250569205, + "y": 0.5407347478369764 }, "isLocked": false, "linkedName": "RightIntake" }, { "anchor": { - "x": 4.761588179976851, - "y": 2.2843272569444433 + "x": 4.8874276620370365, + "y": 1.6390577980324066 }, "prevControl": { - "x": 4.573615089699073, - "y": 1.1070496961805552 + "x": 4.882012080439814, + "y": 0.4239876302083323 }, "nextControl": null, "isLocked": false, diff --git a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeF.path b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeF.path index 7fcc8824..11c82a38 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeF.path +++ b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeF.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4505122950819673, - "y": 1.1899077868852452 + "x": 1.211, + "y": 0.9501 }, "prevControl": null, "nextControl": { - "x": 2.3030878882532644, - "y": 1.4149164674408006 + "x": 2.063575593171297, + "y": 1.1751086805555553 }, "isLocked": false, "linkedName": "RightIntake" diff --git a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeG.path b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeG.path index 8b495844..f12481ea 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeG.path +++ b/ThriftyTest/src/main/deploy/pathplanner/paths/IntakeG.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4505122950819673, - "y": 1.1899077868852452 + "x": 1.211, + "y": 0.9501 }, "prevControl": null, "nextControl": { - "x": 3.6449735485846473, - "y": 1.6806461671240358 + "x": 3.40546125350268, + "y": 1.4408383802387907 }, "isLocked": false, "linkedName": "RightIntake" diff --git a/ThriftyTest/src/main/java/frc/robot/Constants.java b/ThriftyTest/src/main/java/frc/robot/Constants.java index 15e58127..8f3fe9c8 100644 --- a/ThriftyTest/src/main/java/frc/robot/Constants.java +++ b/ThriftyTest/src/main/java/frc/robot/Constants.java @@ -1,1051 +1,226 @@ package frc.robot; -import java.util.List; -import java.util.Map; -import java.util.Set; - -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CANdiConfiguration; -import com.ctre.phoenix6.configs.CANrangeConfiguration; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.DigitalInputsConfigs; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.FovParamsConfigs; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.ProximityParamsConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; -import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.ToFParamsConfigs; -import com.ctre.phoenix6.hardware.CANrange; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.S1CloseStateValue; -import com.ctre.phoenix6.signals.S2CloseStateValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.ctre.phoenix6.signals.UpdateModeValue; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Milliseconds; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.Milliseconds; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularAcceleration; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearAcceleration; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Time; -import edu.wpi.first.wpilibj.PS5Controller.Button; -import frc.robot.generated.TunerConstants; -import frc.robot.utils.Shape; - -/* -PLEASE READ: - -To ensure consistency throughout the code, the same coordinate system is used -here as is specified in WPILib's documentation. - -To read it all, check this out: -https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html - -TL;DR: -We use NWU axes convention. -This means that, when viewed from above, the North, West, and then upwards will -correspond to +X, +Y, and +Z respectively. - -Example: - +X - ^ - | - |-front-| - | | -+Y <--- | | - | | - |-------| - -And +Z is upwards, so it wouldn't show here. -*/ +import frc.robot.subsystems.elevator.ElevatorState; public class Constants { + public static class IDConstants { + public static final int candle1 = 5; + public static final int candle2 = 6; + } + + public static class SimConstants { + public static final double k_simPeriodic = 0.005; + } + + public static class RobotConstants { + public static final Time globalCanTimeout = Milliseconds.of(20); // 20 milliseconds + } + + public static class FieldConstants { + public static final Distance kFieldWidth = Meters.of(8.05); + public static final Distance kFieldLength = Meters.of(17.55); + public static final Translation2d reefCenter = new Translation2d(4.5, 4.0); + public static final double kReefReadyAuton = 2.6; + public static final double kReefReady = 2.1; + public static final Pose2d k_processor = new Pose2d(5.974, 1.16, Rotation2d.kCW_90deg); + + public static final Pose2d kRightIntake = new Pose2d(1.247, 0.950, Rotation2d.fromDegrees(55)); + public static final Pose2d kLeftIntake = new Pose2d(1.211, 7.016, Rotation2d.fromDegrees(-55)); + + public static final Pose2d kGH = new Pose2d(5.791, 4.046, Rotation2d.k180deg); + public static final Pose2d kIJ = new Pose2d(5.155, 5.194, Rotation2d.fromDegrees(-120)); - public static class IDConstants { - public static final int leftRange = 8; - public static final int rightRange = 7; - - public static final int elevatorLeft = 51; - public static final int elevatorRight = 52; - public static final int elevatorCANrange = 53; - - public static final int pivot = 57; - - public static final int coralLeft = 55; - public static final int coralRight = 56; - public static final int coralCANrange = 59; - public static final int upperCANrange = 58; - public static final int innerCANrange = 54; - - public static final int frontIR = 2; - public static final int rearIR = 3; - - public static final int climbLeft = 1; - public static final int climbRight = 2; - - public static final int algae = 60; - - public static final int candle1 = 5; - public static final int candle2 = 6; - - public static final int servo = 7; - - public static final int climbEncoder = 9; - } - - public static class SimConstants { - public static final double k_simPeriodic = 0.005; - } - - public static class RobotConstants { - public static final Time globalCanTimeout = Milliseconds.of(20); // 20 milliseconds - } - - public static class DriveConstants { - public static final PIDConstants k_translationPID = new PIDConstants(2, 0.0, 0.0); // 0.18836 - public static final PIDConstants k_rotationPID = new PIDConstants(1.5, 0.0, 0.0); // 0.17119 - public static final PIDConstants k_driveToPointRotationPID = new PIDConstants(4, 0.0, 0.0); // 0.17119 - - public static final double kMaxAccelerationPerpendicularToTarget = 3.5; // 5.0 - public static double kMaxAccelerationTowardsTarget = 3.5; // 5.0 - - public static final PPHolonomicDriveController k_pathplannerHolonomicDriveController = new PPHolonomicDriveController(k_translationPID, k_rotationPID); - - public static final double k_maxTeleopLinearSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - public static final double k_maxTeleopAngularSpeed = RotationsPerSecond.of(1.5).in(RadiansPerSecond); - - public static final LinearVelocity k_maxLinearSpeed = MetersPerSecond.of(4); - public static final LinearAcceleration k_maxLinearAcceleration = MetersPerSecondPerSecond.of(3); - public static final AngularVelocity k_maxAngularSpeed = RotationsPerSecond.of(2); - public static final AngularAcceleration k_maxAngularAcceleration = RotationsPerSecondPerSecond.of(2); - - public static final LinearVelocity k_maxAlignLinearSpeed = MetersPerSecond.of(1.0); - public static final LinearAcceleration k_maxAlignLinearAcceleration = MetersPerSecondPerSecond.of(1); - public static final AngularVelocity k_maxAlignAngularSpeed = RotationsPerSecond.of(1); - public static final AngularAcceleration k_maxAlignAngularAcceleration = RotationsPerSecondPerSecond.of(1); - - public static final double k_maxRotationalSpeed = k_maxLinearSpeed.in(MetersPerSecond) / (TunerConstants.kWheelRadius.in(Meters) * 2 * Math.PI); // lin speed / circumference = rot speed - - public static final double k_elevatorHeightLinearVelocityGain = -0.357; // for every 1 rotation elevator up, subtract X: 1 mps at max elevator - public static final double k_elevatorHeightLinearAccelerationGain = k_elevatorHeightLinearVelocityGain * 2; - public static final double k_elevatorHeightAngularVelocityGain = -0.0446; // for every 1 rotation elevator up, subtract X: 0.25 rps at max elevator - public static final double k_elevatorHeightAngularAccelerationGain = k_elevatorHeightAngularVelocityGain * 2; - - public static final double k_closedLoopOverrideToleranceTranslation = 0.05; - public static final double k_closedLoopOverrideToleranceRotation = 0.05; - - public static final double rangeZero = 0.175; - public static final double rangeMax = 0.3; - - // This one is as well, however it is only used in auton - public static final Constraints k_rotationConstraints = new Constraints( - k_maxAngularSpeed.in(RadiansPerSecond), - k_maxAngularAcceleration.in(RadiansPerSecondPerSecond) - ); - } - - public static class ButtonBindingConstants { - public static enum DriverChoice {DRAGONREINS, BACKUP;} - public static enum ButtonBoardChoice {PS5, KEYBOARD;} - - public static final DriverChoice driverChoice = DriverChoice.DRAGONREINS; - public static final ButtonBoardChoice buttonBoardChoice = ButtonBoardChoice.PS5; - - public static final String dragonReinsName = "spark"; - public static final String driverBackupName = "inter"; - - public static final String ps5Name = "dual"; - - public static final int driverPort = 0; - public static final int buttonBoardPort = 1; - - public static class DragonReins { - public static final int xAxis = 1; - public static final int yAxis = 0; - public static final int rotAxis = 3; - - public static final boolean flipX = false; - public static final boolean flipY = true; - public static final boolean flipRot = false; - - public static final int resetHeading = 1; - public static final int processor = 2; - - public static final double deadband = 0.01; - } - - public static class PS5 { - public static final int L1 = 180; // POV - public static final int L2 = 270; // POV - public static final int L3 = 90; // POV - public static final int L4 = 0; // POV - - public static final int ejectCoral = Button.kL2.value; - - public static final int leftReef = Button.kSquare.value; - public static final int rightReef = Button.kCircle.value; - - public static final int lowAlgae = Button.kCross.value; - public static final int highAlgae = Button.kTriangle.value; - public static final int groundAlgae = 180; // POV - public static final int processor = 90; // POV - public static final int highGround = 270; // POV - public static final int net = 0; // POV - public static final int algaeModeButton = Button.kR2.value; // R2 - - public static final int autoProcessor = Button.kR1.value; - - public static final int climbReady = Button.kCreate.value; - public static final int climb = Button.kOptions.value; - - public static final int stow = Button.kPS.value; - - public static final int intake = Button.kL1.value; // LB - - public static final int zeroElevator = 15; // old safety mode button (little bar below PS button) - - public static final int secondaryL1 = 14; - } - - public static class ButtonBoardKeyboard { - // WHEN SAFETY ON - AUTOMATION BASED - public static final int L1 = 1; - public static final int L2 = 2; - public static final int L3 = 3; - public static final int L4 = 4; - - public static final int A = 5; - public static final int B = 6; - public static final int C = 7; - public static final int D = 8; - public static final int E = 9; - public static final int F = 10; - public static final int G = 11; - public static final int H = 12; - public static final int I = 13; - public static final int J = 14; - public static final int K = 15; - public static final int L = 16; - - public static final int lowAlgae = 17; - public static final int highAlgae = 18; - public static final int groundAlgae = 19; - public static final int processor = 20; - public static final int net = 21; - - public static final int leftIntake = 22; - public static final int rightIntake = 23; - - public static final int climb = 24; - - public static final int cancelAuto = 25; - } - } - - public static class VisionConstants { - public static final boolean enableVision = true; - public static final boolean k_enableLogging = true; - - public static final double k_rotationCoefficient = Math.PI * 20; - public static final double k_translationCoefficient = 0.10; // previously 0.10 - - public static final AprilTagFieldLayout k_layout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); - - public static final String k_estimationName = "estimation"; - public static final String kRejectedName = "rejected"; - - public static final String k_logPath = "/home/lvuser/logs/vision"; - public static final String k_simLogPath = "logs/vision"; - - private static final double k_moduleHeight = 0.190; - - private static final double k_tightPitch = -Units.degreesToRadians(22.5); - private static final double k_widePitch = -Units.degreesToRadians(25.0); - - private static final double k_tightYaw = Units.degreesToRadians(37.0); // this doesn't seem right - private static final double k_wideYaw = Units.degreesToRadians(-7.0); - - // The camera names - public static Map fakecameras = Map.ofEntries( - Map.entry("test", new Transform3d(0, 0, 1.5, new Rotation3d())) - ); - public static Map cameras = Map.ofEntries( - Map.entry("cam1", new Transform3d( // left tight - new Translation3d(0.256, 0.289, k_moduleHeight), - new Rotation3d(0, k_tightPitch, -k_tightYaw) - )), - Map.entry("cam2", new Transform3d( // left wide - new Translation3d(0.337, 0.331, k_moduleHeight), - new Rotation3d(0, k_widePitch, -k_wideYaw) - )), - Map.entry("cam3", new Transform3d( // right wide - new Translation3d(0.337, -0.331, k_moduleHeight), - new Rotation3d(0, k_widePitch, k_wideYaw) - )), - Map.entry("cam4", new Transform3d( // right tight - new Translation3d(0.256, -0.289, k_moduleHeight), - new Rotation3d(0, k_tightPitch, k_tightYaw) - )) - ); - - public static final String k_leftAlignName = "cam1"; - public static final String k_rightAlignName = "cam4"; - - // The tick time for each pose estimator to run - public static final double k_periodic = 0.02; - // The maximum number of results (per camera) - public static final double k_expectedResults = 10; - // The maximum tolerated latency, in seconds. - public static final double k_latencyThreshold = 0.75; - // The maximum tolerated ambiguity value. - public static final double k_AmbiguityThreshold = 0.2; - // The farthest out off a field a pose estimate can say we are - // (in each dimension separately) - public static final Distance k_XYMargin = Meters.of(0.5); - // The maximum distance from 0 that a camera's pose can report - public static final Distance k_ZMargin = Meters.of(1.5); - - // Some configuration variables: - public static final boolean k_useStdDevs = true; - public static final double k_distanceMultiplier = 5.0; - public static final double k_noisyDistance = 4.0; - public static final double k_ambiguityMultiplier = 0.4; - public static final double k_ambiguityShifter = 0.2; - public static final double k_targetMultiplier = 80; - public static final double k_differenceThreshold = 0.10; - public static final double k_differenceMultiplier = 200.0; - public static final double k_latencyMultiplier = 1.3; - - public static final double k_headingThreshold = Units.degreesToRadians(3); - - // Stats about the camera for simulation - public static final int k_resWidth = 320; - public static final int k_resHeight = 240; - public static final Rotation2d k_fov = Rotation2d.fromDegrees(82.0); - public static final Rotation2d kHorizontalFov = Rotation2d.fromDegrees(70.0); - - - // Simulated error: - public static final Time k_avgLatency = Milliseconds.of(18); - public static final Time k_latencyStdDev = Milliseconds.of(2); - public static final double k_avgErr = 0.18; - public static final double k_errStdDev = 0.02; - - // Stop using vision after X time - public static final double k_visionTimeout = 0.5; - - // reef tag ids (single tag only) - public static final Set k_reefIds = Set.of( - 6, 7, 8, 9, 10, 11, // red tags - 17, 18, 19, 20, 21, 22 // blue tags - ); - } - - public static class FieldConstants { - public static final Distance k_fieldWidth = Meters.of(8.05); - public static final Distance k_fieldLength = Meters.of(17.55); - public static final Translation2d reefCenter = new Translation2d(4.5, 4.0); - public static final double kReefReadyAuton = 2.6; - public static final double kReefReady = 2.1; - public static final Pose2d k_processor = new Pose2d(5.974, 1.16, Rotation2d.kCW_90deg); - - public static final Pose2d kRightIntake = new Pose2d(1.247, 0.950, Rotation2d.fromDegrees(55)); - public static final Pose2d kLeftIntake = new Pose2d(1.211, 7.016, Rotation2d.fromDegrees(-55)); - - public static final Pose2d kGH = new Pose2d(5.791, 4.046, Rotation2d.k180deg); - public static final Pose2d kIJ = new Pose2d(5.155, 5.194, Rotation2d.fromDegrees(-120)); - - public static final Pose2d kBarge1 = new Pose2d(7.459, 4.717, Rotation2d.fromDegrees(21.0)); - } - - public static final class StateSpaceConstants { - public static final double k_dt = 0.01; // fast state space, please! - public static final double k_maxVoltage = 12.0; - } - - public static final class AutonConstants { - public static final double translationTolerance = 0.03; // 0.04 - public static Angle rotationTolerance = Degrees.of(2); - - public static final double driveToPointMaxDistance = 1.5; // beyond X meters, command will insta end - public static final double stage2Distance = 1; - } - - public static final class CANrangeConstants { - - public static final CANrangeConfiguration k_canRangeConfig = new CANrangeConfiguration() - .withFovParams(new FovParamsConfigs() - .withFOVRangeX(7) - .withFOVRangeY(7) - ) - .withToFParams(new ToFParamsConfigs() - .withUpdateMode(UpdateModeValue.ShortRange100Hz) - ); - // .withProximityParams(null) - public static final double farAlignedDistanceMeters = 0.18; - public static final double tolerance = 0.2; // 20% tolerance - public static final double closeAlignedDistanceMeters = 0.12; - public static final int k_filterWindow = 5; // 5 measurements - } - - public static final class ElevatorConstants { - public static final boolean enable = true; - - public static final boolean invertLeftMotorFollower = true; - - public static final double supplyCurrentLimit = 100; - public static final double k_zeroCurrentThreshold = 23.5; - - public static final double rotorToSensorRatio = 5.2; - public static final double sensorToMechanismRatio = 1; - - public static final InvertedValue motorInverted = InvertedValue.CounterClockwise_Positive; - - public static final double gearRatio = rotorToSensorRatio * sensorToMechanismRatio; - - public static final double stage1Mass = Units.lbsToKilograms(5.402); - public static final double stage2Mass = Units.lbsToKilograms(4.819); - public static final double carriageMass = Units.lbsToKilograms(3.084); - public static final double coralMechanismMass = Units.lbsToKilograms(8.173); // includes coral - public static final double algaeMechanismMass = Units.lbsToKilograms(8.359); - - public static final double netMass = stage1Mass + stage2Mass + carriageMass + coralMechanismMass + algaeMechanismMass; // Mass of the elevator carriage - public static final double drumRadius = Units.inchesToMeters(2.256 / 2); // Radius of the elevator drum - // approx. 0.02865 - - public static final double momentOfInertia = netMass * Math.pow(drumRadius, 2); - - public static final LinearSystem stateSpacePlant = LinearSystemId - .createElevatorSystem(KrakenX60FOCConstants.KrakenX60FOCMotor, netMass, drumRadius, gearRatio); - - public static final double absoluteSensorRange = 0.5; - public static final SensorDirectionValue invertEncoder = SensorDirectionValue.CounterClockwise_Positive; - public static final double encoderOffset = 0.291015625 ; //0.490234375 - - public static final double metersToRotations = 1 / (drumRadius * 2 * Math.PI); - // approx 7.96 - - public static final boolean enableCANRange = true; - - public static final double rangeDistanceGain = 64; // how much higher, per unit of range - - /* Please note: - * The maximum height of the elevator (in inches) was calculated to be 80.44 inches. - * Accounting for e rror, we really never should set a setpoint higher than 79 inches (how we chose the net height) - */ - - public static final double inch = Units.inchesToMeters(1) * metersToRotations; - - public static final double groundIntake = 0; - public static final double highGroundIntake = Units.inchesToMeters(12.0) * metersToRotations; - public static final double stow = 0.31; - public static final double eject = stow + 2 * inch; - public static final double processor = 0; - // public static final double L1 = stow + 3.5 * inch; - public static final double L2 = 4.016 + 3 * inch; // 35.5 - public static final double L1 = 2.63; - public static final double secondaryL1 = L1 + 8 * inch; - public static final double L3 = 7.257 - 4 * inch; // 50.5 - public static final double L4 = 9.757 + 0.3 * inch; - public static final double net = 9.31 + 4 * inch; // 67 - short, // 72 - long - public static final double reefLower = 2; - public static final double reefUpper = 4.5; - public static final double prep = L2; - - public static final double forwardSoftLimit = 11.15; - public static final double reverseSoftLimit = 0; - - public static final double unsafeRange = L2 + 2 * inch; - - public static final double tolerance = 0.06; - - public static final double k_maxCanCompensation = 2 * inch; - - public static final double manualUpSpeed = 0.2; - public static final double manualDownSpeed = -0.3; - - public static final double maxSpeedUp = 32; // 16 - public static final double maxAccelerationUp = 48; // 48 - public static final double maxJerkUp = 480; // 480 - - public static final double maxSpeedDown = 10; // 10 - public static final double maxAccelerationDown = 30; // 30 - public static final double maxJerkDown = 300; // 300 - - public static final CANcoderConfiguration encoderConfig = new CANcoderConfiguration() - .withMagnetSensor(new MagnetSensorConfigs() - .withAbsoluteSensorDiscontinuityPoint(absoluteSensorRange) - .withSensorDirection(invertEncoder) - .withMagnetOffset(encoderOffset)); + public static final Pose2d kBargeFromCenter = + new Pose2d(7.459, 4.717, Rotation2d.fromDegrees(21.0)); + public static final Pose2d kBargeFromLeft = new Pose2d(7.5, 6.6, Rotation2d.fromDegrees(21)); - public static final TalonFXConfiguration motorConfig = new TalonFXConfiguration() - .withMotorOutput(new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withInverted(motorInverted)) + public static final Pose2d kStartCenter = new Pose2d(7.076, 3.991, Rotation2d.kPi); + public static final Pose2d kStartRight = new Pose2d(7.076, 1.883, Rotation2d.kPi); + public static final Pose2d kStartLeft = new Pose2d(7.076, 6.027, Rotation2d.kPi); + } - .withFeedback(new FeedbackConfigs() - .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) - .withSensorToMechanismRatio(gearRatio)) + public static final class StateSpaceConstants { + public static final double k_dt = 0.01; // fast state space, please! + public static final double k_maxVoltage = 12.0; + } - .withCurrentLimits(new CurrentLimitsConfigs() - .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(supplyCurrentLimit)) + public static final class TalonFXConstants { + public static final double nominalVoltageVolts = 12.0; // DC Volts + public static final double stallTorqueNewtonMeters = 4.69; // Nm + public static final double stallCurrentAmps = 257.0; // Amps + public static final double freeCurrentAmps = 1.5; // Amps + public static final double freeSpeedRadPerSec = 6380.0 * 2.0 * Math.PI / 60.0; // RPM * 2pi / 60 + // = Rad per + // second - .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitThreshold(forwardSoftLimit) - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitThreshold(reverseSoftLimit) - .withReverseSoftLimitEnable(false)) + public static final double positionStdDevs = 1.0 / 2048.0; // rotations + public static final double velocityStdDevs = 2.0 / 2048.0; // rotations - .withSlot0(new Slot0Configs() - .withGravityType(GravityTypeValue.Elevator_Static) - .withKP(20) - .withKI(0) - .withKD(0) - .withKS(0.125) - .withKV(3.59 * (drumRadius * 2 * Math.PI)) - .withKA(0.05 * (drumRadius * 2 * Math.PI)) - .withKG(0.42)) + public static final DCMotor TalonFXDCMotor = + new DCMotor(nominalVoltageVolts, stallTorqueNewtonMeters, + stallCurrentAmps, freeCurrentAmps, freeSpeedRadPerSec, 1); + } + + public static final class KrakenX60Constants { + public static final double nominalVoltageVolts = 12.0; + public static final double stallTorqueNewtonMeters = 7.16; + public static final double stallCurrentAmps = 374.38; + public static final double freeCurrentAmps = 2.0; + public static final double freeSpeedRadPerSec = Units.rotationsToRadians(6000); + public static final double positionStdDevs = 1.0 / 2048.0; + public static final double velocityStdDevs = 2.0 / 2048.0; - .withSlot1(new Slot1Configs() - .withGravityType(GravityTypeValue.Elevator_Static) - .withKP(7) - .withKI(0) - .withKD(0) - .withKS(0) - .withKV(3.59 * (drumRadius * 2 * Math.PI)) - .withKA(0.05 * (drumRadius * 2 * Math.PI)) - .withKG(0.42)) + public static final DCMotor KrakenX60Motor = + new DCMotor(nominalVoltageVolts, stallTorqueNewtonMeters, + stallCurrentAmps, freeCurrentAmps, freeSpeedRadPerSec, 1); + } + + public static final class KrakenX60FOCConstants { + public static final double nominalVoltageVolts = 12.0; + public static final double stallTorqueNewtonMeters = 9.37; + public static final double stallCurrentAmps = 483; + public static final double freeCurrentAmps = 2.0; + public static final double freeSpeedRadPerSec = Units.rotationsToRadians(5800); + public static final double positionStdDevs = 1.0 / 2048.0; + public static final double velocityStdDevs = 2.0 / 2048.0; + + public static final DCMotor KrakenX60FOCMotor = + new DCMotor(nominalVoltageVolts, stallTorqueNewtonMeters, + stallCurrentAmps, freeCurrentAmps, freeSpeedRadPerSec, 1); + } + + public enum ScoringLocations { + A(new Pose2d(3.188, 4.191, Rotation2d.fromDegrees(0))), // GOOD + B(new Pose2d(3.188, 3.861, Rotation2d.fromDegrees(0))), // GOOD + + C(new Pose2d(3.72, 2.982, Rotation2d.fromDegrees(58.7))), // GOOD + D(new Pose2d(3.967, 2.810, Rotation2d.fromDegrees(58.2))), // GOOD + + E(new Pose2d(4.998, 2.816, Rotation2d.fromDegrees(120))), // GOOD + F(new Pose2d(5.283, 2.981, Rotation2d.fromDegrees(120))), // GOOD + + G(new Pose2d(5.791, 3.861, Rotation2d.fromDegrees(180))), // GOOD + H(new Pose2d(5.791, 4.191, Rotation2d.fromDegrees(180))), // GOOD + + I(new Pose2d(5.283, 5.071, Rotation2d.fromDegrees(-120))), // GOOD + J(new Pose2d(4.998, 5.236, Rotation2d.fromDegrees(-120))), // GOOD + + K(new Pose2d(3.951, 5.236, Rotation2d.fromDegrees(-60))), // GOOD + L(new Pose2d(3.696, 5.071, Rotation2d.fromDegrees(-60))), // GOOD + + RightIntake(new Pose2d(1.227, 1.048, Rotation2d.fromDegrees(55))), // Right intake station + LeftIntake(new Pose2d(1.227, 6.983, Rotation2d.fromDegrees(-55))), // Left intake station + + PROCESSOR(new Pose2d(6.0, 0.6, Rotation2d.fromDegrees(-90))), + + NET(new Pose2d(7.7, 6.0, Rotation2d.fromDegrees(0))); + + public Pose2d value; + + private ScoringLocations(Pose2d value) { + this.value = value; + } + + private Pose2d middle(ScoringLocations other) { + return value.interpolate(other.value, 0.5); + } + } + + public enum ScoringLocationsLeft { + A(ScoringLocations.A.value), C(ScoringLocations.C.value), E(ScoringLocations.E.value), G( + ScoringLocations.G.value), I(ScoringLocations.I.value), K(ScoringLocations.K.value); + + public Pose2d value; + + private ScoringLocationsLeft(Pose2d value) { + this.value = value; + } + } + + public enum ScoringLocationsRight { + B(ScoringLocations.B.value), D(ScoringLocations.D.value), F(ScoringLocations.F.value), H( + ScoringLocations.H.value), J(ScoringLocations.J.value), L(ScoringLocations.L.value); + + public Pose2d value; + + private ScoringLocationsRight(Pose2d value) { + this.value = value; + } + } + + public enum ScoringLocationsCenter { + /** AB */ + AB(ScoringLocations.A.middle(ScoringLocations.B)), + /** CD */ + CD(ScoringLocations.C.middle(ScoringLocations.D)), + /** EF */ + EF(ScoringLocations.E.middle(ScoringLocations.F)), + /** GH */ + GH(ScoringLocations.G.middle(ScoringLocations.H)), + /** IJ */ + IJ(ScoringLocations.I.middle(ScoringLocations.J)), + /** KL */ + KL(ScoringLocations.K.middle(ScoringLocations.L)); + + public Pose2d value; + + private ScoringLocationsCenter(Pose2d pose) { + value = pose; + } + } + + public static class LedConstants { + public static final int numLED = 133; + public static final double flashSpeed = 0.75; + public static final double strobeSpeed = 0.1; + public static final double endgameWarning = 30; + public static final double endgameAlert = 15; + public static final int funnelOffset = 8; // 8 + public static final int elevatorOffset = 95; // 94 + public static final int funnelNumLED = 81; // 85 + public static final int elevatorNumLED = 40; // 40 + public static final int funnelOffset2 = 8; // 8 + public static final int elevatorOffset2 = 93; // 94 + public static final int funnelNumLED2 = elevatorOffset2 - funnelOffset2; // 85 + public static final int elevatorNumLED2 = 40; // 40 - .withMotionMagic(new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(maxSpeedUp) - .withMotionMagicAcceleration(maxAccelerationUp) - .withMotionMagicJerk(maxJerkUp)); + public static final String dragonReinsName = "spark"; + public static final String ps5Name = "dual"; + } - public static final CANrangeConfiguration kCANrangeConfig = new CANrangeConfiguration() - .withFovParams(new FovParamsConfigs() - .withFOVRangeX(6.75) - .withFOVRangeY(6.75)) - .withProximityParams(new ProximityParamsConfigs() - .withMinSignalStrengthForValidMeasurement(3500) - .withProximityThreshold(0.13) - .withProximityHysteresis(0)); + public static class FFConstants { + public static final double k_bargeX = 8.774176; + public static final double k_radius = 1.27; + public static final double k_decceleration = 6.0; + } - public static final Time kRangeDebounceTime = Seconds.of(0.06); - } - - public static final class PivotConstants { - public static final double encoderOffset = 0.665283203125; - - public static final double rotorOffset = 0.344; - - public static final double rotorToSensorRatio = 64.0 / 14.0; - public static final double sensorToMechanismRatio = 32.0 / 14.0; - - public static final InvertedValue invertMotor = InvertedValue.CounterClockwise_Positive; - public static final SensorDirectionValue invertEncoder = SensorDirectionValue.Clockwise_Positive; - - public static final double forwardSoftLimitThreshold = 0.359; - public static final double reverseSoftLimitThreshold = 0.0; - - public static final double radiansAtMax = forwardSoftLimitThreshold; - public static final double radiansAtZero = 0; - - public static final double absoluteSensorRange = 0.5; - - public static final double supplyCurrentLimit = 40; - - public static final double tolerance = 0.03; - - public static final double groundPickup = 0.0669; - public static final double processor = 0.085; - public static final double reefPickup = 0.2; - public static final double reefExtract = 0.281; - public static final double net = 0.342; - public static final double stow = 0.343; - - public static final double manualUpSpeed = 0.1; - public static final double manualDownSpeed = -0.1; - - public static final double momentOfIntertia = 0.14622; - public static final double gearRatio = rotorToSensorRatio * sensorToMechanismRatio; - - public static final LinearSystem stateSpacePlant = LinearSystemId - .createSingleJointedArmSystem(TalonFXConstants.TalonFXDCMotor, momentOfIntertia, gearRatio); - - public static final double maxSpeed = 1.5; // cancoder rotations per second - public static final double accelerationMultiplier = 2; - - public static final CANcoderConfiguration encoderConfig = new CANcoderConfiguration() - .withMagnetSensor(new MagnetSensorConfigs() - .withAbsoluteSensorDiscontinuityPoint(absoluteSensorRange) - .withSensorDirection(invertEncoder) - .withMagnetOffset(encoderOffset)); - - public static final TalonFXConfiguration motorConfig = new TalonFXConfiguration() - .withMotorOutput(new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withInverted(invertMotor)) - - // .withFeedback(new FeedbackConfigs() - // .withFeedbackRemoteSensorID(IDConstants.pivotEncoder) - // .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) - // .withRotorToSensorRatio(rotorToSensorRatio) - // .withSensorToMechanismRatio(sensorToMechanismRatio)) - .withFeedback(new FeedbackConfigs() - .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) - .withSensorToMechanismRatio(gearRatio)) - - .withCurrentLimits(new CurrentLimitsConfigs() - .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(supplyCurrentLimit)) - - .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitThreshold(forwardSoftLimitThreshold) - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitThreshold(reverseSoftLimitThreshold) - .withReverseSoftLimitEnable(true)) - - .withSlot0(new Slot0Configs() - .withGravityType(GravityTypeValue.Arm_Cosine) - .withKP(25) - .withKI(0) - .withKD(0) - .withKS(0) - .withKV(1.3) - .withKA(0.12) - .withKG(0.625)) - .withSlot1(new Slot1Configs() - .withGravityType(GravityTypeValue.Arm_Cosine) - .withKP(30) - .withKI(0) - .withKD(0) - .withKS(0) - .withKV(1.3) - .withKA(0.12) - .withKG(0.85)) - - .withMotionMagic(new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(maxSpeed) - .withMotionMagicAcceleration(maxSpeed * accelerationMultiplier) - .withMotionMagicJerk(maxSpeed * accelerationMultiplier * 10)); - - public static final double armLength = 0.443; - } - - public static class CoralConstants { - public static final double intakeVoltage = 2.4; - public static final double retractVoltage = -3.5; - public static final double ejectVoltage = 5; - - public static final double l1EjectVoltage = 2.5; - public static final double l2EjectVoltage = 4.0; // 5.1 - public static final double l3EjectVoltage = 4.0; // 5.1 - public static final double l4EjectVoltage = 5.5; - - public static final double rangeDistanceGain = 13; // how many more volts, per unit of range - - public static final double spitOutVoltage = -6; - public static final double fastEjectVoltage = -10; - - public static final double l1LeftEjectVoltage = 2; - public static final double l1RightEjectVoltage = 4; - - public static final boolean rightMotorInvert = true; - - public static final double supplyCurrentLimit = 20.0; - - public static final double IRThreshold = 0.51; - - public static final boolean enableCANRange = true; - - public static final InvertedValue kInvertRight = InvertedValue.Clockwise_Positive; - - public static final TalonFXConfiguration motorConfig = new TalonFXConfiguration() - .withMotorOutput(new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Coast) - .withInverted(InvertedValue.CounterClockwise_Positive)) - - .withCurrentLimits(new CurrentLimitsConfigs() - .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(supplyCurrentLimit)); - - public static final CANdiConfiguration candiConfig = new CANdiConfiguration() - .withDigitalInputs(new DigitalInputsConfigs() - .withS1CloseState(S1CloseStateValue.CloseWhenHigh) - .withS2CloseState(S2CloseStateValue.CloseWhenHigh)); - - public static final CANrangeConfiguration frontRangeConfig = new CANrangeConfiguration() - .withFovParams(new FovParamsConfigs() - .withFOVRangeX(6.5) - .withFOVRangeY(6.5)) - .withProximityParams(new ProximityParamsConfigs() - .withMinSignalStrengthForValidMeasurement(15015) - .withProximityThreshold(0.1)) - .withToFParams(new ToFParamsConfigs() - .withUpdateMode(UpdateModeValue.ShortRange100Hz)); - - - public static final CANrangeConfiguration upperRangeConfig = new CANrangeConfiguration() - .withFovParams(new FovParamsConfigs() - .withFOVRangeX(6.5) - .withFOVRangeY(15)) - .withProximityParams(new ProximityParamsConfigs() - .withMinSignalStrengthForValidMeasurement(2500) - .withProximityThreshold(0.65)) - .withToFParams(new ToFParamsConfigs() - .withUpdateMode(UpdateModeValue.ShortRange100Hz)); - - public static final CANrangeConfiguration innerRangeConfig = new CANrangeConfiguration() - .withFovParams(new FovParamsConfigs() - .withFOVRangeX(27) - .withFOVRangeY(27)) - .withProximityParams(new ProximityParamsConfigs() - .withMinSignalStrengthForValidMeasurement(1500) - .withProximityHysteresis(0) - .withProximityThreshold(0.06)) - .withToFParams(new ToFParamsConfigs() - .withUpdateMode(UpdateModeValue.ShortRange100Hz)); - - public static double intakeTimeout = 1; - } - - public static final class ClimberConstants { - public static final boolean rightMotorInvert = true; - public static final double climberUpVolts = 12.0; // 12.0 - public static final double climbDownVolts = -12.0; - public static final double climbRollVolts = -4; - - public static final double climberCurrentLimit = 80.0; - public static final InvertedValue invertMotor = InvertedValue.CounterClockwise_Positive; - - public static final double forwardSoftLimit = 0.0; - public static final double reverseSoftLimit = -0.25; - public static final double climbPosition = -0.110; - public static final double kShakePosition = -0.02; - - public static final double encoderOffset = -0.01318359; - public static final SensorDirectionValue invertEncoder = SensorDirectionValue.CounterClockwise_Positive; - - public static final CANcoderConfiguration encoderConfig = new CANcoderConfiguration() - .withMagnetSensor(new MagnetSensorConfigs() - .withAbsoluteSensorDiscontinuityPoint(0.5) - .withSensorDirection(invertEncoder) - .withMagnetOffset(encoderOffset)); - - public static final TalonFXConfiguration motorConfig = new TalonFXConfiguration() - .withMotorOutput(new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withInverted(invertMotor)) - - .withCurrentLimits(new CurrentLimitsConfigs() - .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(climberCurrentLimit)) - - .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitThreshold(forwardSoftLimit) - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitThreshold(reverseSoftLimit) - .withReverseSoftLimitEnable(true)) - - .withFeedback(new FeedbackConfigs() - .withFeedbackRemoteSensorID(IDConstants.climbEncoder) - .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)); - - public static final double climbReadyRangeValue = 0.08; - public static final double climbedRangeValue = 0.145; - - public static final double climbMaxEncoderValue = 63.833; - public static final double climbReadyMaxEncoderValue = 90; - - public static final double k_openServoPosition = 0.0; - public static final double k_closedServoPosition = 1.0; - public static final double k_servoTolerance = 0.01; - - public static final double climbReadyTolerance = -0.001; - - public static final double kClimbTime = 30.0; - } - - public static final class AlgaeRollerConstants { - public static final double intakeVoltage = 12; - public static final double ejectVoltage = -3.0; // 3.0 - public static final double processorEjectVoltage = -3.2; + public enum CoralLevel { + L1, SecondaryL1, L2, L3, L4; - public static final double torqueCurrentThreshold = 75; - - public static final double supplyCurrentLimit = 25.0; - - public static final double holdVoltage = 2.7; - public static final double k_updateObjectPeriodSeconds = 0.200; // 200 milliseconds - public static final InvertedValue invertMotor = InvertedValue.Clockwise_Positive; - public static final double algaeEjectTime = 0.4; // was 0.6 but i want faster when we're done - public static final double reefPickupSafetyDistance = 1.75; - - public static final TalonFXConfiguration motorConfig = new TalonFXConfiguration() - .withMotorOutput(new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withInverted(invertMotor)) - - .withCurrentLimits(new CurrentLimitsConfigs() - .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(supplyCurrentLimit)); - } - - public static final class TalonFXConstants { - public static final double nominalVoltageVolts = 12.0; // DC Volts - public static final double stallTorqueNewtonMeters = 4.69; // Nm - public static final double stallCurrentAmps = 257.0; // Amps - public static final double freeCurrentAmps = 1.5; // Amps - public static final double freeSpeedRadPerSec = 6380.0 * 2.0 * Math.PI / 60.0; // RPM * 2pi / 60 = Rad per - // second - - public static final double positionStdDevs = 1.0 / 2048.0; // rotations - public static final double velocityStdDevs = 2.0 / 2048.0; // rotations - - public static final DCMotor TalonFXDCMotor = new DCMotor(nominalVoltageVolts, stallTorqueNewtonMeters, - stallCurrentAmps, freeCurrentAmps, freeSpeedRadPerSec, 1); - } - - public static final class KrakenX60Constants { - public static final double nominalVoltageVolts = 12.0; - public static final double stallTorqueNewtonMeters = 7.16; - public static final double stallCurrentAmps = 374.38; - public static final double freeCurrentAmps = 2.0; - public static final double freeSpeedRadPerSec = Units.rotationsToRadians(6000); - public static final double positionStdDevs = 1.0 / 2048.0; - public static final double velocityStdDevs = 2.0 / 2048.0; - - public static final DCMotor KrakenX60Motor = new DCMotor(nominalVoltageVolts, stallTorqueNewtonMeters, - stallCurrentAmps, freeCurrentAmps, freeSpeedRadPerSec, 1); - } - - public static final class KrakenX60FOCConstants { - public static final double nominalVoltageVolts = 12.0; - public static final double stallTorqueNewtonMeters = 9.37; - public static final double stallCurrentAmps = 483; - public static final double freeCurrentAmps = 2.0; - public static final double freeSpeedRadPerSec = Units.rotationsToRadians(5800); - public static final double positionStdDevs = 1.0 / 2048.0; - public static final double velocityStdDevs = 2.0 / 2048.0; - - public static final DCMotor KrakenX60FOCMotor = new DCMotor(nominalVoltageVolts, stallTorqueNewtonMeters, - stallCurrentAmps, freeCurrentAmps, freeSpeedRadPerSec, 1); - } - - public enum ScoringLocations { - A(new Pose2d(3.188, 4.191, Rotation2d.fromDegrees(0))), // GOOD - B(new Pose2d(3.188, 3.861, Rotation2d.fromDegrees(0))), // GOOD - - C(new Pose2d(3.72, 2.982, Rotation2d.fromDegrees(58.7))), // GOOD - D(new Pose2d(3.967, 2.810, Rotation2d.fromDegrees(58.2))), // GOOD - - E(new Pose2d(4.998, 2.816, Rotation2d.fromDegrees(120))), // GOOD - F(new Pose2d(5.283, 2.981, Rotation2d.fromDegrees(120))), // GOOD - - G(new Pose2d(5.791, 3.861, Rotation2d.fromDegrees(180))), // GOOD - H(new Pose2d(5.791, 4.191, Rotation2d.fromDegrees(180))), // GOOD - - I(new Pose2d(5.283, 5.071, Rotation2d.fromDegrees(-120))), // GOOD - J(new Pose2d(4.998, 5.236, Rotation2d.fromDegrees(-120))), // GOOD - - K(new Pose2d(3.951, 5.236, Rotation2d.fromDegrees(-60))), // GOOD - L(new Pose2d(3.696, 5.071, Rotation2d.fromDegrees(-60))), // GOOD - - RIGHTHP(new Pose2d(1.227, 1.048, Rotation2d.fromDegrees(55))), - LEFTHP(new Pose2d(1.227, 6.983, Rotation2d.fromDegrees(-55))), - PROCESSOR(new Pose2d(6.0, 0.6, Rotation2d.fromDegrees(-90))), - NET(new Pose2d(7.7, 6.0, Rotation2d.fromDegrees(0))); - - public Pose2d value; - - private ScoringLocations(Pose2d value) { - this.value = value; - } - } - - public enum ScoringLocationsLeft { - A(ScoringLocations.A.value), - C(ScoringLocations.C.value), - E(ScoringLocations.E.value), - G(ScoringLocations.G.value), - I(ScoringLocations.I.value), - K(ScoringLocations.K.value); - - public Pose2d value; - - private ScoringLocationsLeft(Pose2d value) { - this.value = value; - } - } - - public enum ScoringLocationsRight { - B(ScoringLocations.B.value), - D(ScoringLocations.D.value), - F(ScoringLocations.F.value), - H(ScoringLocations.H.value), - J(ScoringLocations.J.value), - L(ScoringLocations.L.value); - - public Pose2d value; - - private ScoringLocationsRight(Pose2d value) { - this.value = value; - } - } - - public enum ScoringLocationsMiddle { - AB(ScoringLocations.A.value.interpolate(ScoringLocations.B.value, 0.5)), - CD(ScoringLocations.C.value.interpolate(ScoringLocations.D.value, 0.5)), - EF(ScoringLocations.E.value.interpolate(ScoringLocations.F.value, 0.5)), - GH(ScoringLocations.G.value.interpolate(ScoringLocations.H.value, 0.5)), - IJ(ScoringLocations.I.value.interpolate(ScoringLocations.J.value, 0.5)), - KL(ScoringLocations.K.value.interpolate(ScoringLocations.L.value, 0.5)); - - public Pose2d value; - - private ScoringLocationsMiddle(Pose2d value) { - this.value = value; - } - } - - public enum ClimbLocations { - WALL(new Pose2d(8.5, 7.26, Rotation2d.fromDegrees(0))), - MIDDLE(new Pose2d(8.5, 6.1, Rotation2d.fromDegrees(0))), - CENTER(new Pose2d(8.5, 5.0, Rotation2d.fromDegrees(0))); - - public Pose2d value; - - private ClimbLocations(Pose2d value) { - this.value = value; - } - } - - public enum ReefClipLocations { - LEFT, RIGHT; - } - - public static final class CommandBounds { - // 1 robot of space around the entire reef - public static final List reef = List.of( - new Translation2d(2.729, 3.013), - new Translation2d(4.498, 1.975), - new Translation2d(6.242, 3.013), - new Translation2d(6.242, 5.024), - new Translation2d(4.498, 6.010), - new Translation2d(2.729, 5.024) - ); - public static final Shape reefBounds = Shape.fromUnsortedVertices(reef, "Reef"); - - // 1.5 robot of space away from the opposite alliance barge side intake - public static final List leftIntake = List.of( - new Translation2d(0.0, 1.25), - new Translation2d(1.7, 0.0), - new Translation2d(3.2, 0.0), - new Translation2d(0.0, 2.35) - ); - public static final Shape leftIntakeBounds = Shape.fromUnsortedVertices(leftIntake, "Left Intake"); - - // 1.5 robot of space away from the same alliance barge side intake - public static final Shape rightIntakeBounds = Shape.flipHotdog(leftIntakeBounds, "Right Intake"); - - // processor where we score - public static final List oppositeAllianceProcessor = List.of( - new Translation2d(5.5, 0.0), - new Translation2d(6.5, 0.0), - new Translation2d(6.5, 1), - new Translation2d(5.5, 1) - ); - public static final Shape processorBounds = Shape.fromUnsortedVertices(oppositeAllianceProcessor, "Processor"); - - // net where we score - public static final List net = List.of( - new Translation2d(7.2, 4.25), - new Translation2d(10.3, 4.25), - new Translation2d(10.3, 8), - new Translation2d(7.2, 8) - ); - public static final Shape netBounds = Shape.fromUnsortedVertices(net, "Net"); - - public static final List tooClose = List.of( - new Translation2d(8.6 ,4.25 ), - new Translation2d(11.7,4.25), - new Translation2d(11.7,8), - new Translation2d(8.6,8) - - ); - - public static final Shape netTooCloseBounds = Shape.fromUnsortedVertices(tooClose, "NoNet"); - - public static Map displayBounds = Map.ofEntries( - Map.entry("Blue Alliance Reef", reefBounds), - Map.entry("Blue Alliance Net", netBounds), - Map.entry("Blue Alliance Left Intake", leftIntakeBounds), - Map.entry("Blue Alliance Right Intake", rightIntakeBounds), - Map.entry("Blue Alliance Processor", processorBounds), - Map.entry("Red Alliance Reef", reefBounds.flip()), - Map.entry("Red Alliance Net", netBounds.flip()), - Map.entry("Red Alliance Left Intake", leftIntakeBounds.flip()), - Map.entry("Red Alliance Right Intake", rightIntakeBounds.flip()), - Map.entry("Red Alliance Processor", processorBounds.flip()) - ); - } - - public static class LedConstants { - public static final int numLED = 133; - public static final double flashSpeed = 0.75; - public static final double strobeSpeed = 0.1; - public static final double endgameWarning = 30; - public static final double endgameAlert = 15; - public static final int funnelOffset = 8; // 8 - public static final int elevatorOffset = 95; // 94 - public static final int funnelNumLED = 81; // 85 - public static final int elevatorNumLED = 40; // 40 - public static final int funnelOffset2 = 8; // 8 - public static final int elevatorOffset2 = 93; // 94 - public static final int funnelNumLED2 = elevatorOffset2 - funnelOffset2; // 85 - public static final int elevatorNumLED2 = 40; // 40 - } - - public static class FFConstants { - public static final double k_bargeX = 8.774176; - public static final double k_radius = 1.27; - public static final double k_decceleration = 6.0; - } + public ElevatorState toElevatorState() { + switch (this) { + case L1: + return ElevatorState.L1; + case L2: + return ElevatorState.L2; + case L3: + return ElevatorState.L3; + case L4: + return ElevatorState.L4; + case SecondaryL1: + return ElevatorState.SecondaryL1; + default: + return ElevatorState.Stow; + } + } + } } diff --git a/ThriftyTest/src/main/java/frc/robot/Main.java b/ThriftyTest/src/main/java/frc/robot/Main.java index 25fa2ae5..fe215d73 100644 --- a/ThriftyTest/src/main/java/frc/robot/Main.java +++ b/ThriftyTest/src/main/java/frc/robot/Main.java @@ -7,10 +7,9 @@ import edu.wpi.first.wpilibj.RobotBase; public final class Main { - private Main() { - } + private Main() {} - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } } diff --git a/ThriftyTest/src/main/java/frc/robot/Robot.java b/ThriftyTest/src/main/java/frc/robot/Robot.java index b1d06c5a..506b267d 100644 --- a/ThriftyTest/src/main/java/frc/robot/Robot.java +++ b/ThriftyTest/src/main/java/frc/robot/Robot.java @@ -4,97 +4,123 @@ package frc.robot; -import com.pathplanner.lib.commands.FollowPathCommand; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.utils.LoopTimer; +import frc.robot.utils.OnboardLogger; +import frc.robot.utils.StatusSignalUtil; public class Robot extends TimedRobot { - private Command m_autonomousCommand; - - private final RobotContainer m_robotContainer; - - public Robot() { - m_robotContainer = new RobotContainer(); - } - - @Override - public void robotInit() { - FollowPathCommand.warmupCommand().schedule(); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(0); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("stream").setNumber(0); - m_robotContainer.enablePDPSwitch(); - } - - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - } - - @Override - public void disabledInit() { - } - - @Override - public void disabledPeriodic() { - } - - @Override - public void disabledExit() { - } - - @Override - public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - m_robotContainer.resetReferences(); - } - - @Override - public void autonomousPeriodic() { - } - - @Override - public void autonomousExit() { - } - - @Override - public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } - m_robotContainer.resetReferences(); - m_robotContainer.setupAutoclimb(); - } - - @Override - public void teleopPeriodic() { - } - - @Override - public void teleopExit() { - } - - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } - - @Override - public void testPeriodic() { - } - - @Override - public void testExit() { - } - - @Override - public void simulationPeriodic() { - } + private final Logger m_logger = LoggerFactory.getLogger(Robot.class); + private Command m_autonomousCommand; + + private final RobotContainer m_robotContainer; + + private final LoopTimer m_loopTimer; + + private final OnboardLogger m_ologger; + + public Robot() { + m_robotContainer = new RobotContainer(); + m_loopTimer = new LoopTimer("Robot"); + m_ologger = new OnboardLogger("Robot"); + m_ologger.registerDouble("Battery Voltage", RobotController::getBatteryVoltage); + } + + @Override + public void robotInit() { + RobotObserver.setField(new Field2d()); + DataLogManager.start(); + DriverStation.startDataLog(DataLogManager.getLog()); + m_robotContainer.enablePDPSwitch(); + SmartDashboard.putData("Command Scheduler", CommandScheduler.getInstance()); + LiveWindow.disableAllTelemetry(); + if (isSimulation()) { + CommandScheduler.getInstance().onCommandInitialize(command -> { + m_logger.trace("Starting: {}", command.getName()); + }); + CommandScheduler.getInstance().onCommandFinish(command -> { + m_logger.trace("Ended: {}", command.getName()); + }); + CommandScheduler.getInstance().onCommandInterrupt((dead, reason) -> { + reason.ifPresentOrElse(killer -> { + m_logger.trace("Killed by {}: {}", killer.getName(), dead.getName()); + }, () -> { + m_logger.trace("Cancelled: {}", dead.getName()); + }); + }); + } + } + + @Override + public void robotPeriodic() { + m_loopTimer.reset(); + StatusSignalUtil.refreshAll(); + CommandScheduler.getInstance().run(); + SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); + m_robotContainer.updateAlgaeTracking(); + m_ologger.log(); + m_loopTimer.log(); + } + + @Override + public void disabledInit() {} + + @Override + public void disabledPeriodic() {} + + @Override + public void disabledExit() {} + + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + @Override + public void autonomousPeriodic() {} + + @Override + public void autonomousExit() {} + + @Override + public void teleopInit() { + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + @Override + public void teleopPeriodic() {} + + @Override + public void teleopExit() {} + + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); + } + + @Override + public void testPeriodic() {} + + @Override + public void testExit() {} + + @Override + public void simulationPeriodic() {} } diff --git a/ThriftyTest/src/main/java/frc/robot/RobotContainer.java b/ThriftyTest/src/main/java/frc/robot/RobotContainer.java index d79a666d..dc368ecb 100644 --- a/ThriftyTest/src/main/java/frc/robot/RobotContainer.java +++ b/ThriftyTest/src/main/java/frc/robot/RobotContainer.java @@ -1,563 +1,76 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot; -import java.util.ArrayList; -import java.util.List; -import java.util.Set; -import java.util.function.Supplier; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import static edu.wpi.first.units.Units.MetersPerSecond; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.DriverStation.MatchType; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.DeferredCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import frc.robot.Constants.AlgaeRollerConstants; -import frc.robot.Constants.ButtonBindingConstants; -import frc.robot.Constants.ButtonBindingConstants.DragonReins; -import frc.robot.Constants.ButtonBindingConstants.PS5; -import frc.robot.Constants.ClimbLocations; -import frc.robot.Constants.ClimberConstants; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.FieldConstants; -import frc.robot.Constants.ReefClipLocations; -import frc.robot.Constants.ScoringLocations; -import frc.robot.Constants.ScoringLocationsLeft; -import frc.robot.Constants.ScoringLocationsMiddle; -import frc.robot.Constants.ScoringLocationsRight; -import frc.robot.Constants.VisionConstants; -import frc.robot.commands.ClimbReadyCommand; -import frc.robot.commands.ClimberCommand; -import frc.robot.commands.CoralEjectCommand; -import frc.robot.commands.DriveToPointCommand; -import frc.robot.commands.ElevatorDefaultCommand; -import frc.robot.commands.ElevatorToPointCommand; -import frc.robot.commands.ElevatorZero; -import frc.robot.commands.OpenFunnel; -import frc.robot.commands.PitClimbSetupCommand; -import frc.robot.commands.StowCommand; -import frc.robot.commands.TeleopCommand; +import frc.robot.binding.Binder; +import frc.robot.binding.DashboardBindings; +import frc.robot.binding.DriveBindings; +import frc.robot.binding.NamedCommandBindings; +import frc.robot.binding.OperatorBindings; +import frc.robot.binding.RobotBindings; import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.AlgaeRollers; -import frc.robot.subsystems.Climber; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.CoralRollers; -import frc.robot.subsystems.Elevator; import frc.robot.subsystems.LedFeedback; -import frc.robot.subsystems.Pivot; -import frc.robot.utils.AutonomousUtil; -import frc.robot.utils.FieldUtils; -import frc.robot.vision.VisionHandler; +import frc.robot.subsystems.algae.Algae; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.coral.Coral; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.pivot.Pivot; +import frc.robot.superstructure.Superstructure; public class RobotContainer { - private final Logger m_logger = LoggerFactory.getLogger(RobotContainer.class); - - @SuppressWarnings("unused") - private final Telemetry m_telemetry = new Telemetry(TunerConstants.kSpeedAt12Volts.in(MetersPerSecond)); - - public final CommandSwerveDrivetrain m_drivetrain = TunerConstants.createDrivetrain(); - - private final VisionHandler m_vision = new VisionHandler(m_drivetrain); - - private final PowerDistribution pdp = new PowerDistribution(1, ModuleType.kRev); - - public RobotContainer() { - configureSysId(); - configureSubsystems(); - generateScoringLocations(); - configureNamedCommands(); - configureDriverBindings(); - configureOperatorBindings(); - configureAutonChooser(); - configureVision(); - configureTesting(); - configureDashboard(); - confiureSimulation(); - RobotObserver.setFFEnabledSupplier(this::getFFEnabled); - } - - private void configureSysId() { - SmartDashboard.putData("quasistatic forward steer", m_drivetrain.sysIdQuasistaticSteer(Direction.kForward)); - SmartDashboard.putData("quasistatic reverse steer", m_drivetrain.sysIdQuasistaticSteer(Direction.kReverse)); - SmartDashboard.putData("dynamic forward steer", m_drivetrain.sysIdDynamicSteer(Direction.kForward)); - SmartDashboard.putData("dynamic reverse steer", m_drivetrain.sysIdDynamicSteer(Direction.kReverse)); - - SmartDashboard.putData("quasistatic forward translation", m_drivetrain.sysIdQuasistaticTranslation(Direction.kForward)); - SmartDashboard.putData("quasistatic reverse translation", m_drivetrain.sysIdQuasistaticTranslation(Direction.kReverse)); - SmartDashboard.putData("dynamic forward translation", m_drivetrain.sysIdDynamicTranslation(Direction.kForward)); - SmartDashboard.putData("dynamic reverse translation", m_drivetrain.sysIdDynamicTranslation(Direction.kReverse)); - } - - public List scoringLocationsListLeft; - public List scoringLocationsRightList; - public List scoringLocationsMiddleList; - public List climbLocationsList; - - private void generateScoringLocations() { - // initializes lists of poses of all the enums - to use in auton util - ScoringLocationsLeft[] locationsLeft = ScoringLocationsLeft.values(); - this.scoringLocationsListLeft = new ArrayList<>(); - for (ScoringLocationsLeft location : locationsLeft) { - this.scoringLocationsListLeft.add(location.value); - } - - ScoringLocationsRight[] locationsRight = ScoringLocationsRight.values(); - this.scoringLocationsRightList = new ArrayList<>(); - for (ScoringLocationsRight location : locationsRight) { - this.scoringLocationsRightList.add(location.value); - } - - ScoringLocationsMiddle[] locationsMiddle = ScoringLocationsMiddle.values(); - this.scoringLocationsMiddleList = new ArrayList<>(); - for (ScoringLocationsMiddle location : locationsMiddle) { - this.scoringLocationsMiddleList.add(location.value); - } - - ClimbLocations[] climbLocations = ClimbLocations.values(); - this.climbLocationsList = new ArrayList<>(); - for (ClimbLocations location : climbLocations) { - this.climbLocationsList.add(location.value); - } - } - - private void confiureSimulation() { - DriverStation.silenceJoystickConnectionWarning(true); - } - - public void enablePDPSwitch() { - pdp.setSwitchableChannel(true); - } - - private void configureDashboard() { - SmartDashboard.putData("LIFT CLIMB", new ClimberCommand(m_climber, false)); - SmartDashboard.putData("LOWER CLIMB", new PitClimbSetupCommand(m_climber)); - SmartDashboard.putData("Lazy Zero Elevator", Commands.runOnce(m_elevator::zeroElevator).ignoringDisable(true)); - SmartDashboard.putData("Set Center", new InstantCommand(() -> { - m_drivetrain.setPose(FieldUtils.flipPose(new Pose2d(7.6, 4.025, Rotation2d.k180deg))); - }).ignoringDisable(true)); - } - - private void configureTesting() { - SmartDashboard.putData("Reset Pose", m_drivetrain.runOnce(() -> {m_drivetrain.setPose(new Pose2d(0, 0, Rotation2d.kCCW_90deg));})); - SmartDashboard.putData("Net", netCommand()); - SmartDashboard.putData("Net Wait", netCommand().andThen(Commands.waitUntil(m_elevator::safe))); - } - - // ********** BINDINGS ********** - - private void configureDriverBindings() { - CommandPS5Controller controller = new CommandPS5Controller(ButtonBindingConstants.driverPort); - // controller.setRumble(RumbleType.kRightRumble, 1.0); - - int xAxis; - int yAxis; - int rAxis; // rotation - int resetHeading; - - double flipX; - double flipY; - double flipR; - - xAxis = DragonReins.xAxis; - yAxis = DragonReins.yAxis; - rAxis = DragonReins.rotAxis; - - resetHeading = DragonReins.resetHeading; - - flipX = DragonReins.flipX ? -1.0 : 1.0; - flipY = DragonReins.flipY ? -1.0 : 1.0; - flipR = DragonReins.flipRot ? -1.0 : 1.0; - - Supplier xSup = () -> controller.getRawAxis(xAxis) * flipX; - Supplier ySup = () -> controller.getRawAxis(yAxis) * flipY; - Supplier rSup = () -> controller.getRawAxis(rAxis) * flipR; - - m_drivetrain.setDefaultCommand( - new TeleopCommand(m_drivetrain, xSup, ySup, rSup) - ); - - controller.button(resetHeading).onTrue(m_drivetrain.runOnce(() -> m_drivetrain.resetHeading())); - controller.button(resetHeading).onFalse(m_drivetrain.runOnce(() -> m_drivetrain.resetHeading())); - - bindAutoProcessCommand(controller.button(DragonReins.processor)); - - controller.axisMagnitudeGreaterThan(xAxis, DriveConstants.k_closedLoopOverrideToleranceTranslation) - .or(controller.axisMagnitudeGreaterThan(yAxis, DriveConstants.k_closedLoopOverrideToleranceTranslation)) - .or(controller.axisMagnitudeGreaterThan(rAxis, DriveConstants.k_closedLoopOverrideToleranceRotation)) - .onTrue(new InstantCommand(() -> AutonomousUtil.clearQueue())); - } - - private void configureOperatorBindings() { - CommandPS5Controller controller = new CommandPS5Controller(ButtonBindingConstants.buttonBoardPort); - - Trigger algaeOn = controller.button(PS5.algaeModeButton); - - controller.button(PS5.ejectCoral).whileTrue(new CoralEjectCommand(m_coralRollers, m_elevator)); - - bindAlignCommand(ReefClipLocations.LEFT, controller.button(PS5.leftReef)); - bindAlignCommand(ReefClipLocations.RIGHT, controller.button(PS5.rightReef)); - - bindL1Command(false, controller.pov(PS5.L1).and(algaeOn.negate()).and(controller.button(PS5.secondaryL1).negate())); - bindL1Command(true, controller.pov(PS5.L1).and(controller.button(PS5.secondaryL1)).and(algaeOn.negate())); - bindCoralCommands(2, controller.pov(PS5.L2).and(algaeOn.negate())); - bindCoralCommands(3, controller.pov(PS5.L3).and(algaeOn.negate())); - bindCoralCommands(4, controller.pov(PS5.L4).and(algaeOn.negate())); - - bindCoralIntakeCommand(controller.button(PS5.intake)); - - bindAlgaeIntakeCommand(AlgaeLocationPresets.REEFLOWER, controller.button(PS5.lowAlgae)); - bindAlgaeIntakeCommand(AlgaeLocationPresets.REEFUPPER, controller.button(PS5.highAlgae)); - bindAlgaeIntakeCommand(AlgaeLocationPresets.GROUND, controller.pov(PS5.groundAlgae).and(algaeOn)); - bindAlgaeIntakeCommand(AlgaeLocationPresets.HIGHGROUND, controller.pov(PS5.highGround).and(algaeOn)); - - bindAlgaeScoreCommand(AlgaeLocationPresets.PROCESSOR, controller.pov(PS5.processor).and(algaeOn)); - bindAlgaeScoreCommand(AlgaeLocationPresets.NET, controller.pov(PS5.net).and(algaeOn)); - - controller.button(PS5.climbReady).whileTrue(new ClimbReadyCommand(m_climber)); - controller.button(PS5.climb).whileTrue(new ClimberCommand(m_climber)); - - controller.button(PS5.stow).onTrue(new StowCommand(m_elevator, m_algaePivot)); - - bindFunnelOpenCommand(controller.button(11).and(controller.button(12))); - - bindElevatorZeroCommand(controller.button(PS5.zeroElevator)); - } - - private void bindElevatorZeroCommand(Trigger trigger) { - trigger.whileTrue(zero()); - } - - private void bindL1Command(boolean higher, Trigger trigger) { - Runnable elevatorCommand = (higher) ? m_elevator::setSecondaryL1 : m_elevator::setL1; - trigger.whileTrue( - m_elevator.run(elevatorCommand) - .onlyIf(m_coralRollers::holdingPiece) - ); - trigger.onFalse( - m_coralRollers.run(m_coralRollers::setL1Eject) - .onlyWhile(m_coralRollers::getFrontCANrange) - .onlyIf(m_elevator::atSetpoint) - .onlyIf(m_coralRollers::holdingPiece) - .finallyDo(m_elevator::release) - .finallyDo(m_coralRollers::stop) - ); - } - - private void bindCoralCommands(int level, Trigger trigger) { - trigger.whileTrue(coralPrepAndScoreCommand(level)); - trigger.onFalse(coralScoreCommand(level).andThen(zero())); - } - - private void bindAutoProcessCommand(Trigger trigger) { - trigger.whileTrue(new DriveToPointCommand(FieldConstants.k_processor, m_drivetrain, true)); - } - - private Command zero() { - // return new InstantCommand(); - return new ElevatorZero(m_elevator).withTimeout(2).asProxy() - .unless(RobotObserver::getNoElevatorZone); - } - - // ********** AUTONOMOUS ********** - - private SendableChooser autoChooser = new SendableChooser<>(); + private final PowerDistribution m_pdp = new PowerDistribution(1, ModuleType.kRev); - private void configureAutonChooser() { - autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); - autoChooser.getSelected(); - } + private SendableChooser m_autoChooser; - public Command getAutonomousCommand() { - Command autonCommand = autoChooser.getSelected(); - m_logger.info("Requirements: {}", autonCommand.getRequirements()); - return autonCommand; - } + private final Superstructure m_superstructure = new Superstructure( + new Algae(), + new Coral(), + new Pivot(), + new Elevator(), + new Climber(), + TunerConstants.createDrivetrain(), + new LedFeedback()); - private void configureNamedCommands() { - NamedCommands.registerCommand("L4", coralScoreCommand(4)); - NamedCommands.registerCommand("L3", coralScoreCommand(3)); - NamedCommands.registerCommand("Intake", zero() - .andThen(coralIntakeCommand() - .withTimeout(5))); - NamedCommands.registerCommand("Intake Wait", new WaitUntilCommand(m_coralRollers::intakeReady) - .alongWith(m_drivetrain.runOnce(m_drivetrain::stop))); - NamedCommands.registerCommand("Interrupt", new WaitUntilCommand(() -> !DriverStation.isAutonomousEnabled())); - for (ScoringLocations location : Constants.ScoringLocations.values()) { - String name = "Align ".concat(location.toString()); - NamedCommands.registerCommand(name, new DriveToPointCommand(location.value, m_drivetrain, true) - .withTimeout(5.0)); - } - NamedCommands.registerCommand("Align IJ", new DriveToPointCommand(Constants.FieldConstants.kIJ, m_drivetrain, true)); - NamedCommands.registerCommand("Align GH", new DriveToPointCommand(Constants.FieldConstants.kGH, m_drivetrain, true)); - NamedCommands.registerCommand("Align Barge", new DriveToPointCommand(FieldConstants.kBarge1, m_drivetrain, true)); - NamedCommands.registerCommand("LIntake Align", new DriveToPointCommand(FieldConstants.kLeftIntake, m_drivetrain, true) - .until(m_coralRollers::intakeReady)); - NamedCommands.registerCommand("RIntake Align", new DriveToPointCommand(FieldConstants.kRightIntake, m_drivetrain, true) - .until(m_coralRollers::intakeReady)); - NamedCommands.registerCommand("AlgaeUpper", zero().andThen(algaeIntakeCommand(AlgaeLocationPresets.REEFUPPER)) - .until(m_algaeRollers::algaeHeld)); - NamedCommands.registerCommand("AlgaeLower", zero().andThen(algaeIntakeCommand(AlgaeLocationPresets.REEFLOWER)) - .until(m_algaeRollers::algaeHeld)); - NamedCommands.registerCommand("Stop", m_drivetrain.runOnce(m_drivetrain::stop)); - NamedCommands.registerCommand("Net", netCommand()); - NamedCommands.registerCommand("Process", new DriveToPointCommand(FieldConstants.k_processor, m_drivetrain, true) - .alongWith(processorSetupCommand())); - NamedCommands.registerCommand("Algae End", m_algaePivot.run(m_algaePivot::setGroundPickup) - .alongWith(m_elevator.run(m_elevator::setGroundIntake))); - } + private final Binder m_driver = new DriveBindings(); + private final Binder m_operator = new OperatorBindings(); + private final Binder m_robot = new RobotBindings(); + private final Binder m_namedCommands = new NamedCommandBindings(); + private final Binder m_dashboard = new DashboardBindings(); - private void configureVision() { - if (VisionConstants.enableVision) { - m_vision.startThread(); - } else { - m_logger.warn("Disabled vision temporarily"); - } - } + private final Runnable m_algaeTracker; - // ********** SUBSYSTEMS ********** + public RobotContainer() { + m_driver.bind(m_superstructure); + m_operator.bind(m_superstructure); + m_robot.bind(m_superstructure); + m_namedCommands.bind(m_superstructure); + m_dashboard.bind(m_superstructure); - private Elevator m_elevator; - private Pivot m_algaePivot; - private Climber m_climber; - private AlgaeRollers m_algaeRollers; - private CoralRollers m_coralRollers; - @SuppressWarnings("unused") - private LedFeedback m_ledFeedback; + m_superstructure.buildVision().startThread(); + m_algaeTracker = m_superstructure.buildAlgaeTracker(); - private void configureSubsystems() { - // m_drivetrain.registerTelemetry(m_telemetry::telemeterize); - m_elevator = new Elevator(); - m_algaePivot = new Pivot(); - m_climber = new Climber(); - m_algaeRollers = new AlgaeRollers(); - m_coralRollers = new CoralRollers(); - m_ledFeedback = new LedFeedback(); - m_elevator.setDefaultCommand(new ElevatorDefaultCommand(m_elevator)); + if (Robot.isSimulation()) { + DriverStation.silenceJoystickConnectionWarning(true); } - // ** BUTTON BOARD HELPERS ** - private void bindCoralIntakeCommand(Trigger trigger) { - trigger.whileTrue(coralIntakeCommand()); - trigger.onFalse(coralIntakeCommand().onlyWhile(m_coralRollers::presentPiece)); - } + m_autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auton Chooser", m_autoChooser); + } - private void bindAlignCommand(ReefClipLocations location, Trigger trigger) { - switch (location) { - case LEFT -> trigger.whileTrue(new DeferredCommand(() -> (AutonomousUtil.closestPathThenRunCommand(() -> new InstantCommand(), scoringLocationsListLeft).beforeStarting(new InstantCommand(() -> RobotObserver.setReefClipLocation(ReefClipLocations.LEFT)))), Set.of())); - case RIGHT -> trigger.whileTrue(new DeferredCommand(() -> (AutonomousUtil.closestPathThenRunCommand(() -> new InstantCommand(), scoringLocationsRightList).beforeStarting(new InstantCommand(() -> RobotObserver.setReefClipLocation(ReefClipLocations.RIGHT)))), Set.of())); - } - } + public Command getAutonomousCommand() { + return m_autoChooser.getSelected(); + } - private void bindAlgaeIntakeCommand(AlgaeLocationPresets location, Trigger trigger) { - trigger.whileTrue(algaeIntakeCommand(location).andThen(zero())); - } - - private void bindAlgaeScoreCommand(AlgaeLocationPresets type, Trigger trigger) { - switch (type) { - case NET -> { - trigger.whileTrue(m_elevator.run(m_elevator::setNet).onlyIf(m_algaeRollers::algaeHeld)); - trigger.onFalse( - netCommand().onlyIf(m_elevator::atSetpoint) - .andThen( - zero().unless(RobotObserver::getNoElevatorZone) - ).finallyDo(m_elevator::release) - ); - } - case PROCESSOR -> { - trigger.whileTrue(processorSetupCommand()); - trigger.onFalse(processorCommand().andThen(zero())); - } - default -> {} - } - } - - private void bindFunnelOpenCommand(Trigger trigger) { - trigger.whileTrue(new OpenFunnel(m_climber)); - } - - private Command algaeEjectCommand() { - return m_algaeRollers.startEnd( - m_algaeRollers::ejectAlgae, - m_algaeRollers::stopMotor - ).onlyWhile(m_algaeRollers::algaeHeld); - } + public void enablePDPSwitch() { + m_pdp.setSwitchableChannel(true); + } - // ** SUBSYSTEM PASS IN HELPERS ** - - private Command coralIntakeCommand() { - // return new CoralIntakeCommand(m_coralRollers, m_elevator); - return Commands.sequence( - m_elevator.runOnce(m_elevator::setStow).asProxy(), - m_coralRollers.runOnce(m_coralRollers::setIntake), - Commands.waitUntil(m_coralRollers::holdingPiece) - ) - .finallyDo(m_elevator::release) - .finallyDo(m_coralRollers::stop) - .unless(m_coralRollers::holdingPiece); - } - - private Command coralPrepAndScoreCommand(int level) { - return new ElevatorToPointCommand(level, m_elevator) - .andThen(new WaitUntilCommand(m_drivetrain::isAligned)) - .andThen(coralScoreCommand(level)) - .onlyIf(m_coralRollers::holdingPiece); - } - - private Command elevatorPrepCommand(int level) { - return new ElevatorToPointCommand(level, m_elevator) - .onlyIf(m_coralRollers::holdingPiece); - } - - private Command coralScoreCommand(int level) { - // return new CoralScoreCommand(m_coralRollers, m_elevator, level) - // .andThen(new WaitUntilCommand(m_elevator::atSetpoint) - // .onlyIf(m_coralRollers::holdingPiece) - // ); - Runnable elevatorCommand = () -> { - m_logger.trace("Running elevator command"); - m_elevator.setLevel(level); - }; - Runnable coralCommand = () -> { - m_logger.trace("Doing coral command"); - switch (level) { - case 1 -> m_coralRollers.setL1Eject(); - case 2 -> m_coralRollers.setL2Eject(); - case 3 -> m_coralRollers.setL3Eject(); - case 4 -> m_coralRollers.setL4Eject(); - } - }; - return Commands.sequence( - m_elevator.runOnce(elevatorCommand).asProxy(), - Commands.runOnce(() -> {m_logger.trace("Finished elevator command");}), - Commands.waitUntil(m_elevator::atSetpoint), - m_coralRollers.run(coralCommand) - .onlyWhile(m_coralRollers::holdingPiece) - ) - .finallyDo(m_elevator::release) - .finallyDo(m_coralRollers::stop) - .onlyIf(m_coralRollers::holdingPiece); - } - - private Command algaeIntakeCommand(AlgaeLocationPresets location) { - if (location == AlgaeLocationPresets.GROUND || location == AlgaeLocationPresets.HIGHGROUND) { - Runnable elevatorCommand = () -> { - if (location == AlgaeLocationPresets.GROUND) { - m_elevator.setGroundIntake(); - } else { - m_elevator.setHighGroundIntake(); - } - }; - return Commands.sequence( - m_algaeRollers.runOnce(m_algaeRollers::intakeAlgae), - m_elevator.runOnce(elevatorCommand).asProxy(), - Commands.waitUntil(m_elevator::atSetpoint), - m_algaePivot.runOnce(m_algaePivot::setGroundPickup), - Commands.waitUntil(m_algaeRollers::algaeHeld) - ) - .until(m_algaeRollers::algaeHeld) - .finallyDo(m_elevator::release) - .finallyDo(m_algaePivot::setStow) - .finallyDo(m_algaeRollers::smartStop); - } - /* reef intake */ - Runnable elevatorCommand = () -> { - if (location == AlgaeLocationPresets.REEFLOWER) { - m_elevator.setReefLower(); - } else { - m_elevator.setReefUpper(); - } - }; - return Commands.sequence( - m_algaeRollers.runOnce(m_algaeRollers::intakeAlgae), - m_elevator.runOnce(elevatorCommand).asProxy(), - Commands.waitUntil(m_elevator::atSetpoint), - m_algaePivot.runOnce(m_algaePivot::setReefPickup), - Commands.waitUntil(m_algaeRollers::algaeHeld), - m_algaePivot.runOnce(m_algaePivot::setReefExtract) - ) - .finallyDo(m_elevator::release) - .finallyDo(m_algaePivot::setStow) - .finallyDo(m_algaeRollers::smartStop); - } - - private Command netCommand() { - return Commands.sequence( - m_elevator.runOnce(m_elevator::setNet).asProxy(), - m_algaePivot.runOnce(m_algaePivot::setNet), - Commands.waitUntil(m_elevator::atSetpoint), - Commands.waitUntil(m_algaePivot::atSetpoint), - m_algaeRollers.runOnce(m_algaeRollers::ejectAlgae), - Commands.waitSeconds(AlgaeRollerConstants.algaeEjectTime) - ) - .onlyIf(m_algaeRollers::algaeHeld) - .finallyDo(m_algaeRollers::smartStop) - .finallyDo(m_algaePivot::setStow) - .finallyDo(m_elevator::release); - } - - private Command processorCommand() { - return Commands.sequence( - processorSetupCommand(), - Commands.waitUntil(m_elevator::atSetpoint), - m_algaeRollers.runOnce(m_algaeRollers::processorEjectAlgae), - Commands.waitSeconds(2) - ) - .finallyDo(m_algaeRollers::smartStop) - .finallyDo(m_algaePivot::setStow) - .finallyDo(m_elevator::release); - } - - private Command processorSetupCommand() { - return Commands.sequence( - m_algaePivot.runOnce(m_algaePivot::setProcessor), - m_elevator.runOnce(m_elevator::setProcessor).asProxy() - ) - .onlyIf(m_algaeRollers::algaeHeld); - } - - - public enum AlgaeLocationPresets { - REEFLOWER, REEFUPPER, PROCESSOR, GROUND, NET, HIGHGROUND; - } - - public void resetReferences() { - // m_elevator.setPosition(m_elevator.getPosition()); - m_algaePivot.setStow(); - } - - public boolean getFFEnabled() { - return m_elevator.elevatorUp() && !DriverStation.isAutonomous(); - } - - public void setupAutoclimb() { - SmartDashboard.putBoolean("Match mode?", false); - Command autoclimb = Commands.sequence(Commands.waitUntil(() -> { - return (DriverStation.getMatchType() != MatchType.None || SmartDashboard.getBoolean("Match mode?", false)) - && (DriverStation.getMatchTime() < ClimberConstants.kClimbTime) - && (DriverStation.isTeleopEnabled()); - }), - new ClimbReadyCommand(m_climber).asProxy()); - autoclimb.schedule(); - } + public void updateAlgaeTracking() { + m_algaeTracker.run(); + } } diff --git a/ThriftyTest/src/main/java/frc/robot/RobotObserver.java b/ThriftyTest/src/main/java/frc/robot/RobotObserver.java index 7d6db14c..b68fc2a4 100644 --- a/ThriftyTest/src/main/java/frc/robot/RobotObserver.java +++ b/ThriftyTest/src/main/java/frc/robot/RobotObserver.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants.ReefClipLocations; public class RobotObserver { private static RobotObserver m_instance; @@ -19,17 +18,6 @@ private static RobotObserver getInstance() { return m_instance; } - /* Pose2d to watch the pose of the robot and associated methods */ - private Supplier m_poseSupplier; - - public static void setPoseSupplier(Supplier poseSupplier) { - getInstance().m_poseSupplier = poseSupplier; - } - - public static Pose2d getPose() { - return getInstance().m_poseSupplier.get(); - } - /* the velocity of the robot */ private DoubleSupplier m_veloSupplier; @@ -54,51 +42,10 @@ public static Field2d getField() { return getInstance().m_field; } - /* Keeps track of the latest time an april tag was seen */ - private Supplier m_visionValidSupplier; - - public static void setVisionValidSupplier(Supplier visionValidSupplier) { - getInstance().m_visionValidSupplier = visionValidSupplier; - } - - public static boolean getVisionValid() { - return getInstance().m_visionValidSupplier.get(); - } - - private Supplier m_elevatorHeightSupplier; - - public static void setElevatorHeightSupplier(Supplier visionValidSupplier) { - getInstance().m_elevatorHeightSupplier = visionValidSupplier; - } - - public static double getElevatorHeightSupplier() { - return getInstance().m_elevatorHeightSupplier.get(); - } - - private boolean m_reefMode = false; - - public static void setReefMode(boolean enabled) { - getInstance().m_reefMode = enabled; - } - - public static boolean getReefMode() { - return getInstance().m_reefMode; - } - - private ReefClipLocations m_reefClipLocation = ReefClipLocations.LEFT; - - public static void setReefClipLocation(ReefClipLocations reefClipLocation) { - getInstance().m_reefClipLocation = reefClipLocation; - } - - public static ReefClipLocations getReefClipLocation() { - return getInstance().m_reefClipLocation; - } - private BooleanSupplier m_coralPieceHeldSupplier; private BooleanSupplier m_algaeHeldSupplier; - public static void setPieceHeldSupplier(BooleanSupplier pieceHeldSupplier) { + public static void setCoralHeldSupplier(BooleanSupplier pieceHeldSupplier) { getInstance().m_coralPieceHeldSupplier = pieceHeldSupplier; } @@ -172,4 +119,5 @@ public static void setAlginedSupplier(BooleanSupplier alignedSupplier) { public static boolean getAligned() { return getInstance().m_alignedSupplier.getAsBoolean(); } + } diff --git a/ThriftyTest/src/main/java/frc/robot/Telemetry.java b/ThriftyTest/src/main/java/frc/robot/Telemetry.java index 584792e0..6d870be1 100644 --- a/ThriftyTest/src/main/java/frc/robot/Telemetry.java +++ b/ThriftyTest/src/main/java/frc/robot/Telemetry.java @@ -13,6 +13,7 @@ import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; @@ -106,7 +107,7 @@ public void telemeterize(SwerveDriveState state) { m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - // SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + SmartDashboard.putData("Drivetrain/Module " + i, m_moduleMechanisms[i]); } } } diff --git a/ThriftyTest/src/main/java/frc/robot/binding/Binder.java b/ThriftyTest/src/main/java/frc/robot/binding/Binder.java new file mode 100644 index 00000000..88e3c27d --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/binding/Binder.java @@ -0,0 +1,13 @@ +package frc.robot.binding; + +import frc.robot.superstructure.Superstructure; + +/** + * To bind robot actions to event, group trigger bindings to a grouping inside of a Binder + */ +public interface Binder { + /** + * Binds all associated bindings for this binder + */ + void bind(Superstructure superstructure); +} diff --git a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java new file mode 100644 index 00000000..fbf8a519 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java @@ -0,0 +1,60 @@ +package frc.robot.binding; + +import edu.wpi.first.wpilibj.PS5Controller.Button; + +public class BindingConstants { + public static final int kDriverPort = 0; + public static final int kOperatorPort = 1; + + public static class Driver { + public static final int xAxis = 1; + public static final int yAxis = 0; + public static final int rotAxis = 3; + + public static final boolean kFlipX = false; + public static final boolean kFlipY = true; + public static final boolean kFlipRot = false; + + public static final int kResetHeading = 1; + public static final int kSmartAlign = 2; + public static final int kRightAlign = 3; + public static final int kLeftAlign = 4; + + public static final double deadband = 0.01; + } + + public static class Operator { + public static final int kL1 = 180; + public static final int kL2 = 270; + public static final int kL3 = 90; + public static final int kL4 = 0; + + public static final int kSecondaryL1 = 14; + + public static final int kEjectCoral = Button.kL2.value; + + public static final int kLeftAlign = Button.kSquare.value; + public static final int kRightAlign = Button.kCircle.value; + + public static final int kLowerAlgae = Button.kCross.value; + public static final int kUpperAlgae = Button.kTriangle.value; + public static final int kGroundAlgaeIntake = 180; + public static final int kProcessor = 90; + public static final int kHighGroundAlgaeIntake = 270; + public static final int kNet = 0; + public static final int kAlgae = Button.kR2.value; + + public static final int kRaiseClimb = Button.kCreate.value; + public static final int kClimb = Button.kOptions.value; + + public static final int kStow = Button.kPS.value; + + public static final int kCoralIntake = Button.kL1.value; + + public static final int kCalibrateElevator = 15; + + public static final int kRightFunnel = 11; + public static final int kLeftFunnel = 12; + } +} + diff --git a/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java new file mode 100644 index 00000000..3dd81ef3 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java @@ -0,0 +1,28 @@ +package frc.robot.binding; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure; +import frc.robot.superstructure.Superstructure.Subsystems; +import frc.robot.superstructure.states.SeedPose; +import frc.robot.superstructure.states.Stow; +import frc.robot.superstructure.states.TrackAlgae; + +public class DashboardBindings implements Binder { + public void bind(Superstructure superstructure) { + SmartDashboard.putData("Prep/Set Center", superstructure.enter(SeedPose.center())); + SmartDashboard.putData("Prep/Set Left", superstructure.enter(SeedPose.left())); + SmartDashboard.putData("Prep/Set Right", superstructure.enter(SeedPose.right())); + + SmartDashboard.putData("Test/Stow", superstructure.enter(new Stow())); + SmartDashboard.putData("Test/Elevator Up", superstructure.enter(new EnterableState() { + public Command build(Subsystems subsystems) { + return subsystems.elevator().go(ElevatorState.L4); + } + })); + + SmartDashboard.putData("Test/Follow Algae", superstructure.enter(new TrackAlgae())); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java new file mode 100644 index 00000000..7b49fd12 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java @@ -0,0 +1,45 @@ +package frc.robot.binding; + +import java.util.function.DoubleSupplier; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.FieldConstants; +import frc.robot.binding.BindingConstants.Driver; +import com.therekrab.autopilot.APTarget; +import frc.robot.superstructure.Superstructure; +import frc.robot.superstructure.states.Align; +import frc.robot.superstructure.states.DeferredAlign; +import frc.robot.superstructure.states.DeferredAlign.AlignLocation; +import frc.robot.superstructure.states.HeadingReset; +import frc.robot.superstructure.states.TeleopDrive; + +public class DriveBindings implements Binder { + private final CommandPS5Controller m_controller = + new CommandPS5Controller(BindingConstants.kDriverPort); + + private final DoubleSupplier m_x = + () -> m_controller.getRawAxis(Driver.xAxis) * (Driver.kFlipX ? -1.0 : 1.0);; + private final DoubleSupplier m_y = + () -> m_controller.getRawAxis(Driver.yAxis) * (Driver.kFlipY ? -1.0 : 1.0); + + private final DoubleSupplier m_rot = + () -> m_controller.getRawAxis(Driver.rotAxis) * (Driver.kFlipRot ? -1.0 : 1.0); + + private final Trigger m_resetHeading = m_controller.button(Driver.kResetHeading); + private final Trigger m_smartAlign = m_controller.button(Driver.kSmartAlign); + private final Trigger m_leftAlign = m_controller.button(Driver.kLeftAlign); + private final Trigger m_rightAlign = m_controller.button(Driver.kRightAlign); + + public void bind(Superstructure superstructure) { + superstructure.setDrive(superstructure.enterWithoutProxy(new TeleopDrive(m_x, m_y, m_rot))); + + m_resetHeading.onTrue(superstructure.enter(new HeadingReset())); + m_smartAlign.and(superstructure.holdingAlgae()).whileTrue(superstructure.enter(new Align( + new APTarget(FieldConstants.k_processor).withEntryAngle(Rotation2d.kCW_Pi_2)))); + m_smartAlign.and(superstructure.holdingAlgae().negate()).whileTrue(superstructure.enter( + new DeferredAlign(AlignLocation.Center))); + m_leftAlign.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Left))); + m_rightAlign.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Right))); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java new file mode 100644 index 00000000..ce786668 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -0,0 +1,90 @@ +package frc.robot.binding; + +import static edu.wpi.first.units.Units.Meters; +import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.Constants; +import frc.robot.Constants.CoralLevel; +import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.ScoringLocations; +import com.therekrab.autopilot.APTarget; +import frc.robot.superstructure.Superstructure; +import frc.robot.superstructure.states.AlgaeStow; +import frc.robot.superstructure.states.Align; +import frc.robot.superstructure.states.CoralScore; +import frc.robot.superstructure.states.CoralWait; +import frc.robot.superstructure.states.ElevatorZero; +import frc.robot.superstructure.states.IntakeComplete; +import frc.robot.superstructure.states.LowerReefAlgaeIntake; +import frc.robot.superstructure.states.Net; +import frc.robot.superstructure.states.UpperReefAlgaeIntake; + +public class NamedCommandBindings implements Binder { + /** + * Configures PathPlanner's Named Commands + */ + public NamedCommandBindings() {} + + public void bind(Superstructure superstructure) { + /* coral */ + NamedCommands.registerCommand("L4", superstructure.enter(new CoralScore(CoralLevel.L4))); + NamedCommands.registerCommand("L3", superstructure.enter(new CoralScore(CoralLevel.L3))); + NamedCommands.registerCommand("Coral Wait", superstructure.enter(new CoralWait())); + NamedCommands.registerCommand("Intake", superstructure.enter(new IntakeComplete())); + NamedCommands.registerCommand("Recalibrate", superstructure.enter(new ElevatorZero())); + + /* algae */ + NamedCommands.registerCommand("Lower Algae", superstructure.enter(new LowerReefAlgaeIntake())); + NamedCommands.registerCommand("Upper Algae", superstructure.enter(new UpperReefAlgaeIntake())); + NamedCommands.registerCommand("Net", superstructure.enter(new Net())); + NamedCommands.registerCommand("Algae Stow", superstructure.enter(new AlgaeStow())); + + /* align */ + for (ScoringLocations location : Constants.ScoringLocations.values()) { + String name = "Align ".concat(location.toString()); + NamedCommands.registerCommand(name, superstructure.enter( + new Align(new APTarget(location.value).withEntryAngle(location.value.getRotation())) + .allianceRelative())); + } + APTarget lIntake = new APTarget(FieldConstants.kLeftIntake) + .withRotationRadius(Meters.of(2.0)); + APTarget rIntake = new APTarget(FieldConstants.kRightIntake) + .withRotationRadius(Meters.of(2.0)); + NamedCommands.registerCommand("Align LIntake", superstructure.enter( + new Align(lIntake.withEntryAngle(Rotation2d.kPi)) + .allianceRelative() + .fast())); + NamedCommands.registerCommand("Align RIntake", superstructure.enter( + new Align(rIntake.withEntryAngle(Rotation2d.kPi)) + .allianceRelative() + .fast())); + NamedCommands.registerCommand("Beeline LIntake", superstructure.enter( + new Align(lIntake) + .allianceRelative() + .fast())); + NamedCommands.registerCommand("Beeline RIntake", superstructure.enter( + new Align(rIntake) + .allianceRelative() + .fast())); + NamedCommands.registerCommand("Align IJ", superstructure.enter( + new Align(new APTarget(FieldConstants.kIJ).withEntryAngle(FieldConstants.kIJ.getRotation())) + .allianceRelative())); + NamedCommands.registerCommand("Align GH", superstructure.enter( + new Align(new APTarget(FieldConstants.kGH).withEntryAngle(FieldConstants.kGH.getRotation())) + .allianceRelative())); + APTarget bargeFromCenter = new APTarget(FieldConstants.kBargeFromCenter) + .withEntryAngle(Rotation2d.fromDegrees(55.0)) + .withRotationRadius(Meters.of(1.5)); + APTarget bargeFromLeft = new APTarget(FieldConstants.kBargeFromLeft) + .withEntryAngle(Rotation2d.fromDegrees(35.0)) + .withRotationRadius(Meters.of(1.5)); + NamedCommands.registerCommand("Align Barge Center", superstructure.enter( + new Align(bargeFromCenter).allianceRelative().slow())); + NamedCommands.registerCommand("Align Barge Left", superstructure.enter( + new Align(bargeFromLeft).allianceRelative().slow())); + NamedCommands.registerCommand("Beeline Barge Center", superstructure.enter( + new Align(bargeFromCenter.withoutEntryAngle()).allianceRelative().slow())); + NamedCommands.registerCommand("Beeline Barge Left", superstructure.enter( + new Align(bargeFromLeft.withoutEntryAngle()).allianceRelative().slow())); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java new file mode 100644 index 00000000..3632871e --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java @@ -0,0 +1,111 @@ +package frc.robot.binding; + +import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.CoralLevel; +import frc.robot.binding.BindingConstants.Operator; +import frc.robot.superstructure.Superstructure; +import frc.robot.superstructure.states.Climb; +import frc.robot.superstructure.states.RaiseClimb; +import frc.robot.superstructure.states.CompleteCoralIntake; +import frc.robot.superstructure.states.CompleteCoralScore; +import frc.robot.superstructure.states.CoralEject; +import frc.robot.superstructure.states.CoralIntake; +import frc.robot.superstructure.states.CoralScore; +import frc.robot.superstructure.states.CoralScorePrep; +import frc.robot.superstructure.states.ElevatorZero; +import frc.robot.superstructure.states.OpenFunnel; +import frc.robot.superstructure.states.GroundAlgaeIntake; +import frc.robot.superstructure.states.HighGroundAlgaeIntake; +import frc.robot.superstructure.states.UpperReefAlgaeIntake; +import frc.robot.superstructure.states.DeferredAlign.AlignLocation; +import frc.robot.superstructure.states.LowerReefAlgaeIntake; +import frc.robot.superstructure.states.Net; +import frc.robot.superstructure.states.NetPrep; +import frc.robot.superstructure.states.Processor; +import frc.robot.superstructure.states.ProcessorPrep; +import frc.robot.superstructure.states.DeferredAlign; +import frc.robot.superstructure.states.Stow; + +public class OperatorBindings implements Binder { + private final CommandPS5Controller m_controller = + new CommandPS5Controller(BindingConstants.kOperatorPort); + + private final Trigger m_l1 = m_controller.pov(Operator.kL1); + private final Trigger m_secondaryL1 = m_controller.pov(Operator.kSecondaryL1); + private final Trigger m_l2 = m_controller.pov(Operator.kL2); + private final Trigger m_l3 = m_controller.pov(Operator.kL3); + private final Trigger m_l4 = m_controller.pov(Operator.kL4); + + private final Trigger m_left = m_controller.button(Operator.kLeftAlign); + private final Trigger m_right = m_controller.button(Operator.kRightAlign); + + private final Trigger m_coralIntake = m_controller.button(Operator.kCoralIntake); + private final Trigger m_ejectCoral = m_controller.button(Operator.kEjectCoral); + + private final Trigger m_algae = m_controller.button(Operator.kAlgae); + + private final Trigger m_algaeGround = m_controller.pov(Operator.kGroundAlgaeIntake); + private final Trigger m_algaeHighGround = m_controller.pov(Operator.kHighGroundAlgaeIntake); + private final Trigger m_algaeLowReef = m_controller.button(Operator.kLowerAlgae); + private final Trigger m_algaeHighReef = m_controller.button(Operator.kUpperAlgae); + + private final Trigger m_processor = m_controller.pov(Operator.kProcessor); + private final Trigger m_net = m_controller.pov(Operator.kNet); + + private final Trigger m_raiseClimb = m_controller.button(Operator.kRaiseClimb); + private final Trigger m_climb = m_controller.button(Operator.kClimb); + + private final Trigger m_stow = m_controller.button(Operator.kStow); + + private final Trigger m_funnelLeft = m_controller.button(Operator.kLeftFunnel); + private final Trigger m_funnelRight = m_controller.button(Operator.kRightFunnel); + private final Trigger m_funnel = m_funnelLeft.and(m_funnelRight); + + private final Trigger m_zeroElevator = m_controller.button(Operator.kCalibrateElevator); + + public void bind(Superstructure superstructure) { + /* algae intake */ + m_algae.and(m_algaeGround).whileTrue(superstructure.enter(new GroundAlgaeIntake())); + m_algae.and(m_algaeHighGround).whileTrue(superstructure.enter(new HighGroundAlgaeIntake())); + m_algaeLowReef.whileTrue(superstructure.enter(new LowerReefAlgaeIntake())); + m_algaeHighReef.whileTrue(superstructure.enter(new UpperReefAlgaeIntake())); + + /* algae score */ + m_algae.and(m_processor).whileTrue(superstructure.enter(new ProcessorPrep())); + m_algae.and(m_processor).onFalse(superstructure.enter(new Processor())); + m_algae.and(m_net).whileTrue(superstructure.enter(new NetPrep())); + m_algae.and(m_net).onFalse(superstructure.enter(new Net())); + + /* coral intake & score */ + m_coralIntake.whileTrue(superstructure.enter(new CoralIntake())); + m_coralIntake.onFalse(superstructure.enter(new CompleteCoralIntake())); + m_ejectCoral.whileTrue(superstructure.enter(new CoralEject())); + bindCoral(m_l1.and(m_secondaryL1.negate()), CoralLevel.L1, superstructure); + bindCoral(m_l1.and(m_secondaryL1), CoralLevel.SecondaryL1, superstructure); + bindCoral(m_l2, CoralLevel.L2, superstructure); + bindCoral(m_l3, CoralLevel.L3, superstructure); + bindCoral(m_l4, CoralLevel.L4, superstructure); + + /* align */ + m_left.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Left))); + m_right.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Right))); + + /* climb */ + m_climb.whileTrue(superstructure.enter(new Climb())); + m_raiseClimb.whileTrue(superstructure.enter(new RaiseClimb())); + + /* misc */ + m_zeroElevator.whileTrue(superstructure.enter(new ElevatorZero())); + m_stow.whileTrue(superstructure.enter(new Stow())); + m_funnel.whileTrue(superstructure.enter(new OpenFunnel())); + } + + private void bindCoral(Trigger trigger, CoralLevel level, Superstructure superstructure) { + trigger.and(m_algae.negate()).whileTrue(superstructure.enter(new CoralScorePrep(level))); + trigger.and(m_algae.negate()).and(superstructure.aligned()) + .onTrue(superstructure.enter(new CoralScore(level))); + trigger.and(m_algae.negate()).onFalse(superstructure.enter(new CompleteCoralScore(level))); + } +} + diff --git a/ThriftyTest/src/main/java/frc/robot/binding/RobotBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/RobotBindings.java new file mode 100644 index 00000000..903f7ff3 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/binding/RobotBindings.java @@ -0,0 +1,18 @@ +package frc.robot.binding; + +import frc.robot.superstructure.Superstructure; +import frc.robot.superstructure.modifiers.AlgaeElevatorHold; +import frc.robot.superstructure.modifiers.ElevatorPrep; + +public class RobotBindings implements Binder { + public RobotBindings() {} + + public void bind(Superstructure superstructure) { + /* elevator prefire */ + superstructure.modify(new ElevatorPrep(), + superstructure.inReefZone().and(superstructure.holdingCoral())); + /* no elevator movement when in reef and holding algae */ + superstructure.modify(new AlgaeElevatorHold(), + superstructure.inReefZone().and(superstructure.holdingAlgae())); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/AlgaeEjectCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/AlgaeEjectCommand.java deleted file mode 100644 index 38555c57..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/AlgaeEjectCommand.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.AlgaeRollers; -import frc.robot.subsystems.Elevator; - -public class AlgaeEjectCommand extends Command { - private final AlgaeRollers m_rollers; - private final Elevator m_elevator; - - public AlgaeEjectCommand(AlgaeRollers rollers, Elevator elevator) { - m_elevator = elevator; - m_rollers = rollers; - addRequirements(rollers); - } - - @Override - public void initialize() { - m_rollers.processorEjectAlgae(); - m_elevator.setProcessor(); - } - - @Override - public boolean isFinished() { - return true; - } -} \ No newline at end of file diff --git a/ThriftyTest/src/main/java/frc/robot/commands/AlgaeIntakeCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/AlgaeIntakeCommand.java deleted file mode 100644 index 1f73056f..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/AlgaeIntakeCommand.java +++ /dev/null @@ -1,78 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.RobotContainer.AlgaeLocationPresets; -import frc.robot.subsystems.AlgaeRollers; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Pivot; - -public class AlgaeIntakeCommand extends Command { - private final AlgaeRollers rollers; - private final Elevator elevator; - private final Pivot pivot; - private final AlgaeLocationPresets location; - private boolean isDone; - - public AlgaeIntakeCommand(AlgaeRollers rollers, Elevator elevator, Pivot pivot, AlgaeLocationPresets location) { - this.rollers = rollers; - this.elevator = elevator; - this.pivot = pivot; - this.location = location; - addRequirements(rollers, elevator, pivot); - } - - @Override - public void initialize() { - rollers.intakeAlgae(); - isDone = false; - switch (location) { - case GROUND -> { - elevator.setGroundIntake(); - } - case REEFLOWER -> { - // isDone = !CommandBounds.reefBounds.isActive(); - elevator.setReefLower(); - } - case REEFUPPER -> { - // isDone = !CommandBounds.reefBounds.isActive(); - elevator.setReefUpper(); - } - case HIGHGROUND -> { - elevator.setHighGroundIntake(); - } - default -> isDone = true; - } - } - - @Override - public void execute() { - switch (location) { - case GROUND, HIGHGROUND -> { - if (elevator.atSetpoint()) pivot.setGroundPickup(); - if (rollers.algaeHeld()) isDone = true; - } - case REEFLOWER, REEFUPPER -> { - if (elevator.atSetpoint()) { - if (rollers.algaeHeld()) { - pivot.setReefExtract(); - } else { - pivot.setReefPickup(); - } - } - } - default -> isDone = true; - } - } - - @Override - public void end(boolean interrupted) { - elevator.release(); - pivot.setStow(); - rollers.smartStop(); - } - - @Override - public boolean isFinished() { - return isDone; - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/AlgaeIntakeManualCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/AlgaeIntakeManualCommand.java deleted file mode 100644 index f0ff5c3d..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/AlgaeIntakeManualCommand.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.AlgaeRollers; - -public class AlgaeIntakeManualCommand extends Command { - private final AlgaeRollers algae; - - public AlgaeIntakeManualCommand(AlgaeRollers algae) { - this.algae = algae; - } - - @Override - public void initialize() { - algae.intakeAlgae(); - } - - @Override - public void end(boolean interrupted) { - algae.smartStop(); - } - - @Override - public boolean isFinished() { - return algae.algaeHeld(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/AlgaeScoreCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/AlgaeScoreCommand.java deleted file mode 100644 index 8941ff5e..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/AlgaeScoreCommand.java +++ /dev/null @@ -1,84 +0,0 @@ -package frc.robot.commands; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import com.ctre.phoenix6.Utils; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.AlgaeRollerConstants; -import frc.robot.RobotContainer.AlgaeLocationPresets; -import frc.robot.RobotObserver; -import frc.robot.subsystems.AlgaeRollers; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Pivot; - -public class AlgaeScoreCommand extends Command { - private final Logger m_logger = LoggerFactory.getLogger(AlgaeScoreCommand.class); - - private final AlgaeRollers rollers; - private final Elevator elevator; - private final Pivot pivot; - private final AlgaeLocationPresets location; - private boolean isDone; - private double initialTime; - - public AlgaeScoreCommand(AlgaeRollers rollers, Elevator elevator, Pivot pivot, AlgaeLocationPresets location) { - this.rollers = rollers; - this.elevator = elevator; - this.pivot = pivot; - this.location = location; - addRequirements(rollers, elevator, pivot); - } - - @Override - public void initialize() { - isDone = false; - initialTime = Utils.getCurrentTimeSeconds(); - switch (location) { - case NET -> { - m_logger.trace("Setting NET"); - elevator.setNet(); - pivot.setNet(); - } - case PROCESSOR -> { - elevator.setProcessor(); - pivot.setProcessor(); - } - default -> isDone = true; - } - } - - @Override - public void execute() { - m_logger.trace("Running execute"); - if (elevator.atSetpoint() || RobotObserver.getNoElevatorZone() && pivot.atSetpoint()) { - m_logger.trace("Everything's at setpoint"); - if (location == AlgaeLocationPresets.NET) { - m_logger.trace("Ejecting"); - rollers.ejectAlgae(); - } else { - m_logger.trace("Scoring location is {}", location); - } - } else { - initialTime = Utils.getCurrentTimeSeconds(); - } - } - - @Override - public void end(boolean interrupted) { - m_logger.trace("ENDING"); - if (location == AlgaeLocationPresets.NET) { - m_logger.trace("Stowing and releasing"); - pivot.setStow(); - } - elevator.release(); - rollers.smartStop(); - } - - @Override - public boolean isFinished() { - m_logger.trace("isdone = {}", isDone); - return isDone || (Utils.getCurrentTimeSeconds() - initialTime) >= AlgaeRollerConstants.algaeEjectTime; - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/ClimbReadyCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/ClimbReadyCommand.java deleted file mode 100644 index 7582878a..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/ClimbReadyCommand.java +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Climber; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class ClimbReadyCommand extends Command { - private final Climber m_climber; - - /** Creates a new ClimbReadyCommand. */ - public ClimbReadyCommand(Climber climber) { - addRequirements(climber); - m_climber = climber; - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_climber.setUp(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_climber.stopMotor(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return m_climber.climbReady(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/ClimberCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/ClimberCommand.java deleted file mode 100644 index e1f1b88b..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/ClimberCommand.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.RobotObserver; -import frc.robot.subsystems.Climber; - -public class ClimberCommand extends Command { - private final Climber climber; - private boolean m_climbing; - - public ClimberCommand(Climber climber) { - this(climber, true); - } - - public ClimberCommand(Climber climber, boolean climbing) { - addRequirements(climber); - this.climber = climber; - m_climbing = climbing; - } - - @Override - public void initialize() { - if (m_climbing) { - climber.setDown(); - } else { - climber.setUp(); - } - } - - @Override - public void end(boolean interrupted) { - climber.stopMotor(); - climber.closeFunnel(); - RobotObserver.setClimbed(true); - } - - @Override - public boolean isFinished() { - if (m_climbing) { - return climber.atClimb(); - } else { - return climber.climbReady(); - } - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/CoralEjectCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/CoralEjectCommand.java deleted file mode 100644 index 5031da7a..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/CoralEjectCommand.java +++ /dev/null @@ -1,39 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.CoralRollers; -import frc.robot.subsystems.Elevator; - -public class CoralEjectCommand extends Command { - private final CoralRollers coral; - private final Elevator elevator; - - public CoralEjectCommand(CoralRollers coralRollers, Elevator elevator) { - this.coral = coralRollers; - this.elevator = elevator; - addRequirements(coralRollers, elevator); - } - - @Override - public void initialize() { - elevator.setEject(); - } - - @Override - public void execute() { - if (elevator.atSetpoint()) { - coral.setSpitOut(); - } - } - - @Override - public void end(boolean interrupted) { - coral.stop(); - elevator.release(); - } - - @Override - public boolean isFinished() { - return !coral.intakeReady(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/CoralIntakeCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/CoralIntakeCommand.java deleted file mode 100644 index 3ae24b0c..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/CoralIntakeCommand.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.commands; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.CoralRollers; -import frc.robot.subsystems.Elevator; - -public class CoralIntakeCommand extends Command { - @SuppressWarnings("unused") - private final Logger m_logger = LoggerFactory.getLogger(CoralRollers.class); - - private final CoralRollers coral; - private final Elevator elevator; - - public CoralIntakeCommand(CoralRollers coralRollers, Elevator elevator) { - this.coral = coralRollers; - this.elevator = elevator; - addRequirements(coralRollers, elevator); - } - - @Override - public void initialize() { - elevator.setStow(); - } - - @Override - public void execute() { - if(elevator.atSetpoint()) { - coral.setIntake(); - } - } - - @Override - public void end(boolean interrupted) { - coral.stop(); - elevator.release(); - } - - @Override - public boolean isFinished() { - return coral.holdingPiece(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/CoralL1Command.java b/ThriftyTest/src/main/java/frc/robot/commands/CoralL1Command.java deleted file mode 100644 index bb021dc4..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/CoralL1Command.java +++ /dev/null @@ -1,39 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.CoralRollers; -import frc.robot.subsystems.Elevator; - -public class CoralL1Command extends Command { - private final CoralRollers coral; - private final Elevator elevator; - - public CoralL1Command(CoralRollers coralRollers, Elevator elevator) { - this.coral = coralRollers; - this.elevator = elevator; - addRequirements(coralRollers, elevator); - } - - @Override - public void initialize() { - elevator.setL1(); - } - - @Override - public void execute() { - if (elevator.atSetpoint()) { - coral.slowScore(); - } - } - - @Override - public void end(boolean interrupted) { - coral.stop(); - elevator.release(); - } - - @Override - public boolean isFinished() { - return !coral.holdingPiece(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/CoralScoreCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/CoralScoreCommand.java deleted file mode 100644 index 106d3bf8..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/CoralScoreCommand.java +++ /dev/null @@ -1,55 +0,0 @@ -package frc.robot.commands; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.CoralRollers; -import frc.robot.subsystems.Elevator; - -public class CoralScoreCommand extends Command { - private final Logger m_logger = LoggerFactory.getLogger(CoralScoreCommand.class); - - private final CoralRollers coral; - private final Elevator elevator; - private final int level; - - private boolean finish = false; - - public CoralScoreCommand(CoralRollers coralRollers, Elevator elevator, int level) { - this.coral = coralRollers; - this.elevator = elevator; - this.level = level; - addRequirements(coralRollers, elevator); - } - - @Override - public void initialize() { - // finish = !coral.holdingPiece(); - elevator.setLevel(level); - } - - @Override - public void execute() { - if(elevator.atSetpoint()) { - switch(level) { - case 1 -> coral.setL1Eject(); - case 2 -> coral.setL2Eject(); - case 3 -> coral.setL3Eject(); - case 4 -> coral.setL4Eject(); - default -> m_logger.warn("invalid setpoint: {}", level); - } - } - } - - @Override - public void end(boolean interrupted) { - coral.stop(); - elevator.release(); - } - - @Override - public boolean isFinished() { - return !coral.getFrontCANrange() || finish; - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/DriveToPointCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/DriveToPointCommand.java deleted file mode 100644 index 8ddfc844..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/DriveToPointCommand.java +++ /dev/null @@ -1,158 +0,0 @@ -package frc.robot.commands; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import static edu.wpi.first.units.Units.Radians; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.AutonConstants; -import frc.robot.Constants.DriveConstants; -import frc.robot.RobotObserver; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.utils.FieldUtils; - -public class DriveToPointCommand extends Command { - private final Logger m_logger = LoggerFactory.getLogger(DriveToPointCommand.class); - - private final double dt = 0.02; - - private static Rotation2d m_targetRotation; - - private final SwerveRequest.FieldCentricFacingAngle m_request = new SwerveRequest.FieldCentricFacingAngle() - .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) - .withHeadingPID(DriveConstants.k_driveToPointRotationPID.kP, 0, 0) - .withSteerRequestType(SteerRequestType.MotionMagicExpo) - .withDriveRequestType(DriveRequestType.Velocity); - - private Pose2d m_goal; - private final CommandSwerveDrivetrain m_drivetrain; - - private boolean m_flip; - - public DriveToPointCommand(Pose2d pose, CommandSwerveDrivetrain drivetrain) { - this(pose, drivetrain, false); - } - - public DriveToPointCommand(Pose2d pose, CommandSwerveDrivetrain drivetrain, boolean flipPose) { - m_goal = pose; - m_drivetrain = drivetrain; - m_flip = flipPose; - addRequirements(drivetrain); - } - - @Override - public void initialize() { - m_drivetrain.setAligned(false); - // flip goal if necessary - if (m_flip) { - m_goal = FieldUtils.flipPose(m_goal); - m_flip = false; - } - m_targetRotation = m_goal.getRotation(); - - RobotObserver.getField().getObject("target").setPose(m_goal); - } - - @Override - public void execute() { - Translation2d adjusted = adjust(m_drivetrain.getPose(), m_goal); - if (adjusted.getX() == 0 || adjusted.getY() == 0){ - m_logger.info("X/Y is Zero, current pose is {}, velocity is {}", m_drivetrain.getPose(), m_drivetrain.getVelocityComponents()); - } - m_drivetrain.setControl(m_request - .withVelocityX(adjusted.getX()) - .withVelocityY(adjusted.getY()) - .withTargetDirection(m_targetRotation) - ); - } - - private Translation2d adjust(Pose2d current, Pose2d goal) { - - Translation2d robotToTarget = goal.getTranslation().minus(current.getTranslation()); - - double distance = robotToTarget.getNorm(); - - if (distance == 0) { - return Translation2d.kZero; - } - - double theoreticalMaxVelocity = Math.sqrt(2 * distance * DriveConstants.kMaxAccelerationTowardsTarget); - - Translation2d currentVelocity = m_drivetrain.getVelocityComponents(); - - Translation2d direction = robotToTarget.div(distance); - - if (currentVelocity.getNorm() == 0) { - m_logger.trace("Current Velocity was Zero"); - double velocity = Math.min(theoreticalMaxVelocity, dt * DriveConstants.kMaxAccelerationTowardsTarget); - - return direction.times(velocity); - } - - double dot = direction.getX() * currentVelocity.getX() + direction.getY() * currentVelocity.getY(); - - if (dot == 0) { - m_logger.trace("Dot was Zero, currentVelocity {}, Distance {}, Direction {}", currentVelocity, distance, direction); - // if we are completely perpendicular with the ideal translation, we can assume that current velocity IS u - double velocityI = Math.min(theoreticalMaxVelocity, dt * DriveConstants.kMaxAccelerationTowardsTarget); - Translation2d veloI = direction.times(velocityI); - - double adjustmentU = Math.min(dt * DriveConstants.kMaxAccelerationPerpendicularToTarget, currentVelocity.getNorm()); - // currentVelocity - maxAdjustmentU * currentVelocity hat - Translation2d directionU = currentVelocity.div(currentVelocity.getNorm()); - Translation2d veloU = currentVelocity.minus(directionU.times(adjustmentU)); - - return veloI.plus(veloU); - } - - double currentVelocityTowardsTarget = Math.pow(currentVelocity.getNorm(), 2) / dot; - Translation2d currentVeloI = direction.times(currentVelocityTowardsTarget); - - Translation2d currentVeloU = currentVelocity.minus(currentVeloI); - double currentVelocityPerpendicularToTarget = currentVeloU.getNorm(); - - double adjustmentI = Math.min(theoreticalMaxVelocity, dt * DriveConstants.kMaxAccelerationTowardsTarget + currentVelocityTowardsTarget); - Translation2d veloI = direction.times(adjustmentI); - - double adjustmentU = Math.min(currentVeloU.getNorm(), dt * DriveConstants.kMaxAccelerationPerpendicularToTarget); - Translation2d veloU = Translation2d.kZero; - if (currentVelocityPerpendicularToTarget != 0) { - Translation2d directionU = currentVeloU.div(currentVelocityPerpendicularToTarget); - veloU = currentVeloU.minus(directionU.times(adjustmentU)); - } - - Translation2d r = veloI.plus(veloU); - m_logger.trace("Adjust() returns {}" , r); - - return r; - } - - @Override - public void end(boolean interrupted) { - m_drivetrain.setAligned(!interrupted); - m_drivetrain.stop(); - RobotObserver.getField().getObject("target").setPoses(); - } - - @Override - public boolean isFinished() { - double errX = m_drivetrain.getPose().getX() - m_goal.getX(); - double errY = m_drivetrain.getPose().getY() - m_goal.getY(); - double err = Math.hypot(errX, errY); - - double errRotation = Math.abs(m_drivetrain.getPose() - .getRotation() - .minus(m_targetRotation) - .getRadians()); - - return (err < AutonConstants.translationTolerance && errRotation < AutonConstants.rotationTolerance.in(Radians)); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/ElevatorDefaultCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/ElevatorDefaultCommand.java deleted file mode 100644 index 511fbff9..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/ElevatorDefaultCommand.java +++ /dev/null @@ -1,41 +0,0 @@ -package frc.robot.commands; - -import frc.robot.RobotObserver; -import frc.robot.subsystems.Elevator; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorDefaultCommand extends Command { - private final Logger m_logger = LoggerFactory.getLogger(ElevatorDefaultCommand.class); - private final Elevator m_elevator; - - public ElevatorDefaultCommand(Elevator elevator) { - addRequirements(elevator); - m_elevator = elevator; - } - - @Override - public void initialize() { - m_logger.debug("Starting"); - } - - @Override - public void execute() { - if (m_elevator.taken()) return; - if (RobotObserver.getReefReady() && !DriverStation.isAutonomous()) { - m_elevator.setPrep(); - } else { - m_elevator.setStow(); - } - m_elevator.release(); - } - - @Override - public void end(boolean interrupted) { - m_logger.debug("Ending"); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/ElevatorToPointCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/ElevatorToPointCommand.java deleted file mode 100644 index f786ea05..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/ElevatorToPointCommand.java +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Elevator; - -public class ElevatorToPointCommand extends Command { - private final int level; - private final Elevator elevator; - - public ElevatorToPointCommand(int level, Elevator elevator) { - this.level = level; - this.elevator = elevator; - addRequirements(elevator); - } - - @Override - public void initialize() { - elevator.setLevel(level); - } - - @Override - public boolean isFinished() { - return elevator.atSetpoint(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/ElevatorZero.java b/ThriftyTest/src/main/java/frc/robot/commands/ElevatorZero.java deleted file mode 100644 index cfdad768..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/ElevatorZero.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.RobotObserver; -import frc.robot.subsystems.Elevator; - -public class ElevatorZero extends Command { - private boolean applied; - - private final Elevator m_elevator; - /** Creates a new ElevatorZero. */ - public ElevatorZero(Elevator elevator) { - m_elevator = elevator; - addRequirements(elevator); - } - - @Override - public void initialize() { - applied = false; - m_elevator.setStow(); - } - - @Override - public void execute() { - if (m_elevator.atSetpoint() && !RobotObserver.getCoralPieceHeld()) { - applied = true; - m_elevator.prepZero(); - } - } - - @Override - public void end(boolean interrupted) { - if (!interrupted && !RobotObserver.getCoralPieceHeld()) m_elevator.zeroElevator(); - m_elevator.enableLimits(); - m_elevator.release(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return m_elevator.atZero() && applied || RobotObserver.getCoralPieceHeld(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/ManualElevatorCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/ManualElevatorCommand.java deleted file mode 100644 index b45fe1fc..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/ManualElevatorCommand.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.subsystems.Elevator; - -public class ManualElevatorCommand extends Command { - private final boolean isUp; - private final Elevator elevator; - - public ManualElevatorCommand(Elevator elevator, boolean isUp) { - this.isUp = isUp; - this.elevator = elevator; - addRequirements(elevator); - } - - @Override - public void execute() { - elevator.setSpeed(isUp ? ElevatorConstants.manualUpSpeed : ElevatorConstants.manualDownSpeed); - } - - @Override - public void end(boolean interrupted) { - elevator.stop(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/ManualPivotCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/ManualPivotCommand.java deleted file mode 100644 index 9d123423..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/ManualPivotCommand.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Pivot; - -public class ManualPivotCommand extends Command { - private final boolean isUp; - private final Pivot pivot; - - public ManualPivotCommand(Pivot pivot, boolean isUp) { - this.isUp = isUp; - this.pivot = pivot; - addRequirements(pivot); - } - - @Override - public void execute() { - pivot.setSpeed(isUp ? PivotConstants.manualUpSpeed : PivotConstants.manualDownSpeed); - } - - @Override - public void end(boolean interrupted) { - pivot.stop(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/OpenFunnel.java b/ThriftyTest/src/main/java/frc/robot/commands/OpenFunnel.java deleted file mode 100644 index 046900ec..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/OpenFunnel.java +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Climber; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class OpenFunnel extends Command { - private Climber m_climber; - private int timeRemaining; - /** Creates a new OpenFunnel. */ - public OpenFunnel(Climber climber) { - m_climber = climber; - addRequirements(climber); - } - - @Override - public void initialize() { - m_climber.openFunnel(); - timeRemaining = 75; - } - - @Override - public void execute() { - timeRemaining --; - } - - @Override - public boolean isFinished() { - return timeRemaining == 0; - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/PitClimbSetupCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/PitClimbSetupCommand.java deleted file mode 100644 index 3c83c02a..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/PitClimbSetupCommand.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Climber; - -public class PitClimbSetupCommand extends Command { - private final Climber m_climber; - - public PitClimbSetupCommand(Climber climber) { - m_climber = climber; - } - - @Override - public void initialize() { - m_climber.setClimbRoll(); - } - - @Override - public void end(boolean interrupted) { - m_climber.stopMotor(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/ProcessorCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/ProcessorCommand.java deleted file mode 100644 index affceec3..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/ProcessorCommand.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.AlgaeRollers; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Pivot; - -public class ProcessorCommand extends Command { - private Elevator m_elevator; - private Pivot m_pivot; - - public ProcessorCommand(Elevator elevator, AlgaeRollers rollers, Pivot pivot) { - m_elevator = elevator; - m_pivot = pivot; - addRequirements(elevator, rollers, pivot); - } - - @Override - public void initialize() { - m_elevator.setProcessor(); - m_pivot.setProcessor(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/StowCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/StowCommand.java deleted file mode 100644 index fa831430..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/StowCommand.java +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Pivot; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class StowCommand extends Command { - private final Pivot m_pivot; - private final Elevator m_elevator; - - /** Creates a new StowCommand. */ - public StowCommand(Elevator elevator, Pivot pivot) { - m_pivot = pivot; - m_elevator = elevator; - addRequirements(m_pivot, m_elevator); - } - - @Override - public void initialize() { - m_pivot.setStow(); - m_elevator.setStow(); - } - - @Override - public void end(boolean interrupted) { - m_pivot.stop(); - } - - @Override - public boolean isFinished() { - return m_elevator.atSetpoint() && m_pivot.atSetpoint(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/commands/TeleopCommand.java b/ThriftyTest/src/main/java/frc/robot/commands/TeleopCommand.java deleted file mode 100644 index 8672fcca..00000000 --- a/ThriftyTest/src/main/java/frc/robot/commands/TeleopCommand.java +++ /dev/null @@ -1,104 +0,0 @@ -package frc.robot.commands; - -import java.util.function.Supplier; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; - -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.ButtonBindingConstants.DragonReins; -import frc.robot.driveassist.DriverAssist; -import frc.robot.subsystems.CommandSwerveDrivetrain; - -public class TeleopCommand extends Command { - private final CommandSwerveDrivetrain m_drivetrain; - private final Supplier m_xSupplier; - private final Supplier m_ySupplier; - private final Supplier m_rotSupplier; - - private final double maxTranslationalVelocity = DriveConstants.k_maxTeleopLinearSpeed; - private final double maxRotationalVelocity = DriveConstants.k_maxTeleopAngularSpeed; - - private final DriverAssist m_assist = new DriverAssist(); - - private final SwerveRequest.FieldCentric driveClosedLoop = new SwerveRequest.FieldCentric() - .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) - .withDeadband(maxTranslationalVelocity * DragonReins.deadband) - .withRotationalDeadband(maxRotationalVelocity * DragonReins.deadband) - .withDriveRequestType(DriveRequestType.Velocity); - - public TeleopCommand( - CommandSwerveDrivetrain drivetrain, - Supplier xSupplier, - Supplier ySupplier, - Supplier rotSupplier - ) { - m_drivetrain = drivetrain; - m_xSupplier = xSupplier; - m_ySupplier = ySupplier; - m_rotSupplier = rotSupplier; - addRequirements(drivetrain); - } - - @Override - public void execute() { - // calculate the field-relative speeds - Transform2d robotRelative = new Transform2d( - new Translation2d( - m_xSupplier.get() * maxTranslationalVelocity, - m_ySupplier.get() * maxTranslationalVelocity - ), - new Rotation2d( - m_rotSupplier.get() * maxRotationalVelocity - ) - ); - if (robotRelative.getTranslation().getNorm() > 0) { - m_drivetrain.setAligned(false); - } - Transform2d fieldRelative = getFieldRelative(robotRelative); - // avoid obstacles using drive assist - Translation2d filtered = m_assist.calculate( - fieldRelative.getTranslation(), - m_drivetrain.getPose(), - m_drivetrain.getNearestAntitarget() - ); - Transform2d out = new Transform2d(filtered, fieldRelative.getRotation()); - applyVelocities(out); - } - - /** - * From a robot relative position, returns the field relative pose, using - * the drivetrain's operator perspective - */ - private Transform2d getFieldRelative(Transform2d robotRelative) { - // get the offset - Rotation2d forward = m_drivetrain.getOperatorForwardDirection(); - // get the original position - double x = robotRelative.getX(); - double y = robotRelative.getY(); - Rotation2d theta = robotRelative.getRotation(); - // calculate the new position after rotation - double px = x * forward.getCos() - y * forward.getSin(); - double py = y * forward.getCos() + x * forward.getSin(); - // combine the results - return new Transform2d(new Translation2d(px, py), theta); - } - - /** - * Applies a transform2d with field relative velocities to the drivetrain - */ - private void applyVelocities(Transform2d fieldRelative) { - m_drivetrain.setControl( - driveClosedLoop - .withVelocityX(fieldRelative.getX()) - .withVelocityY(fieldRelative.getY()) - .withRotationalRate(fieldRelative.getRotation().getRadians()) - ); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/driveassist/DriverAssist.java b/ThriftyTest/src/main/java/frc/robot/driveassist/DriverAssist.java deleted file mode 100644 index 7efee08d..00000000 --- a/ThriftyTest/src/main/java/frc/robot/driveassist/DriverAssist.java +++ /dev/null @@ -1,79 +0,0 @@ -package frc.robot.driveassist; - - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.FFConstants; -import frc.robot.RobotObserver; - -public class DriverAssist { - private final Logger m_logger = LoggerFactory.getLogger(DriverAssist.class); - - /** - * Returns the calculated drive command to apply to the drivetrian - * @param velocity the robot's requested velocity, in field relative - * units. - * @param current The robot's current position on the field - * @param antitarget The closest force field antitarget - * @return the field relative adjusted velocity - */ - public Translation2d calculate( - Translation2d velocity, - Pose2d current, - Pose2d antitarget - ) { - Translation2d x = antitarget.getTranslation().minus(current.getTranslation()); - double xNorm = x.getNorm(); - if (xNorm == 0) { - m_logger.warn("position is exactly ff center"); - return velocity; - } - boolean inside = false; - if (xNorm < FFConstants.k_radius) { - inside = true; - } else { - x = x.times((xNorm - FFConstants.k_radius) / xNorm); - } - Translation2d t = x.times( - Math.sqrt(2 * FFConstants.k_decceleration/ x.getNorm()) - ); - boolean active = t.getNorm() < DriveConstants.k_maxTeleopLinearSpeed - && RobotObserver.getFFEnabled(); - if (!active) { - RobotObserver.getField().getObject("FF").setPoses(); - return velocity; - } - RobotObserver.getField().getObject("FF").setPose(antitarget); - return adjust(velocity, t, inside); - } - - private Translation2d adjust( - Translation2d c, - Translation2d t, - boolean inside - ) { - double d = dot(c, t); - if (d <= 0) { - return c; - } - double magI = d / t.getNorm(); - if (magI <= 0) { - return c; - } - Translation2d i = t.times(magI / t.getNorm()); - Translation2d u = c.minus(i); - Translation2d a = t.times(magI / DriveConstants.k_maxTeleopLinearSpeed); - if (t.getNorm() <= 0 || inside) { - return u; - } - return u.plus(a); - } - - private double dot(Translation2d a, Translation2d b) { - return a.getX() * b.getX() + a.getY() * b.getY(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/driveassist/ForceField.java b/ThriftyTest/src/main/java/frc/robot/driveassist/ForceField.java new file mode 100644 index 00000000..e65619cf --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/driveassist/ForceField.java @@ -0,0 +1,15 @@ +package frc.robot.driveassist; + +import frc.robot.Constants.FFConstants; + +public class ForceField { + public static double calculate(double velo, double disp) { + if (disp * velo <= 0) { + // we're going in the same direction as displacement, i.e. away from barge + return velo; + } + double maxVelocity = Math.sqrt( + 2 * FFConstants.k_decceleration * Math.max(0, Math.abs(disp) - FFConstants.k_radius)); + return Math.min(maxVelocity, Math.abs(velo)) * Math.signum(velo); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/generated/TunerConstants.java b/ThriftyTest/src/main/java/frc/robot/generated/TunerConstants.java index 46a4171d..9a50bf2c 100644 --- a/ThriftyTest/src/main/java/frc/robot/generated/TunerConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/generated/TunerConstants.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; @@ -33,273 +32,266 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Voltage; -import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(49.33).withKI(0).withKD(3.18488) - .withKS(0.099311).withKV(2.45135).withKA(0.1040918) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.182115).withKI(0).withKD(0) - .withKS(0.16669).withKV(0.123925).withKA(0.0075743); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(81)) - .withStatorCurrentLimitEnable(true) - ); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("CANivore", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.724); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 0; - - private static final double kDriveGearRatio = 6.75; - private static final double kSteerGearRatio = 25; - public static final Distance kWheelRadius = Inches.of(2); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 50; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - - // Front Left - private static final int kFrontLeftDriveMotorId = 11; - private static final int kFrontLeftSteerMotorId = 12; - private static final int kFrontLeftEncoderId = 13; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.1201171875); - private static final boolean kFrontLeftSteerMotorInverted = false; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(12.25); - private static final Distance kFrontLeftYPos = Inches.of(12.25); - - // Front Right - private static final int kFrontRightDriveMotorId = 21; - private static final int kFrontRightSteerMotorId = 22; - private static final int kFrontRightEncoderId = 23; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.30517578125); - private static final boolean kFrontRightSteerMotorInverted = false; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(12.25); - private static final Distance kFrontRightYPos = Inches.of(-12.25); - - // Back Left - private static final int kBackLeftDriveMotorId = 31; - private static final int kBackLeftSteerMotorId = 32; - private static final int kBackLeftEncoderId = 33; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.1318359375); - private static final boolean kBackLeftSteerMotorInverted = false; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-12.25); - private static final Distance kBackLeftYPos = Inches.of(12.25); - - // Back Right - private static final int kBackRightDriveMotorId = 41; - private static final int kBackRightSteerMotorId = 42; - private static final int kBackRightEncoderId = 43; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.429443359375); - private static final boolean kBackRightSteerMotorInverted = false; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-12.25); - private static final Distance kBackRightYPos = Inches.of(-12.25); - - - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted - ); - + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(40.0); + + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.182115).withKI(0).withKD(0) + .withKS(0.16669).withKV(0.123925).withKA(0.0075743); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = + SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + // TODO: is this correct? I think this is the default and we've just ran with it + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(81)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("CANivore", "./logs/example.hoot"); + // TODO: is example.hoot real? + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.724); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 0; // FIXME: I 100% KNOW this is wrong + + private static final double kDriveGearRatio = 6.75; + private static final double kSteerGearRatio = 25; + public static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 50; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 11; + private static final int kFrontLeftSteerMotorId = 12; + private static final int kFrontLeftEncoderId = 13; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.1201171875); + private static final boolean kFrontLeftSteerMotorInverted = false; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(12.25); + private static final Distance kFrontLeftYPos = Inches.of(12.25); + + // Front Right + private static final int kFrontRightDriveMotorId = 21; + private static final int kFrontRightSteerMotorId = 22; + private static final int kFrontRightEncoderId = 23; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.30517578125); + private static final boolean kFrontRightSteerMotorInverted = false; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(12.25); + private static final Distance kFrontRightYPos = Inches.of(-12.25); + + // Back Left + private static final int kBackLeftDriveMotorId = 31; + private static final int kBackLeftSteerMotorId = 32; + private static final int kBackLeftEncoderId = 33; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.1318359375); + private static final boolean kBackLeftSteerMotorInverted = false; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-12.25); + private static final Distance kBackLeftYPos = Inches.of(12.25); + + // Back Right + private static final int kBackRightDriveMotorId = 41; + private static final int kBackRightSteerMotorId = 42; + private static final int kBackRightEncoderId = 43; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.429443359375); + private static final boolean kBackRightSteerMotorInverted = false; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-12.25); + private static final Distance kBackRightYPos = Inches.of(-12.25); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + + /** + * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot + * program,. + */ + public static CommandSwerveDrivetrain createDrivetrain() { + return new CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + } + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module */ - public static CommandSwerveDrivetrain createDrivetrain() { - return new CommandSwerveDrivetrain( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules); } + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules); + } /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation in the form + * [x, y, theta]áµ€, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, + * y, theta]áµ€, with units in meters and radians + * @param modules Constants for each specific module */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); - } + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules); } + } } diff --git a/ThriftyTest/src/main/java/frc/robot/generated/TunerConstantsThrifty.java b/ThriftyTest/src/main/java/frc/robot/generated/TunerConstantsThrifty.java index c29b2dfa..c08e649c 100644 --- a/ThriftyTest/src/main/java/frc/robot/generated/TunerConstantsThrifty.java +++ b/ThriftyTest/src/main/java/frc/robot/generated/TunerConstantsThrifty.java @@ -34,7 +34,7 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Voltage; -import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html @@ -308,4 +308,4 @@ public TunerSwerveDrivetrain( ); } } -} \ No newline at end of file +} diff --git a/ThriftyTest/src/main/java/frc/robot/stateSpace/StateSpaceController.java b/ThriftyTest/src/main/java/frc/robot/stateSpace/StateSpaceController.java index a849585e..628ba47f 100644 --- a/ThriftyTest/src/main/java/frc/robot/stateSpace/StateSpaceController.java +++ b/ThriftyTest/src/main/java/frc/robot/stateSpace/StateSpaceController.java @@ -24,7 +24,7 @@ public class StateSpaceController lastMeasurement = m_outputSupplier.get(); m_loop.correct(lastMeasurement); @@ -66,10 +68,10 @@ private void periodic() { // Send new readings to SmartDashboard for logging purposes for (int row = 0; row < lastMeasurement.getNumRows(); row++) { double measurement = lastMeasurement.get(row); - SmartDashboard.putNumber(m_name + " (" + row + ")", measurement); + SmartDashboard.putNumber("statespace - " + m_name + "/" + row, measurement); } - isAtSetpoint = Math.abs(lastMeasurement.get(0) - getReference()) < m_tolerance; + m_atSetpoint = Math.abs(lastMeasurement.get(0) - getReference()) < m_tolerance; } /** @@ -85,8 +87,8 @@ public double getReference() { return m_loop.getNextR(0); } - public boolean isAtSetpoint() { - return isAtSetpoint; + public boolean isM_atSetpoint() { + return m_atSetpoint; } @Override diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/AlgaeRollers.java b/ThriftyTest/src/main/java/frc/robot/subsystems/AlgaeRollers.java deleted file mode 100644 index b6c94c6c..00000000 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/AlgaeRollers.java +++ /dev/null @@ -1,108 +0,0 @@ -package frc.robot.subsystems; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.math.filter.MedianFilter; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.AlgaeRollerConstants; -import frc.robot.Constants.IDConstants; -import frc.robot.Robot; -import frc.robot.RobotObserver; - -public class AlgaeRollers extends SubsystemBase implements AutoCloseable { - @SuppressWarnings("unused") - private final Logger m_logger = LoggerFactory.getLogger(AlgaeRollers.class); - - private final TalonFX m_algaeRoller = new TalonFX(IDConstants.algae); - - private double m_voltage; - private boolean m_voltageChanged; - - private boolean m_hasAlgae; - - private MedianFilter m_filter = new MedianFilter(10); - - public AlgaeRollers() { - configIntakeMotor(); - RobotObserver.setAlgaePieceHeldSupplier(this::algaeHeld); - } - - private void configIntakeMotor() { - m_algaeRoller.clearStickyFaults(); - m_algaeRoller.getConfigurator().apply(AlgaeRollerConstants.motorConfig); - } - - private void setMotor(double voltage) { - if (voltage != m_voltage) { - m_voltageChanged = true; - } - m_voltage = voltage; - } - - public boolean algaeHeld() { - return m_hasAlgae; - } - - public void intakeAlgae() { - if (algaeHeld()) { - setMotor(AlgaeRollerConstants.holdVoltage); - } else { - setMotor(AlgaeRollerConstants.intakeVoltage); - } - } - - public void smartStop() { - if (algaeHeld()) { - setMotor(AlgaeRollerConstants.holdVoltage); - } else { - stopMotor(); - } - } - - private double getTorqueCurrent() { - double measurement = m_algaeRoller.getTorqueCurrent().getValueAsDouble(); - return m_filter.calculate(measurement); - } - - public void ejectAlgae() { - setMotor(AlgaeRollerConstants.ejectVoltage); - } - - public void processorEjectAlgae() { - setMotor(AlgaeRollerConstants.processorEjectVoltage); - } - - public void stopMotor() { - setMotor(0); - } - - private void updateObjectState() { - if (Robot.isReal()) { - m_hasAlgae = getTorqueCurrent() >= AlgaeRollerConstants.torqueCurrentThreshold; - } else { - m_hasAlgae = SmartDashboard.getBoolean("Algae Held", false); - } - - SmartDashboard.putBoolean("Algae Held", m_hasAlgae); - } - - @Override - public void periodic() { - SmartDashboard.putNumber("Algae Temp", m_algaeRoller.getDeviceTemp().getValueAsDouble()); - updateObjectState(); - if (m_voltageChanged) { - m_algaeRoller.setVoltage(m_voltage); - m_voltageChanged = false; - } - } - - @Override - public void close() throws Exception { - m_algaeRoller.close(); - } - -} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/Climber.java b/ThriftyTest/src/main/java/frc/robot/subsystems/Climber.java deleted file mode 100644 index 79b79c03..00000000 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/Climber.java +++ /dev/null @@ -1,121 +0,0 @@ -package frc.robot.subsystems; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ClimberConstants; -import frc.robot.Constants.IDConstants; - -public class Climber extends SubsystemBase implements AutoCloseable { - @SuppressWarnings("unused") - private final Logger m_logger = LoggerFactory.getLogger(Climber.class); - private final TalonFX m_leftClimbMotor = new TalonFX(IDConstants.climbLeft); - private final TalonFX m_rightClimbMotor = new TalonFX(IDConstants.climbRight); - private final CANcoder m_encoder = new CANcoder(IDConstants.climbEncoder); - - private final Servo m_servo = new Servo(IDConstants.servo); - - private double m_voltage; - private boolean m_voltageChanged; - - private final VoltageOut m_request = new VoltageOut(0); - - public Climber() { - configMotors(); - configEncoder(); - } - - private void configEncoder() { - m_encoder.getConfigurator().apply(ClimberConstants.encoderConfig); - } - - private void configMotors() { - m_leftClimbMotor.clearStickyFaults(); - m_rightClimbMotor.clearStickyFaults(); - m_leftClimbMotor.getConfigurator().apply(ClimberConstants.motorConfig); - m_rightClimbMotor.getConfigurator().apply(ClimberConstants.motorConfig); - m_rightClimbMotor.setControl(new Follower(IDConstants.climbLeft, ClimberConstants.rightMotorInvert)); - } - - public void openFunnel() { - if (DriverStation.getMatchTime() > ClimberConstants.kClimbTime || DriverStation.isAutonomous()) { - return; - } - m_servo.set(ClimberConstants.k_openServoPosition); - } - - public void closeFunnel() { - m_servo.set(ClimberConstants.k_closedServoPosition); - } - - private void setMotor(double voltage) { - m_voltageChanged = (m_voltage != voltage); - m_voltage = voltage; - } - - /** - * Causes the climber to go up. NOT THE ROBOT - */ - public void setUp() { - setMotor(ClimberConstants.climberUpVolts); - } - - /** - * Causes the climber to go down. NOT THE ROBOT. - */ - public void setDown() { - setMotor(ClimberConstants.climbDownVolts); - } - - public void setClimbRoll() { - m_leftClimbMotor.setControl(m_request.withOutput(ClimberConstants.climbRollVolts)); - } - - public void stopMotor() { - m_leftClimbMotor.stopMotor(); - m_voltage = 0.0; - m_voltageChanged = false; - } - - public double getPosition() { - return m_leftClimbMotor.getPosition().getValueAsDouble(); - } - - @Override - public void periodic() { - if (m_voltageChanged) { - m_leftClimbMotor.setControl(m_request.withOutput(m_voltage)); - m_voltageChanged = false; - } - SmartDashboard.putBoolean("Climb Ready", climbReady()); - SmartDashboard.putBoolean("Climbed", atClimb()); - SmartDashboard.putNumber("climber pos", m_encoder.getPosition().getValueAsDouble()); - } - - public double getVelocity() { - return m_leftClimbMotor.getVelocity().getValueAsDouble(); - } - - public boolean atClimb() { - return getPosition() <= ClimberConstants.climbPosition; - } - - public boolean climbReady() { - return getPosition() > ClimberConstants.climbReadyTolerance; - } - - @Override - public void close() throws Exception { - m_leftClimbMotor.close(); - m_rightClimbMotor.close(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java deleted file mode 100644 index b930533c..00000000 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ /dev/null @@ -1,347 +0,0 @@ -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; - -import java.util.function.Supplier; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModule; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.ctre.phoenix6.swerve.SwerveRequest.ApplyRobotSpeeds; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.util.DriveFeedforwards; -import com.pathplanner.lib.util.swerve.SwerveSetpoint; -import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.FFConstants; -import frc.robot.Constants.FieldConstants; -import frc.robot.Constants.SimConstants; -import frc.robot.Robot; -import frc.robot.RobotObserver; -import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; -import frc.robot.utils.AutonomousUtil; -import frc.robot.utils.FieldUtils; -import frc.robot.vision.TimestampedPoseEstimate; - -/** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. - */ -public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { - @SuppressWarnings("unused") - private final Logger m_logger = LoggerFactory.getLogger(CommandSwerveDrivetrain.class); - - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - private boolean m_aligned; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - private Pose2d m_estimatedPose = new Pose2d(); - - private double m_oldVisionTimestamp = -1; - - private boolean m_validPose = false; - - private SwerveSetpointGenerator setpointGenerator; - private SwerveSetpoint previousSetpoint; - private final ApplyRobotSpeeds autoRequest = new ApplyRobotSpeeds().withDriveRequestType(SwerveModule.DriveRequestType.Velocity); - - public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules) { - super(drivetrainConstants, modules); - setup(); - } - - public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - setup(); - } - - public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, double odometryUpdateFrequency, - Matrix odometryStandardDeviation, Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, - modules); - setup(); - } - - private void setup() { - AutonomousUtil.initializePathPlanner(this); - if (Robot.isSimulation()) { - startSimThread(); - } - m_aligned = false; - - RobotObserver.setVisionValidSupplier(this::getVisionValid); - RobotObserver.setPoseSupplier(this::getPose); - RobotObserver.setVelocitySupplier(this::getVelocity); - RobotObserver.setNoElevatorZoneSupplier(this::noElevatorZone); - RobotObserver.setReefReadySupplier(this::getReefReady); - RobotObserver.setAlginedSupplier(this::isAligned); - } - - public void initializeSetpointGenerator(RobotConfig config) { - setpointGenerator = new SwerveSetpointGenerator(config, Units.rotationsToRadians(DriveConstants.k_maxRotationalSpeed)); - - ChassisSpeeds currSpeeds = getRobotRelativeSpeeds(); - SwerveModuleState[] currStates = getState().ModuleStates; - previousSetpoint = new SwerveSetpoint(currSpeeds, currStates, DriveFeedforwards.zeros(config.numModules)); - } - - public Translation2d getVelocityComponents() { - double vx = getRobotRelativeSpeeds().vxMetersPerSecond; - double vy = getRobotRelativeSpeeds().vyMetersPerSecond; - Rotation2d theta = getPose().getRotation(); - return new Translation2d(vx, vy).rotateBy(theta); - - } - - public double getVelocity() { - Translation2d velo = getVelocityComponents(); - return velo.getNorm(); - } - - public Pose2d getPose() { - return m_estimatedPose; - } - - public Pose2d getNearestAntitarget() { - return new Pose2d(FFConstants.k_bargeX, m_estimatedPose.getY(), new Rotation2d()); - } - - /** - * returns the current pose, with red side poses flipped - */ - public Pose2d getBluePose() { - return FieldUtils.flipPose(m_estimatedPose); - } - - public void zeroPose() { - setPose(new Pose2d()); - } - - public void resetHeading() { - setOperatorPerspectiveForward(getPose().getRotation()); - } - - public void setPose(Pose2d pose) { - resetPose(pose); - } - - public ChassisSpeeds getRobotRelativeSpeeds() { - return getState().Speeds; - } - - public void driveWithChassisSpeeds(ChassisSpeeds speeds) { - previousSetpoint = setpointGenerator.generateSetpoint( - previousSetpoint, // The previous setpoint - speeds, // The desired target speeds - 0.02 // The loop time of the robot code, in seconds - ); - - setControl(autoRequest.withSpeeds(previousSetpoint.robotRelativeSpeeds())); - } - - public void stop() { - setControl(new SwerveRequest.SwerveDriveBrake()); - } - - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - @Override - public void periodic() { - m_estimatedPose = this.getState().Pose; - - SmartDashboard.putBoolean("Drivetrain Aligned", m_aligned); - SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); - - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation); - m_hasAppliedOperatorPerspective = true; - }); - } - - handleVisionToggle(); - } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(SimConstants.k_simPeriodic); - } - - private boolean getVisionValid() { - return m_validPose; - } - - private void handleVisionToggle() { - if (m_oldVisionTimestamp >= 0) { - m_validPose = Utils.getCurrentTimeSeconds() - m_oldVisionTimestamp < Constants.VisionConstants.k_visionTimeout; - } - SmartDashboard.putBoolean("VIABLE POSE", m_validPose); - } - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this)); - - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this)); - - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this)); - - public void driveRobotRelative(ChassisSpeeds speeds) { - setControl(new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds)); - } - - public void addPoseEstimate(TimestampedPoseEstimate estimate) { - m_oldVisionTimestamp = estimate.timestamp(); - // This should NOT run in simulation! - if (Robot.isSimulation()) return; - // Depending on our configs, we should use or not use the std devs - if (Constants.VisionConstants.k_useStdDevs) { - addVisionMeasurement( - estimate.pose(), - estimate.timestamp(), - estimate.stdDevs()); - } else { - addVisionMeasurement( - estimate.pose(), - estimate.timestamp()); - } - } - - public Command sysIdQuasistaticTranslation(SysIdRoutine.Direction direction) { - return m_sysIdRoutineTranslation.quasistatic(direction); - } - - public Command sysIdDynamicTranslation(SysIdRoutine.Direction direction) { - return m_sysIdRoutineTranslation.dynamic(direction); - } - - public Command sysIdQuasistaticSteer(SysIdRoutine.Direction direction) { - return m_sysIdRoutineSteer.quasistatic(direction); - } - - public Command sysIdDynamicSteer(SysIdRoutine.Direction direction) { - return m_sysIdRoutineSteer.dynamic(direction); - } - - public Command sysIdQuasistaticRotation(SysIdRoutine.Direction direction) { - return m_sysIdRoutineRotation.quasistatic(direction); - } - - public Command sysIdDynamicRotation(SysIdRoutine.Direction direction) { - return m_sysIdRoutineRotation.dynamic(direction); - } - - private boolean noElevatorZone() { - double distance = getNearestAntitarget() - .getTranslation() - .minus(m_estimatedPose.getTranslation()) - .getNorm(); - return distance < FFConstants.k_radius && !DriverStation.isAutonomous(); - } - - public void setAligned(boolean aligned) { - m_aligned = aligned; - } - - public boolean isAligned() { - return m_aligned; - } - - public boolean getReefReady() { - double distanceToReef = getBluePose().getTranslation() - .minus(FieldConstants.reefCenter) - .getNorm(); - boolean inRange = (DriverStation.isAutonomous()) ? distanceToReef <= FieldConstants.kReefReadyAuton : distanceToReef <= FieldConstants.kReefReady; - return inRange; - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/CoralRollers.java b/ThriftyTest/src/main/java/frc/robot/subsystems/CoralRollers.java deleted file mode 100644 index c98a6104..00000000 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/CoralRollers.java +++ /dev/null @@ -1,211 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.hardware.CANrange; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.CoralConstants; -import frc.robot.Constants.IDConstants; -import frc.robot.Robot; -import frc.robot.RobotObserver; - -public class CoralRollers extends SubsystemBase { - @SuppressWarnings("unused") - private final Logger m_logger = LoggerFactory.getLogger(CoralRollers.class); - - private final TalonFX m_coralLeft = new TalonFX(IDConstants.coralLeft); - private final TalonFX m_coralRight = new TalonFX(IDConstants.coralRight); - - private final CANrange m_frontRange = new CANrange(IDConstants.coralCANrange); - private final CANrange m_upperRange = new CANrange(IDConstants.upperCANrange); - private final CANrange m_innerRange = new CANrange(IDConstants.innerCANrange); - - private double m_voltage; - private boolean m_voltageChanged; - private double m_stoppedTime; - private boolean m_stoppedTimeChanged; - - public CoralRollers() { - configMotors(); - configDashboard(); - configCANrange(); - RobotObserver.setPieceHeldSupplier(this::holdingPiece); - } - - private void configMotors() { - m_coralLeft.clearStickyFaults(); - m_coralRight.clearStickyFaults(); - - m_coralLeft.getConfigurator().apply(CoralConstants.motorConfig); - m_coralRight.getConfigurator().apply(CoralConstants.motorConfig.withMotorOutput(CoralConstants.motorConfig.MotorOutput.withInverted(CoralConstants.kInvertRight))); - - // m_coralRight.setControl(new Follower(IDConstants.coralLeft, CoralConstants.rightMotorInvert)); - } - - private void configDashboard() { - if (Robot.isReal()) { - // NOTHING YET - } else { - SmartDashboard.putBoolean("Coral Override", false); - } - } - - private void configCANrange() { - m_frontRange.getConfigurator().apply(CoralConstants.frontRangeConfig); - m_upperRange.getConfigurator().apply(CoralConstants.upperRangeConfig); - m_innerRange.getConfigurator().apply(CoralConstants.innerRangeConfig); - } - - public void setVoltage(double voltage) { - m_coralLeft.setVoltage(voltage); - m_coralRight.setVoltage(voltage); - // m_voltageChanged = (voltage != m_voltage); - // m_voltage = voltage; - } - - public void setIntake() { - setVoltage(CoralConstants.intakeVoltage); - } - - public void setRetract() { - setVoltage(CoralConstants.retractVoltage); - } - - public void timeoutIntake() { - // a whole lotta logic that essentially allows u to stop the motor in default command - // when you leave the yay zone after intakeTimeout seconds automatically (max process time) or when coral detected - if (!m_stoppedTimeChanged) { - m_stoppedTimeChanged = true; - m_stoppedTime = Utils.getCurrentTimeSeconds(); - } - - if (holdingPiece()) { - stop(); - return; - } - - if (presentPiece()) { - setIntake(); - return; - } - - double elapsed = Utils.getCurrentTimeSeconds() - m_stoppedTime; - - if (elapsed > CoralConstants.intakeTimeout) { - stop(); - resetTimeout(); - return; - } - - if (holdingPiece()) { - stop(); - } - } - - public void resetTimeout() { - m_stoppedTime = -1; - m_stoppedTimeChanged = false; - } - - public void setEject() { - setVoltage(CoralConstants.ejectVoltage); - } - - public void setL2Eject() { - m_logger.trace("Setting L2 eject"); - setVoltage(CoralConstants.l2EjectVoltage); - } - - public void setL3Eject() { - m_logger.trace("Setting L3 eject"); - setVoltage(CoralConstants.l3EjectVoltage); - } - - public void setL4Eject() { - m_logger.trace("Setting L4 eject"); - setVoltage(CoralConstants.l4EjectVoltage); - } - - public void setSpitOut() { - setVoltage(CoralConstants.spitOutVoltage); - } - - public void setL1Eject() { - m_coralLeft.setVoltage(CoralConstants.l1LeftEjectVoltage); - m_coralRight.setVoltage(CoralConstants.l1RightEjectVoltage); - } - - public void resetFollow() { - m_coralRight.setControl(new Follower(IDConstants.coralLeft, CoralConstants.rightMotorInvert)); - } - - public void stop() { - setVoltage(0); - } - - public boolean getFrontCANrange() { - return m_frontRange.getIsDetected().getValue(); - } - - public boolean getUpperCANrange() { - return m_upperRange.getIsDetected().getValue(); - } - - public boolean getInnerCANrange() { - return m_innerRange.getIsDetected().getValue(); - } - - public boolean holdingPiece() { - if (Robot.isReal()) { - boolean holding = getFrontCANrange() && !getUpperCANrange(); - m_logger.trace("holding: {}", holding); - return holding; - } else { - boolean present = SmartDashboard.getBoolean("Coral present", false); - SmartDashboard.putBoolean("Coral present", present); - return present; - } - } - - public void fastEject() { - setVoltage(CoralConstants.fastEjectVoltage); - } - - public void slowScore() { - setVoltage(CoralConstants.l1EjectVoltage); - } - - public boolean presentPiece() { - return getInnerCANrange() || getFrontCANrange(); - } - - public boolean intakeReady() { - return getUpperCANrange() || presentPiece(); - } - - @Override - public void periodic() { - SmartDashboard.putBoolean("Inner CANrange", getInnerCANrange()); - SmartDashboard.putBoolean("Coral CANrange", getFrontCANrange()); - SmartDashboard.putBoolean("OCS", getUpperCANrange()); - SmartDashboard.putBoolean("HAS CORAL", holdingPiece()); - - if (m_voltageChanged) { - m_coralLeft.setVoltage(m_voltage); - m_coralRight.setVoltage(m_voltage); - m_voltageChanged = false; - } - } - -} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/Elevator.java b/ThriftyTest/src/main/java/frc/robot/subsystems/Elevator.java deleted file mode 100644 index ff41e5b0..00000000 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/Elevator.java +++ /dev/null @@ -1,326 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.Seconds; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.hardware.CANrange; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.Constants.IDConstants; -import frc.robot.Constants.RobotConstants; -import frc.robot.Constants.SimConstants; -import frc.robot.Robot; -import frc.robot.RobotObserver; - -public class Elevator extends SubsystemBase { - // we want to have a logger, even if we're not using it... yet - @SuppressWarnings("unused") - private final Logger m_logger = LoggerFactory.getLogger(Elevator.class); - - private final TalonFX m_elevatorLeft = new TalonFX(IDConstants.elevatorLeft, "*"); - private final TalonFX m_elevatorRight = new TalonFX(IDConstants.elevatorRight, "*"); - - private final CANrange m_CANrange = new CANrange(IDConstants.elevatorCANrange); - - private final Debouncer m_debouncer = new Debouncer(ElevatorConstants.kRangeDebounceTime.in(Seconds)); - - private double m_position; - private double m_velocity; - - private double m_reference; - - private ElevatorSim m_elevatorSim; - private final DCMotor m_elevatorGearbox = DCMotor.getKrakenX60Foc(2); // 2 motors (left and right) - - private Mechanism2d m_mechVisual; - private MechanismRoot2d m_mechRoot; - private MechanismLigament2d m_elevatorArm; - - private boolean m_taken; - - private double m_speed; - private boolean m_speedChanged; - - public Elevator() { - configMotor(); - configCANrange(); - configSim(); - m_taken = false; - } - - private void configCANrange() { - m_CANrange.getConfigurator().apply(ElevatorConstants.kCANrangeConfig, RobotConstants.globalCanTimeout.in(Seconds)); - } - - private void configMotor() { - m_elevatorRight.getConfigurator().apply(ElevatorConstants.motorConfig, 0.2); - m_elevatorLeft.getConfigurator().apply(ElevatorConstants.motorConfig, 0.2); - Follower follower = new Follower(IDConstants.elevatorRight, ElevatorConstants.invertLeftMotorFollower); - m_elevatorLeft.setControl(follower); - m_elevatorRight.setPosition(0); - } - - private void configSim() { - m_elevatorSim = new ElevatorSim( - ElevatorConstants.stateSpacePlant, - m_elevatorGearbox, - ElevatorConstants.reverseSoftLimit, - ElevatorConstants.forwardSoftLimit, - true, - ElevatorConstants.reverseSoftLimit - ); - - m_mechVisual = new Mechanism2d(1, 12); // Width/height in meters - m_mechRoot = m_mechVisual.getRoot("ElevatorRoot", 0.5, 0.0); // Center at (0.5, 0) - m_elevatorArm = m_mechRoot.append(new MechanismLigament2d("ElevatorArm", 0.0, 90)); // Start at 0.1m height - SmartDashboard.putData("Elevator Visualization", m_mechVisual); - if (RobotBase.isSimulation()) { - // in simulation, we want to emulate the effect produced by - // using an encoder offset (i.e. we start at 0). - m_elevatorRight.setPosition(0.0); - } - } - - // private final MotionMagicVoltage control = new MotionMagicVoltage(0); - private final DynamicMotionMagicVoltage control = new DynamicMotionMagicVoltage(0,0,0,0); - - public void setPosition(double goal) { - m_taken = true; - if (RobotObserver.getNoElevatorZone() && (m_position > ElevatorConstants.unsafeRange || goal > ElevatorConstants.unsafeRange)) { - return; - } - // floor values for the goal between our two extrema for their positions - goal = Math.min(goal, ElevatorConstants.forwardSoftLimit); - goal = Math.max(goal, ElevatorConstants.reverseSoftLimit); - if (goal >= getPosition() || true) { - m_elevatorRight.setControl(control - .withPosition(goal) - .withVelocity(ElevatorConstants.maxSpeedUp) - .withAcceleration(ElevatorConstants.maxAccelerationUp) - .withJerk(ElevatorConstants.maxJerkUp) - .withSlot(0)); - } else { - m_elevatorRight.setControl(control - .withPosition(goal) - .withVelocity(ElevatorConstants.maxSpeedDown) - .withAcceleration(ElevatorConstants.maxAccelerationDown) - .withJerk(ElevatorConstants.maxJerkDown) - .withSlot(1)); - } - - m_reference = goal; - } - - public void setSpeed(double speed) { - m_speedChanged = (speed != m_speed); - m_speed = speed; - } - - public void setGroundIntake() { - setPosition(ElevatorConstants.groundIntake); - } - - public void setHighGroundIntake() { - setPosition(ElevatorConstants.highGroundIntake); - } - - public void setStow() { - setPosition(ElevatorConstants.stow); - } - - public void setEject() { - setPosition(ElevatorConstants.eject); - } - - public void setProcessor() { - setPosition(ElevatorConstants.processor); - } - - public void setZero() { - setPosition(0); - } - - public void setL1() { - setPosition(ElevatorConstants.L1); - } - - public void setSecondaryL1() { - setPosition(ElevatorConstants.secondaryL1); - } - - public void setL2() { - setPosition(ElevatorConstants.L2); - } - - public void setL3() { - setPosition(ElevatorConstants.L3); - } - - public void setL4() { - setPosition(ElevatorConstants.L4); - } - - public void setPrep() { - // no compensation - setPosition(ElevatorConstants.prep); - } - - public void setReefLower() { - setPosition(ElevatorConstants.reefLower); - } - - public void setReefUpper() { - setPosition(ElevatorConstants.reefUpper); - } - - public void setNet() { - setPosition(ElevatorConstants.net); - } - - public void stop() { - setPosition(m_position); - } - - public void setLevel(int level) { - switch (level) { - case 1 -> setL1(); - case 2 -> setL2(); - case 3 -> setL3(); - case 4 -> setL4(); - case 5 -> setNet(); - case 0 -> setStow(); - default -> setStow(); - } - } - - public boolean atSetpoint() { - if (Robot.isSimulation()) return true; - boolean at = Math.abs(m_reference - m_position) < ElevatorConstants.tolerance; - m_logger.debug("Setpoint: {}", at); - return at; - } - - public double getReference() { - return m_reference; - } - - public double getPosition() { - return m_position; - } - - public double getVelocity() { - return m_velocity; - } - - private double getPositionUncached() { - if (RobotBase.isReal()) { - return m_elevatorRight.getPosition().getValueAsDouble(); - } else { - return m_elevatorSim.getPositionMeters(); - } - } - - private double getVelocityUncached() { - if (RobotBase.isReal()) { - return m_elevatorRight.getVelocity().getValueAsDouble(); - } else { - return m_elevatorSim.getVelocityMetersPerSecond(); - } - } - - public void prepZero() { - m_elevatorRight.getConfigurator().apply(new SoftwareLimitSwitchConfigs()); - m_elevatorRight.setControl(new DutyCycleOut(ElevatorConstants.manualDownSpeed) - .withLimitReverseMotion(false) - .withIgnoreHardwareLimits(true)); - } - - public void zeroElevator() { - m_elevatorRight.setPosition(0.0, 0.2); - } - - public void enableLimits() { - m_elevatorRight.getConfigurator().apply(ElevatorConstants.motorConfig.SoftwareLimitSwitch); - } - - public boolean atZero() { - return m_debouncer.calculate(m_CANrange.getIsDetected().getValue()); - // return m_elevatorRight.getSupplyCurrent().getValueAsDouble() >= ElevatorConstants.k_zeroCurrentThreshold; - } - - public void goDownNoStopping() { - m_elevatorRight.setPosition(1); - m_elevatorRight.set(ElevatorConstants.manualDownSpeed); - } - - @Override - public void periodic() { - m_position = getPositionUncached(); - m_velocity = getVelocityUncached(); - - if (m_speedChanged) { - m_elevatorRight.setControl(new DutyCycleOut(m_speed)); - m_speedChanged = false; - } - - SmartDashboard.putBoolean("ELEVATOR AT POSITION", atSetpoint()); - } - - @Override - public void simulationPeriodic() { - // Update the simulation with the motor voltage - double appliedVolts = m_elevatorRight.get() * RobotController.getBatteryVoltage() * 2; - - m_elevatorSim.setInput(appliedVolts); - m_elevatorSim.update(SimConstants.k_simPeriodic); - - m_position = getPositionUncached(); - m_velocity = getVelocityUncached(); - - // Update the simulated encoder values - m_elevatorRight.getSimState().setRawRotorPosition(m_position); - m_elevatorRight.getSimState().setRotorVelocity(m_velocity); - - m_elevatorArm.setLength(m_position + 0.1); // Offset to avoid overlapping with root - - // Simulate battery voltage - double volts = BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()); - RoboRioSim.setVInVoltage(volts); - } - - public boolean elevatorUp() { - return getPosition() > ElevatorConstants.unsafeRange || m_reference > ElevatorConstants.unsafeRange; - } - public boolean safe() {return !elevatorUp();} - - public void release() { - m_taken = false; - } - - public boolean taken() { - return m_taken; - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/LedFeedback.java b/ThriftyTest/src/main/java/frc/robot/subsystems/LedFeedback.java index 41d6f4d5..d7c2398c 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/LedFeedback.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/LedFeedback.java @@ -22,339 +22,278 @@ import edu.wpi.first.wpilibj.DriverStation.MatchType; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ButtonBindingConstants; -import frc.robot.Constants.CANrangeConstants; -import frc.robot.Constants.CommandBounds; import frc.robot.Constants.IDConstants; import frc.robot.Constants.LedConstants; +import frc.robot.binding.BindingConstants; import frc.robot.RobotObserver; public class LedFeedback extends SubsystemBase { - @SuppressWarnings("unused") - private final Logger m_logger = LoggerFactory.getLogger(LedFeedback.class); - private static double matchTime = 0; - // private Supplier isInRange; - // private boolean algaeOnBoard = false; - private boolean coralOnBoard = false; - private boolean aligned = false; - private boolean climbed = false; - private boolean algaeOnBoard = false; - private boolean algaeInRange = false; - private boolean noElevatorZoneActive = false; - private boolean inAuton = false; - private boolean inTeleop = false; - private double rangeLeft = 0.0; - private double rangeRight = 0.0; - - private int selectedSlot = 0; - - private static enum LED_MODE { - CORAL_ON_BOARD, CORAL_READY, END_GAME_WARNING, END_GAME_ALERT, DEFAULT, - BADCONTROLLER, IN_RANGE, CLIMBED, ALGAE_ON_BOARD, DEFAULT_ENDGAME, ALGAE_READY, ALGAE_TOO_CLOSE, ALIGNED, CLOSE, - ALIGNED_REEF, ALIGNED_BRANCH, ALIGNED_RIGHT, ALIGNED_LEFT; - }; - - private static enum LED_COLOR { - RED, YELLOW, GREEN, PURPLE, BLUE, WHITE, OFF, BROWN, ORANGE, BLUE_VIOLET, DEEP_PINK, SKYBLUE; - }; - - private static enum LED_PATTERN { - TWINKLE, STROBE, LARSON, FLASH, SOLID, CLEAR, RAINBOW; - }; - - private static enum LED_SECTION { - FUNNEL_LEFT, ELEVATOR_LEFT, FUNNEL_RIGHT, ELEVATOR_RIGHT; - } - - private static LED_MODE mode = null; - - private CANdle ledcontroller = new CANdle(IDConstants.candle1); - private CANdle ledcontroller2 = new CANdle(IDConstants.candle2); - - public LedFeedback() { - CANdleConfiguration config = new CANdleConfiguration(); - config.stripType = LEDStripType.RGB; // set the strip type to RGB - config.brightnessScalar = 0.7; // dim the LEDs to 70% brightness - ledcontroller.configAllSettings(config, 20); - ledcontroller2.configAllSettings(config, 20); - - defaultColors(); - - } - - @Override - public void periodic() { - matchTime = (DriverStation.getMatchType() == MatchType.None) ? Double.POSITIVE_INFINITY : DriverStation.getMatchTime(); - inAuton = DriverStation.isAutonomousEnabled(); - inTeleop = DriverStation.isTeleopEnabled(); - - coralOnBoard = RobotObserver.getCoralPieceHeld(); - algaeOnBoard = RobotObserver.getAlgaePieceHeld(); - aligned = RobotObserver.getAligned(); - algaeInRange = CommandBounds.netBounds.isActive(); - noElevatorZoneActive = RobotObserver.getNoElevatorZone(); - climbed = RobotObserver.getClimbed(); - - if (badController()) { - - if (mode != LED_MODE.BADCONTROLLER) { - mode = LED_MODE.BADCONTROLLER; - setAll(LED_COLOR.RED, LED_PATTERN.STROBE); - - } - } else if (climbed) { - if (mode != LED_MODE.CLIMBED) { - setAll(LED_COLOR.OFF, LED_PATTERN.RAINBOW); - mode = LED_MODE.CLIMBED; - } - } else if (inTeleop || inAuton) { - if (matchTime <= LedConstants.endgameWarning && !inAuton) { - // Check for Final Seconds of Endgame - if (matchTime <= LedConstants.endgameAlert) { - if (mode != LED_MODE.END_GAME_ALERT) { - mode = LED_MODE.END_GAME_ALERT; - setAll(LED_COLOR.YELLOW, LED_PATTERN.STROBE); - } - // In Start of Endgame - } else { - if (mode != LED_MODE.END_GAME_WARNING) { - mode = LED_MODE.END_GAME_WARNING; - setAll(LED_COLOR.YELLOW, LED_PATTERN.SOLID); - - } - } - } - else if (coralOnBoard && alignedReef()) { - if (mode != LED_MODE.ALIGNED_REEF) { - mode = LED_MODE.ALIGNED_REEF; - setAll(LED_COLOR.ORANGE, LED_PATTERN.STROBE); - - } - // Check if Aligned too far right or Left Branch on Reef - // } else if (coralOnBoard && (alignedRight() || alignedLeft())) { - // if (mode != LED_MODE.ALIGNED_BRANCH) { - // mode = LED_MODE.ALIGNED_BRANCH; - // setAll(LED_COLOR.GREEN, LED_PATTERN.FLASH); - // } - } else if (coralOnBoard && alignedRight()) { - if (mode != LED_MODE.ALIGNED_RIGHT) { - mode = LED_MODE.ALIGNED_RIGHT; - setRight(LED_COLOR.DEEP_PINK, LED_PATTERN.FLASH); - - } - } else if (coralOnBoard && alignedLeft()) { - if (mode != LED_MODE.ALIGNED_LEFT) { - mode = LED_MODE.ALIGNED_LEFT; - setLeft(LED_COLOR.DEEP_PINK, LED_PATTERN.FLASH); - - } - } - // Check if Coral is On Board and In range of reef - else if (coralOnBoard && aligned) { - if (mode != LED_MODE.CORAL_READY) { - mode = LED_MODE.CORAL_READY; - setAll(LED_COLOR.BLUE, LED_PATTERN.SOLID); - - } - // Check if Coral is On Board - } else if (coralOnBoard) { - if (mode != LED_MODE.CORAL_ON_BOARD) { - mode = LED_MODE.CORAL_ON_BOARD; - setAll(LED_COLOR.WHITE, LED_PATTERN.SOLID); - - } - // Check if Algae is On Board and Too Close to Net - } else if (noElevatorZoneActive) { - if (mode != LED_MODE.ALGAE_TOO_CLOSE) { - mode = LED_MODE.ALGAE_TOO_CLOSE; - setAll(LED_COLOR.SKYBLUE, LED_PATTERN.STROBE); - - } - // Check if Algae is On Board and In range to Net - } else if (algaeOnBoard && algaeInRange) { - if (mode != LED_MODE.ALGAE_READY) { - mode = LED_MODE.ALGAE_READY; - setAll(LED_COLOR.BLUE, LED_PATTERN.SOLID); - } - // Check if Algae is on board - } else if (algaeOnBoard) { - if (mode != LED_MODE.ALGAE_ON_BOARD) { - mode = LED_MODE.ALGAE_ON_BOARD; - setAll(LED_COLOR.GREEN, LED_PATTERN.SOLID); - - } - // If Everything is false make it Default Colors - } else { - if (mode != LED_MODE.DEFAULT) { - defaultColors(); - mode = LED_MODE.DEFAULT; - } - } - - // Run this when robot is disabled + @SuppressWarnings("unused") + private final Logger m_logger = LoggerFactory.getLogger(LedFeedback.class); + private static double matchTime = 0; + + private boolean coralOnBoard = false; + private boolean aligned = false; + private boolean climbed = false; + private boolean algaeOnBoard = false; + private boolean noElevatorZoneActive = false; + private boolean inAuton = false; + private boolean inTeleop = false; + + private int selectedSlot = 0; + + private static enum LED_MODE { + CORAL_ON_BOARD, CORAL_READY, END_GAME_WARNING, END_GAME_ALERT, DEFAULT, BADCONTROLLER, IN_RANGE, CLIMBED, ALGAE_ON_BOARD, DEFAULT_ENDGAME, ALGAE_READY, ALGAE_TOO_CLOSE, ALIGNED, CLOSE, ALIGNED_REEF, ALIGNED_BRANCH, ALIGNED_RIGHT, ALIGNED_LEFT; + }; + + private static enum LED_COLOR { + RED, YELLOW, GREEN, PURPLE, BLUE, WHITE, OFF, BROWN, ORANGE, BLUE_VIOLET, DEEP_PINK, SKYBLUE; + }; + + private static enum LED_PATTERN { + TWINKLE, STROBE, LARSON, FLASH, SOLID, CLEAR, RAINBOW; + }; + + private static enum LED_SECTION { + FUNNEL_LEFT, ELEVATOR_LEFT, FUNNEL_RIGHT, ELEVATOR_RIGHT; + } + + private static LED_MODE mode = null; + + private CANdle ledcontroller = new CANdle(IDConstants.candle1); + private CANdle ledcontroller2 = new CANdle(IDConstants.candle2); + + public LedFeedback() { + super(); + CANdleConfiguration config = new CANdleConfiguration(); + config.stripType = LEDStripType.RGB; // set the strip type to RGB + config.brightnessScalar = 0.7; // dim the LEDs to 70% brightness + ledcontroller.configAllSettings(config, 20); + ledcontroller2.configAllSettings(config, 20); + + defaultColors(); + } + + @Override + public void periodic() { + matchTime = (DriverStation.getMatchType() == MatchType.None) ? Double.POSITIVE_INFINITY + : DriverStation.getMatchTime(); + inAuton = DriverStation.isAutonomousEnabled(); + inTeleop = DriverStation.isTeleopEnabled(); + + coralOnBoard = RobotObserver.getCoralPieceHeld(); + algaeOnBoard = RobotObserver.getAlgaePieceHeld(); + aligned = RobotObserver.getAligned(); + noElevatorZoneActive = RobotObserver.getNoElevatorZone(); + climbed = RobotObserver.getClimbed(); + + if (badController()) { + + if (mode != LED_MODE.BADCONTROLLER) { + mode = LED_MODE.BADCONTROLLER; + setAll(LED_COLOR.RED, LED_PATTERN.STROBE); + + } + } else if (climbed) { + if (mode != LED_MODE.CLIMBED) { + setAll(LED_COLOR.OFF, LED_PATTERN.RAINBOW); + mode = LED_MODE.CLIMBED; + } + } else if (inTeleop || inAuton) { + if (matchTime <= LedConstants.endgameWarning && !inAuton) { + // Check for Final Seconds of Endgame + if (matchTime <= LedConstants.endgameAlert) { + if (mode != LED_MODE.END_GAME_ALERT) { + mode = LED_MODE.END_GAME_ALERT; + setAll(LED_COLOR.YELLOW, LED_PATTERN.STROBE); + } + // In Start of Endgame } else { - if (mode != LED_MODE.DEFAULT) { - defaultColors(); - mode = LED_MODE.DEFAULT; - return; - } - } - } - - private boolean alignedReef() { - return (Math.abs(rangeRight - - CANrangeConstants.closeAlignedDistanceMeters) < CANrangeConstants.closeAlignedDistanceMeters - * CANrangeConstants.tolerance - && - Math.abs(rangeLeft - - CANrangeConstants.closeAlignedDistanceMeters) < CANrangeConstants.closeAlignedDistanceMeters - * CANrangeConstants.tolerance); - } - - private boolean alignedLeft() { - return (Math.abs(rangeRight - - CANrangeConstants.closeAlignedDistanceMeters) < CANrangeConstants.closeAlignedDistanceMeters - * CANrangeConstants.tolerance) - && - (Math.abs(rangeLeft - - CANrangeConstants.farAlignedDistanceMeters) < CANrangeConstants.farAlignedDistanceMeters - * CANrangeConstants.tolerance); - } - - private boolean alignedRight() { - return (Math.abs( - rangeLeft - CANrangeConstants.closeAlignedDistanceMeters) < CANrangeConstants.closeAlignedDistanceMeters - * CANrangeConstants.tolerance) - && - (Math.abs(rangeRight - - CANrangeConstants.farAlignedDistanceMeters) < CANrangeConstants.farAlignedDistanceMeters - * CANrangeConstants.tolerance); - } - - private void clearAllAnimations() { - ledcontroller.clearAnimation(0); - ledcontroller.clearAnimation(1); - ledcontroller2.clearAnimation(0); - ledcontroller2.clearAnimation(1); - } - - private void defaultColors() { - clearAllAnimations(); - setElevator(LED_COLOR.PURPLE, LED_PATTERN.FLASH); // changed to heartbeat mode - setFunnel(LED_COLOR.PURPLE, LED_PATTERN.LARSON); - } + if (mode != LED_MODE.END_GAME_WARNING) { + mode = LED_MODE.END_GAME_WARNING; + setAll(LED_COLOR.YELLOW, LED_PATTERN.SOLID); - private boolean badController() { - boolean driverConnected = DriverStation.isJoystickConnected(ButtonBindingConstants.driverPort); - boolean operatorConnected = DriverStation.isJoystickConnected(ButtonBindingConstants.buttonBoardPort); - - if (!driverConnected || !operatorConnected) - return true; - - String driverName = DriverStation.getJoystickName(ButtonBindingConstants.driverPort).toLowerCase(); - String operatorName = DriverStation.getJoystickName(ButtonBindingConstants.buttonBoardPort).toLowerCase(); - - boolean driverOk = driverName.contains(ButtonBindingConstants.dragonReinsName) || driverName.contains(ButtonBindingConstants.driverBackupName); + } + } + } else if (coralOnBoard && aligned) { + if (mode != LED_MODE.CORAL_READY) { + mode = LED_MODE.CORAL_READY; + setAll(LED_COLOR.BLUE, LED_PATTERN.SOLID); - boolean operatorOk = operatorName.contains(ButtonBindingConstants.ps5Name); + } + // Check if Coral is On Board + } else if (coralOnBoard) { + if (mode != LED_MODE.CORAL_ON_BOARD) { + mode = LED_MODE.CORAL_ON_BOARD; + setAll(LED_COLOR.WHITE, LED_PATTERN.SOLID); - return !(driverOk && operatorOk); - } + } + // Check if Algae is On Board and Too Close to Net + } else if (noElevatorZoneActive) { + if (mode != LED_MODE.ALGAE_TOO_CLOSE) { + mode = LED_MODE.ALGAE_TOO_CLOSE; + setAll(LED_COLOR.SKYBLUE, LED_PATTERN.STROBE); - public void setAll(LED_COLOR color, LED_PATTERN pattern) { - clearAllAnimations(); - setColor(color, LED_SECTION.ELEVATOR_LEFT, pattern); // changed to heartbeat mode - setColor(color, LED_SECTION.ELEVATOR_RIGHT, pattern); // changed to heartbeat mode - setColor(color, LED_SECTION.FUNNEL_LEFT, pattern); - setColor(color, LED_SECTION.FUNNEL_RIGHT, pattern); - } + } + // Check if Algae is On Board and In range to Net + } else if (algaeOnBoard) { + if (mode != LED_MODE.ALGAE_READY) { + mode = LED_MODE.ALGAE_READY; + setAll(LED_COLOR.BLUE, LED_PATTERN.SOLID); + } + // Check if Algae is on board + } else if (algaeOnBoard) { + if (mode != LED_MODE.ALGAE_ON_BOARD) { + mode = LED_MODE.ALGAE_ON_BOARD; + setAll(LED_COLOR.GREEN, LED_PATTERN.SOLID); - public void setElevator(LED_COLOR color, LED_PATTERN pattern) { - setColor(color, LED_SECTION.ELEVATOR_LEFT, pattern); - setColor(color, LED_SECTION.ELEVATOR_RIGHT, pattern); - } + } + // If Everything is false make it Default Colors + } else { + if (mode != LED_MODE.DEFAULT) { + defaultColors(); + mode = LED_MODE.DEFAULT; + } + } - public void setFunnel(LED_COLOR color, LED_PATTERN pattern) { - setColor(color, LED_SECTION.FUNNEL_LEFT, pattern); - setColor(color, LED_SECTION.FUNNEL_RIGHT, pattern); + // Run this when robot is disabled + } else { + if (mode != LED_MODE.DEFAULT) { + defaultColors(); + mode = LED_MODE.DEFAULT; + return; + } } - - public void setRight(LED_COLOR color, LED_PATTERN pattern) { - setColor(color, LED_SECTION.ELEVATOR_RIGHT, pattern); - setColor(color, LED_SECTION.FUNNEL_RIGHT, pattern); + } + + private void clearAllAnimations() { + ledcontroller.clearAnimation(0); + ledcontroller.clearAnimation(1); + ledcontroller2.clearAnimation(0); + ledcontroller2.clearAnimation(1); + } + + private void defaultColors() { + clearAllAnimations(); + setElevator(LED_COLOR.PURPLE, LED_PATTERN.FLASH); // changed to heartbeat mode + setFunnel(LED_COLOR.PURPLE, LED_PATTERN.LARSON); + } + + private boolean badController() { + boolean driverConnected = DriverStation.isJoystickConnected(BindingConstants.kDriverPort); + boolean operatorConnected = DriverStation.isJoystickConnected(BindingConstants.kOperatorPort); + + if (!driverConnected || !operatorConnected) + return true; + + String driverName = DriverStation.getJoystickName(BindingConstants.kDriverPort).toLowerCase(); + String operatorName = DriverStation.getJoystickName(BindingConstants.kOperatorPort).toLowerCase(); + + boolean driverOk = driverName.contains(LedConstants.dragonReinsName); + + boolean operatorOk = operatorName.contains(LedConstants.ps5Name); + + return !(driverOk && operatorOk); + } + + public void setAll(LED_COLOR color, LED_PATTERN pattern) { + clearAllAnimations(); + setColor(color, LED_SECTION.ELEVATOR_LEFT, pattern); // changed to heartbeat mode + setColor(color, LED_SECTION.ELEVATOR_RIGHT, pattern); // changed to heartbeat mode + setColor(color, LED_SECTION.FUNNEL_LEFT, pattern); + setColor(color, LED_SECTION.FUNNEL_RIGHT, pattern); + } + + public void setElevator(LED_COLOR color, LED_PATTERN pattern) { + setColor(color, LED_SECTION.ELEVATOR_LEFT, pattern); + setColor(color, LED_SECTION.ELEVATOR_RIGHT, pattern); + } + + public void setFunnel(LED_COLOR color, LED_PATTERN pattern) { + setColor(color, LED_SECTION.FUNNEL_LEFT, pattern); + setColor(color, LED_SECTION.FUNNEL_RIGHT, pattern); + } + + public void setRight(LED_COLOR color, LED_PATTERN pattern) { + setColor(color, LED_SECTION.ELEVATOR_RIGHT, pattern); + setColor(color, LED_SECTION.FUNNEL_RIGHT, pattern); + } + + public void setLeft(LED_COLOR color, LED_PATTERN pattern) { + setColor(color, LED_SECTION.ELEVATOR_LEFT, pattern); + setColor(color, LED_SECTION.FUNNEL_LEFT, pattern); + } + + @SuppressWarnings("incomplete-switch") + public void setColor(LED_COLOR color, LED_SECTION section, LED_PATTERN pattern) { + int nbrLED = 0; + int offsetLED = 0; + + CANdle candle = null; + Color c = new Color(); + + switch (color) { + case BLUE -> c = Color.kBlue; + case GREEN -> c = Color.kGreen; + case RED -> c = Color.kRed; + case YELLOW -> c = Color.kYellow; + case PURPLE -> c = Color.kPurple; + case WHITE -> c = Color.kWhite; + case SKYBLUE -> c = Color.kRoyalBlue; + case ORANGE -> c = Color.kOrange; + case DEEP_PINK -> c = Color.kDeepPink; + case OFF -> c = Color.kBlack; } - public void setLeft(LED_COLOR color, LED_PATTERN pattern) { - setColor(color, LED_SECTION.ELEVATOR_LEFT, pattern); - setColor(color, LED_SECTION.FUNNEL_LEFT, pattern); + int r = (int) (c.red * 255); + int b = (int) (c.blue * 255); + int g = (int) (c.green * 255); + + switch (section) { + case FUNNEL_LEFT: + offsetLED = LedConstants.funnelOffset; + nbrLED = LedConstants.funnelNumLED; + selectedSlot = 0; + candle = ledcontroller; + break; + case ELEVATOR_LEFT: + offsetLED = LedConstants.elevatorOffset; + nbrLED = LedConstants.elevatorNumLED; + selectedSlot = 1; + candle = ledcontroller; + break; + case FUNNEL_RIGHT: + offsetLED = LedConstants.funnelOffset2; + nbrLED = LedConstants.funnelNumLED2; + selectedSlot = 0; + candle = ledcontroller2; + break; + case ELEVATOR_RIGHT: + offsetLED = LedConstants.elevatorOffset2; + nbrLED = LedConstants.elevatorNumLED2; + selectedSlot = 1; + candle = ledcontroller2; } - @SuppressWarnings("incomplete-switch") - public void setColor(LED_COLOR color, LED_SECTION section, LED_PATTERN pattern) { - int nbrLED = 0; - int offsetLED = 0; - - CANdle candle = null; - Color c = new Color(); - - switch (color) { - case BLUE -> c = Color.kBlue; - case GREEN -> c = Color.kGreen; - case RED -> c = Color.kRed; - case YELLOW -> c = Color.kYellow; - case PURPLE -> c = Color.kPurple; - case WHITE -> c = Color.kWhite; - case SKYBLUE -> c = Color.kRoyalBlue; - case ORANGE -> c = Color.kOrange; - case DEEP_PINK -> c = Color.kDeepPink; - case OFF -> c = Color.kBlack; - - } - - int r = (int) (c.red * 255); - int b = (int) (c.blue * 255); - int g = (int) (c.green * 255); - - switch (section) { - case FUNNEL_LEFT: - offsetLED = LedConstants.funnelOffset; - nbrLED = LedConstants.funnelNumLED; - selectedSlot = 0; - candle = ledcontroller; - break; - case ELEVATOR_LEFT: - offsetLED = LedConstants.elevatorOffset; - nbrLED = LedConstants.elevatorNumLED; - selectedSlot = 1; - candle = ledcontroller; - break; - case FUNNEL_RIGHT: - offsetLED = LedConstants.funnelOffset2; - nbrLED = LedConstants.funnelNumLED2; - selectedSlot = 0; - candle = ledcontroller2; - break; - case ELEVATOR_RIGHT: - offsetLED = LedConstants.elevatorOffset2; - nbrLED = LedConstants.elevatorNumLED2; - selectedSlot = 1; - candle = ledcontroller2; - } - - candle.clearAnimation(selectedSlot); - - switch (pattern) { - case SOLID -> candle.setLEDs(r, g, b, 0, offsetLED, nbrLED); - case FLASH -> candle.animate( - new SingleFadeAnimation(r, g, b, 0, LedConstants.flashSpeed, nbrLED, offsetLED), selectedSlot); - case STROBE -> candle.animate(new StrobeAnimation(r, g, b, 0, LedConstants.strobeSpeed, nbrLED, offsetLED), - selectedSlot); - case TWINKLE -> candle.animate( - new TwinkleAnimation(r, g, b, 0, 0.5, nbrLED, TwinklePercent.Percent42, offsetLED), selectedSlot); - case CLEAR -> candle.setLEDs(0, 0, 0, 0, offsetLED, nbrLED); - case LARSON -> candle.animate(new LarsonAnimation(r, g, b, 0, 0.5, nbrLED, BounceMode.Back, 7, offsetLED), - selectedSlot); - case RAINBOW -> candle.animate(new RainbowAnimation(1, 0.9, nbrLED, true, offsetLED), selectedSlot); - } + candle.clearAnimation(selectedSlot); + + switch (pattern) { + case SOLID -> candle.setLEDs(r, g, b, 0, offsetLED, nbrLED); + case FLASH -> candle.animate( + new SingleFadeAnimation(r, g, b, 0, LedConstants.flashSpeed, nbrLED, offsetLED), + selectedSlot); + case STROBE -> candle.animate( + new StrobeAnimation(r, g, b, 0, LedConstants.strobeSpeed, nbrLED, offsetLED), + selectedSlot); + case TWINKLE -> candle.animate( + new TwinkleAnimation(r, g, b, 0, 0.5, nbrLED, TwinklePercent.Percent42, offsetLED), + selectedSlot); + case CLEAR -> candle.setLEDs(0, 0, 0, 0, offsetLED, nbrLED); + case LARSON -> candle.animate( + new LarsonAnimation(r, g, b, 0, 0.5, nbrLED, BounceMode.Back, 7, offsetLED), + selectedSlot); + case RAINBOW -> candle.animate(new RainbowAnimation(1, 0.9, nbrLED, true, offsetLED), + selectedSlot); } + } } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/PassiveSubsystem.java b/ThriftyTest/src/main/java/frc/robot/subsystems/PassiveSubsystem.java new file mode 100644 index 00000000..ac6d8bd8 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/PassiveSubsystem.java @@ -0,0 +1,79 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * Represents a subsystem with the ability to enable and disable its passive state. + * + * Because some subsystems have default, or "passive", behavior that should only run upon a certain + * condition. The PassiveSubsystem deals with this functionality by storing a "taken" flag that is + * set and unset by commands, methods, etc. + * + * Instead of using a default command to add passive behavior, this makes more sense because this + * ensures that the + */ +public abstract class PassiveSubsystem extends SubsystemBase { + private boolean m_taken; + + protected PassiveSubsystem() { + m_taken = false; + enablePassiveBehavior(); + } + + /** + * Returns whether the subsystem is taken, or available for passive action + */ + public boolean taken() { + return m_taken; + } + + /** + * Sets the subsystem's taken state to true so that passive behavior does not run. + */ + public void take() { + m_taken = true; + } + + /** + * Sets the subsystem's taken state to false so that passive behavior may run. + */ + public void release() { + m_taken = false; + } + + /** + * Calls release() if the condition is met. This is useful as a helper method to only + * release a subsystem if a command was interrupted. + */ + public void conditionalRelease(boolean shouldRelease) { + if (shouldRelease) { + release(); + } + } + + /** + * The passive method that should be called every sceduler iteration if and only if the subsystem + * is not taken. + */ + protected abstract void passive(); + + private Command passiveBehavior() { + return Commands.run(() -> { + if (!taken()) { + passive(); + release(); + } + }, this).withName(getName() + "Passive"); + } + + /** + * Enables passive behavior for the PassiveSubsystem. Without calling this method, passive + * behavior will never be triggered. A good place for this is most likely the constructor for the + * subsystem. + */ + protected void enablePassiveBehavior() { + setDefaultCommand(passiveBehavior()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/Pivot.java b/ThriftyTest/src/main/java/frc/robot/subsystems/Pivot.java deleted file mode 100644 index 68493073..00000000 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/Pivot.java +++ /dev/null @@ -1,175 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.IDConstants; -import frc.robot.Constants.PivotConstants; -import frc.robot.Constants.SimConstants; -import frc.robot.Robot; -import frc.robot.RobotObserver; - -public class Pivot extends SubsystemBase { - private final TalonFX m_pivot = new TalonFX(IDConstants.pivot); - - private double m_position; - private double m_velocity; - - private double m_reference; - - private SingleJointedArmSim m_armSim; - private final DCMotor m_gearbox = DCMotor.getKrakenX60(1); // 2 motors (left and right) - - private Mechanism2d m_mechVisual; - private MechanismRoot2d m_mechRoot; - private MechanismLigament2d m_armLigament; - - private double m_speed; - private boolean m_speedChanged; - - public Pivot() { - configSim(); - configMotor(); - } - - private void configMotor() { - m_pivot.getConfigurator().apply(PivotConstants.motorConfig, 0.2); - m_pivot.setPosition(PivotConstants.rotorOffset); - } - - private void configSim() { - m_armSim = new SingleJointedArmSim( - PivotConstants.stateSpacePlant, - m_gearbox, - PivotConstants.gearRatio, - PivotConstants.armLength, - PivotConstants.radiansAtZero, - PivotConstants.radiansAtMax, - true, // Add noise for realism - PivotConstants.stow // Starting angle - ); - - m_mechVisual = new Mechanism2d(1.0, 1.0); // Width/height in meters - m_mechRoot = m_mechVisual.getRoot("ArmRoot", 0.5, 0.0); // Center at (0.5, 0) - m_armLigament = m_mechRoot - .append(new MechanismLigament2d("Arm", PivotConstants.armLength, Math.toDegrees(m_position))); - SmartDashboard.putData("Pivot Arm Visualization", m_mechVisual); - } - - MotionMagicVoltage control = new MotionMagicVoltage(0); - - public void setPosition(double goal) { - if (RobotObserver.getAlgaePieceHeld()) { - m_pivot.setControl(control.withPosition(goal).withSlot(1)); - } else { - m_pivot.setControl(control.withPosition(goal).withSlot(0)); - } - m_reference = goal; - } - - public void setSpeed(double speed) { - m_speedChanged = (speed != m_speed); - m_speed = speed; - } - - public void setStow() { - setPosition(PivotConstants.stow); - } - - public void setProcessor() { - setPosition(PivotConstants.processor); - } - - public void setNet() { - setPosition(PivotConstants.net); - } - - public void setGroundPickup() { - setPosition(PivotConstants.groundPickup); - } - - public void setReefPickup() { - setPosition(PivotConstants.reefPickup); - } - - public void setReefExtract() { - setPosition(PivotConstants.reefExtract); - } - - public void stop() { - setPosition(m_position); - } - - public double getPosition() { - return m_position; - } - - public double getReference() { - return m_reference; - } - - public double getVelocity() { - return m_velocity; - } - - public boolean atSetpoint() { - return Math.abs(getReference() - getPosition()) < PivotConstants.tolerance; - } - - private double getPositionUncached() { - if (Robot.isReal()) { - return m_pivot.getPosition().getValueAsDouble(); - } else { - return m_armSim.getAngleRads(); - } - } - - private double getVelocityUncached() { - if (Robot.isReal()) { - return m_pivot.getVelocity().getValueAsDouble(); - } else { - return m_armSim.getVelocityRadPerSec(); - } - } - - @Override - public void periodic() { - m_armLigament.setAngle(Math.toDegrees(m_position)); - - m_position = getPositionUncached(); - m_velocity = getVelocityUncached(); - - if (m_speedChanged) { - m_pivot.setControl(new DutyCycleOut(m_speed)); - m_speedChanged = false; - } - - SmartDashboard.putBoolean("PIVOT AT POSITION", atSetpoint()); - } - - @Override - public void simulationPeriodic() { - // Update the simulation with the motor voltage - double appliedVolts = m_pivot.get() * RobotController.getBatteryVoltage(); - m_armSim.setInput(appliedVolts); - m_armSim.update(SimConstants.k_simPeriodic); - - // Simulate battery voltage - RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps())); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java new file mode 100644 index 00000000..fdc9a1bc --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java @@ -0,0 +1,120 @@ +package frc.robot.subsystems.algae; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import edu.wpi.first.math.filter.MedianFilter; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Robot; +import frc.robot.RobotObserver; +import frc.robot.subsystems.PassiveSubsystem; +import frc.robot.subsystems.algae.AlgaeIO.AlgaeIOInputs; +import frc.robot.utils.LoopTimer; + +public class Algae extends PassiveSubsystem { + @SuppressWarnings("unused") + private final Logger m_logger = LoggerFactory.getLogger(Algae.class); + + private final LoopTimer m_timer; + + private final AlgaeIO m_io; + private AlgaeIOInputs m_inputs; + private AlgaeIOInputsLogger m_inputsLogger; + + private boolean m_hasAlgae; + + private MedianFilter m_filter = new MedianFilter(10); + + public Algae() { + super(); + if (Robot.isReal()) { + m_io = new AlgaeIOHardware(); + } else { + m_io = new AlgaeIOSim(); + } + m_inputs = new AlgaeIOInputs(); + m_inputsLogger = new AlgaeIOInputsLogger(m_inputs); + RobotObserver.setAlgaePieceHeldSupplier(this.holdingAlgae()); + m_timer = new LoopTimer("Algae"); + } + + private void setVoltage(double voltage) { + take(); + m_io.setVoltage(voltage); + } + + public Trigger holdingAlgae() { + return new Trigger(() -> m_hasAlgae); + } + + private double getTorqueCurrent() { + return m_filter.calculate(m_inputs.torque); + } + + private void stop() { + setVoltage(0); + } + + /** + * If shouldHold is true, then try to hold an algae. Otherwise, stop the motors. + */ + private void keep(boolean shouldHold) { + if (shouldHold) { + setVoltage(AlgaeConstants.kHoldVoltage); + } else { + stop(); + } + } + + + @Override + public void periodic() { + m_timer.reset(); + m_io.updateInputs(m_inputs); + m_inputsLogger.log(); + m_hasAlgae = getTorqueCurrent() >= AlgaeConstants.kTorqueCurrentThreshold; + m_timer.log(); + } + + protected void passive() { + keep(holdingAlgae().getAsBoolean()); + } + + /** + * Intakes an algae, then holds it. If an algae is already held, the command does not run. + */ + public Command intake() { + return Commands.sequence( + runOnce(() -> setVoltage(AlgaeConstants.kIntakeVoltage)), + Commands.waitUntil(holdingAlgae())) + + .finallyDo(() -> keep(holdingAlgae().getAsBoolean())) + .unless(holdingAlgae()); + } + + /** + * Ejects an algae with the correct conditions for a net score + */ + public Command net() { + return Commands.sequence( + runOnce(() -> setVoltage(AlgaeConstants.kNetEjectVoltage)), + Commands.waitSeconds(AlgaeConstants.kNetScoreTime)) + + .finallyDo(this::keep) + .onlyIf(holdingAlgae()); + } + + /** + * Ejects an algae with the correct conditions for a processor score + */ + public Command processorScore() { + return Commands.sequence( + runOnce(() -> setVoltage(AlgaeConstants.kProcessorEjectVoltage)), + Commands.waitSeconds(AlgaeConstants.kProcessorScoreTime)) + + .finallyDo(this::keep) + .onlyIf(holdingAlgae()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java new file mode 100644 index 00000000..9686e01a --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java @@ -0,0 +1,32 @@ +package frc.robot.subsystems.algae; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public final class AlgaeConstants { + protected static final int kMotorID = 60; + + protected static final double kIntakeVoltage = 12; + protected static final double kNetEjectVoltage = -3.0; + protected static final double kProcessorEjectVoltage = -3.2; + protected static final double kHoldVoltage = 2.7; + + protected static final double kTorqueCurrentThreshold = 75; + protected static final double kSupplyCurrentLimit = 25.0; + + protected static final double kProcessorScoreTime = 2.0; + protected static final double kNetScoreTime = 0.4; + + protected static final TalonFXConfiguration kMotorConfig = new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.Clockwise_Positive)) + + .withCurrentLimits(new CurrentLimitsConfigs() + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(kSupplyCurrentLimit)); +} + diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java new file mode 100644 index 00000000..7dcac00b --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.algae; + +public interface AlgaeIO { + void updateInputs(AlgaeIOInputs inputs); + + class AlgaeIOInputs { + public boolean motorConnected = true; + public double current = 0.0; + public double torque = 0.0; + public double voltage = 0.0; + public double temperature = 0.0; + } + + void setVoltage(double voltage); +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOHardware.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOHardware.java new file mode 100644 index 00000000..913d602c --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOHardware.java @@ -0,0 +1,61 @@ +package frc.robot.subsystems.algae; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.utils.StatusSignalUtil; + +public class AlgaeIOHardware implements AlgaeIO { + private final TalonFX m_motor; + + private double m_voltage; + + private final StatusSignal m_voltageSignal; + private final StatusSignal m_currentSignal; + private final StatusSignal m_torqueSignal; + private final StatusSignal m_tempSignal; + private final StatusSignal m_velocitySignal; + + public AlgaeIOHardware() { + m_motor = new TalonFX(AlgaeConstants.kMotorID); + m_motor.clearStickyFaults(); + m_motor.getConfigurator().apply(AlgaeConstants.kMotorConfig); + + m_voltageSignal = m_motor.getMotorVoltage(); + m_currentSignal = m_motor.getSupplyCurrent(); + m_torqueSignal = m_motor.getTorqueCurrent(); + m_tempSignal = m_motor.getDeviceTemp(); + m_velocitySignal = m_motor.getVelocity(); + + StatusSignalUtil.registerRioSignals( + m_voltageSignal, + m_currentSignal, + m_torqueSignal, + m_tempSignal, + m_velocitySignal); + } + + public void updateInputs(AlgaeIOInputs inputs) { + inputs.motorConnected = BaseStatusSignal.isAllGood( + m_voltageSignal, + m_currentSignal, + m_torqueSignal, + m_tempSignal, + m_velocitySignal); + inputs.voltage = m_voltageSignal.getValueAsDouble(); + inputs.current = m_currentSignal.getValueAsDouble(); + inputs.torque = m_torqueSignal.getValueAsDouble(); + inputs.temperature = m_tempSignal.getValueAsDouble(); + } + + public void setVoltage(double voltage) { + if (voltage != m_voltage) { + m_voltage = voltage; + m_motor.setVoltage(voltage); + } + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java new file mode 100644 index 00000000..ca9c77ba --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.algae; + +import frc.robot.subsystems.algae.AlgaeIO.AlgaeIOInputs; +import frc.robot.utils.OnboardLogger; + +public class AlgaeIOInputsLogger { + private final OnboardLogger log; + + public AlgaeIOInputsLogger(AlgaeIOInputs inputs) { + log = new OnboardLogger("Algae"); + log.registerBoolean("Motor Connected", () -> inputs.motorConnected); + log.registerDouble("Current", () -> inputs.current); + log.registerDouble("Torque", () -> inputs.torque); + log.registerDouble("Voltage", () -> inputs.voltage); + } + + public void log() { + log.log(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOSim.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOSim.java new file mode 100644 index 00000000..4db643a5 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOSim.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.algae; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; + +public class AlgaeIOSim implements AlgaeIO { + private final DCMotorSim m_motorSim; + + private double m_voltage; + + public AlgaeIOSim() { + m_motorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 1, 1), + DCMotor.getKrakenX60(1), 0.1, 0.1); + } + + public void updateInputs(AlgaeIOInputs inputs) { + m_motorSim.update(Robot.kDefaultPeriod); + inputs.voltage = m_voltage; + inputs.current = m_motorSim.getCurrentDrawAmps(); + inputs.torque = SmartDashboard.getNumber("Algae/Torque", 0.0); + SmartDashboard.putNumber("Algae/Torque", inputs.torque); + } + + public void setVoltage(double voltage) { + m_voltage = voltage; + m_motorSim.setInputVoltage(voltage); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/Climber.java b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/Climber.java new file mode 100644 index 00000000..ad4fa0f6 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -0,0 +1,135 @@ +package frc.robot.subsystems.climber; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Robot; +import frc.robot.subsystems.PassiveSubsystem; +import frc.robot.subsystems.climber.ClimberIO.ClimberIOInputs; +import frc.robot.utils.LoopTimer; +import frc.robot.utils.OnboardLogger; + +public class Climber extends PassiveSubsystem { + @SuppressWarnings("unused") + private final Logger m_logger = LoggerFactory.getLogger(Climber.class); + private final OnboardLogger m_ologger; + + private final LoopTimer m_timer; + + private final ClimberIO m_io; + private final ClimberIOInputs m_inputs; + private final ClimberIOInputsLogger m_inputsLogger; + + public Climber() { + super(); + if (Robot.isReal()) { + m_io = new ClimberIOHardware(); + } else { + m_io = new ClimberIOSim(); + } + m_inputs = new ClimberIOInputs(); + m_inputsLogger = new ClimberIOInputsLogger(m_inputs); + m_ologger = new OnboardLogger("Climber"); + m_ologger.registerBoolean("Climbed", climbed()); + m_ologger.registerBoolean("Raised", raised()); + m_ologger.registerBoolean("Lowered", lowered()); + m_timer = new LoopTimer("Climber"); + SmartDashboard.putData("Climber/Stow", lower()); + } + + /* + * Opens the funnel, then resets the servo + */ + public Command openFunnel() { + return Commands.sequence( + runOnce(() -> m_io.setServo(ClimberConstants.kOpenServoPosition)), + Commands.waitSeconds(ClimberConstants.kFunnelOpenTime), + runOnce(() -> m_io.setServo(ClimberConstants.kClosedServoPosition))); + } + + private void setMotor(double voltage) { + take(); + m_io.setVoltage(voltage); + } + + private void setUp() { + setMotor(ClimberConstants.kUpVolts); + } + + private void setDown() { + setMotor(ClimberConstants.kDownVolts); + } + + private void stop() { + setMotor(0); + } + + @Override + public void periodic() { + m_timer.reset(); + m_io.updateInputs(m_inputs); + m_inputsLogger.log(); + m_ologger.log(); + m_timer.log(); + } + + public Trigger climbed() { + return new Trigger(() -> m_inputs.position <= ClimberConstants.kClimbPosition); + } + + public Trigger raised() { + return new Trigger(() -> m_inputs.position > ClimberConstants.kClimbReadyTolerance); + } + + public Trigger lowered() { + return new Trigger(() -> m_inputs.position <= ClimberConstants.kStowPosition); + } + + protected void passive() { + if (DriverStation.isFMSAttached() && DriverStation.isTeleop() + && DriverStation.getMatchTime() < 40) { + if (raised().getAsBoolean()) { + setUp(); + } else { + stop(); + } + } + } + + /** + * Drives the cliber up until it has reached it's raised + */ + public Command raise() { + return Commands.sequence( + runOnce(this::setUp), + Commands.waitUntil(raised())) + + .finallyDo(this::stop); + } + + /** + * Drives the climber down until the climber is stowed + */ + public Command lower() { + return Commands.sequence( + runOnce(this::setDown), + Commands.waitUntil(lowered())) + + .finallyDo(this::stop); + } + + /** + * Drives the climber down until the climb position is reached + */ + public Command climb() { + return Commands.sequence( + runOnce(this::setDown), + Commands.waitUntil(climbed())) + + .finallyDo(this::stop); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java new file mode 100644 index 00000000..6605e4c1 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -0,0 +1,73 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; + +public final class ClimberConstants { + protected static final int kLeftMotorID = 1; + protected static final int kRightMotorID = 2; + protected static final int kServoID = 7; + protected static final int kEncoderID = 9; + + protected static final double kStowPosition = -0.25; + protected static final double kClimbPosition = -0.110; + protected static final double kClimbReadyTolerance = -0.001; + + protected static final double kFunnelOpenTime = 1.5; + + protected static final double kUpVolts = 12.0; + protected static final double kDownVolts = -12.0; + + private static final double kSupplyCurrentLimit = 80.0; + + private static final double kForwardSoftLimit = 0.0; + private static final double kReverseSoftLimit = -0.25; + + protected static final double kOpenServoPosition = 0.0; + protected static final double kClosedServoPosition = 1.0; + + private static final double kEncoderOffset = -0.01318359; + private static final SensorDirectionValue kEncoderDirection = + SensorDirectionValue.CounterClockwise_Positive; + + protected static final CANcoderConfiguration kEncoderConfig = new CANcoderConfiguration() + .withMagnetSensor(new MagnetSensorConfigs() + .withAbsoluteSensorDiscontinuityPoint(0.5) + .withSensorDirection(kEncoderDirection) + .withMagnetOffset(kEncoderOffset)); + + protected static final TalonFXConfiguration kMotorConfig = new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive)) + + .withCurrentLimits(new CurrentLimitsConfigs() + .withSupplyCurrentLimit(kSupplyCurrentLimit).withSupplyCurrentLimitEnable(true)) + + .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitThreshold(kForwardSoftLimit).withForwardSoftLimitEnable(true) + .withReverseSoftLimitThreshold(kReverseSoftLimit).withReverseSoftLimitEnable(true)) + + .withFeedback(new FeedbackConfigs() + .withFeedbackRemoteSensorID(ClimberConstants.kEncoderID) + .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)); + + // Linear system plant for simulation (NOT ACCURATE YET) + protected static final LinearSystem kPlant = + LinearSystemId.createSingleJointedArmSystem(DCMotor.getKrakenX60(1), 50, 40); +}; + diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 00000000..523046db --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.climber; + +public interface ClimberIO { + void updateInputs(ClimberIOInputs inputs); + + public class ClimberIOInputs { + public boolean leftConnected = true; + public boolean rightConnected = true; + public double leftCurrent = 0.0; + public double rightCurrent = 0.0; + public double leftVoltage = 0.0; + public double rightVoltage = 0.0; + public double leftTemp = 0.0; + public double rightTemp = 0.0; + public double leftVelocityRPS = 0.0; + public double rightVelocityRPS = 0.0; + public boolean encoderConnected = true; + public double position = 0.0; + } + + void setVoltage(double voltage); + void setServo(double position); +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIOHardware.java b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIOHardware.java new file mode 100644 index 00000000..7d43268e --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIOHardware.java @@ -0,0 +1,107 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Servo; +import frc.robot.utils.StatusSignalUtil; + +public class ClimberIOHardware implements ClimberIO { + private final TalonFX m_leftMotor; + private final TalonFX m_rightMotor; + + private final CANcoder m_CANcoder; + + private final Servo m_servo; + + private double m_voltage; + + private final StatusSignal m_leftVoltageSignal; + private final StatusSignal m_rightVoltageSignal; + private final StatusSignal m_leftCurrentSignal; + private final StatusSignal m_rightCurrentSignal; + private final StatusSignal m_leftTempSignal; + private final StatusSignal m_rightTempSignal; + private final StatusSignal m_leftVelocitySignal; + private final StatusSignal m_rightVelocitySignal; + + private final StatusSignal m_positionSignal; + + public ClimberIOHardware() { + m_leftMotor = new TalonFX(ClimberConstants.kLeftMotorID); + m_rightMotor = new TalonFX(ClimberConstants.kRightMotorID); + m_leftMotor.clearStickyFaults(); + m_rightMotor.clearStickyFaults(); + m_leftMotor.getConfigurator().apply(ClimberConstants.kMotorConfig); + m_rightMotor.getConfigurator().apply(ClimberConstants.kMotorConfig); + m_rightMotor.setControl(new Follower(ClimberConstants.kLeftMotorID, true)); + + m_CANcoder = new CANcoder(ClimberConstants.kEncoderID); + m_CANcoder.getConfigurator().apply(ClimberConstants.kEncoderConfig); + + m_servo = new Servo(ClimberConstants.kServoID); + + m_leftVoltageSignal = m_leftMotor.getMotorVoltage(); + m_rightVoltageSignal = m_rightMotor.getMotorVoltage(); + m_leftCurrentSignal = m_leftMotor.getSupplyCurrent(); + m_rightCurrentSignal = m_rightMotor.getSupplyCurrent(); + m_leftTempSignal = m_leftMotor.getDeviceTemp(); + m_rightTempSignal = m_rightMotor.getDeviceTemp(); + m_leftVelocitySignal = m_leftMotor.getVelocity(); + m_rightVelocitySignal = m_rightMotor.getVelocity(); + + m_positionSignal = m_CANcoder.getPosition(); + + StatusSignalUtil.registerRioSignals( + m_leftVoltageSignal, + m_rightVoltageSignal, + m_leftCurrentSignal, + m_rightCurrentSignal, + m_leftTempSignal, + m_rightTempSignal, + m_leftVelocitySignal, + m_rightVelocitySignal, + m_positionSignal); + } + + public void updateInputs(ClimberIOInputs inputs) { + inputs.leftConnected = BaseStatusSignal.isAllGood( + m_leftVoltageSignal, + m_leftCurrentSignal, + m_leftTempSignal, + m_leftVelocitySignal); + inputs.rightConnected = BaseStatusSignal.isAllGood( + m_rightVoltageSignal, + m_rightCurrentSignal, + m_rightTempSignal, + m_rightVelocitySignal); + inputs.leftVoltage = m_leftVoltageSignal.getValueAsDouble(); + inputs.rightVoltage = m_rightVoltageSignal.getValueAsDouble(); + inputs.leftCurrent = m_leftCurrentSignal.getValueAsDouble(); + inputs.rightCurrent = m_rightCurrentSignal.getValueAsDouble(); + inputs.leftTemp = m_leftTempSignal.getValueAsDouble(); + inputs.rightTemp = m_rightTempSignal.getValueAsDouble(); + inputs.leftVelocityRPS = m_leftVelocitySignal.getValueAsDouble(); + inputs.rightVelocityRPS = m_rightVelocitySignal.getValueAsDouble(); + inputs.encoderConnected = BaseStatusSignal.isAllGood(m_positionSignal); + inputs.position = m_positionSignal.getValueAsDouble(); + } + + public void setVoltage(double voltage) { + if (m_voltage != voltage) { + m_voltage = voltage; + m_leftMotor.setVoltage(voltage); + } + } + + public void setServo(double position) { + m_servo.setPosition(position); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIOInputsLogger.java b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIOInputsLogger.java new file mode 100644 index 00000000..c5ac22ef --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIOInputsLogger.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.climber; + +import frc.robot.subsystems.climber.ClimberIO.ClimberIOInputs; +import frc.robot.utils.OnboardLogger; + +public class ClimberIOInputsLogger { + private final OnboardLogger log; + + public ClimberIOInputsLogger(ClimberIOInputs inputs) { + log = new OnboardLogger("Climber"); + log.registerBoolean("Left Motor Connected", () -> inputs.leftConnected); + log.registerBoolean("Right Motor Connected", () -> inputs.rightConnected); + log.registerDouble("Left Current", () -> inputs.leftCurrent); + log.registerDouble("Right Current", () -> inputs.rightCurrent); + log.registerDouble("Left Voltage", () -> inputs.leftVoltage); + log.registerDouble("Right Voltage", () -> inputs.rightVoltage); + log.registerDouble("Left Temperature", () -> inputs.leftTemp); + log.registerDouble("Right Temperature", () -> inputs.rightTemp); + log.registerDouble("Left Velocity RPS", () -> inputs.leftVelocityRPS); + log.registerDouble("Right Velocity RPS", () -> inputs.rightVelocityRPS); + log.registerBoolean("CANcoder Connected", () -> inputs.encoderConnected); + log.registerDouble("Position", () -> inputs.position); + } + + public void log() { + log.log(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java new file mode 100644 index 00000000..dd524dbc --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -0,0 +1,39 @@ +package frc.robot.subsystems.climber; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Robot; + +public class ClimberIOSim implements ClimberIO { + private final DCMotorSim m_lefMotorSim; + private final DCMotorSim m_rightMotorSim; + + private double m_voltage; + + public ClimberIOSim() { + m_lefMotorSim = new DCMotorSim(ClimberConstants.kPlant, DCMotor.getKrakenX60(1), 0.1, 0.1); + m_rightMotorSim = new DCMotorSim(ClimberConstants.kPlant, DCMotor.getKrakenX60(1), 0.1, 0.1); + m_lefMotorSim.setAngle(Units.rotationsToRadians(ClimberConstants.kStowPosition)); + } + + public void updateInputs(ClimberIOInputs inputs) { + m_lefMotorSim.update(Robot.kDefaultPeriod); + m_rightMotorSim.update(Robot.kDefaultPeriod); + inputs.leftVoltage = m_voltage; + inputs.rightVoltage = m_voltage; + inputs.leftCurrent = m_lefMotorSim.getCurrentDrawAmps(); + inputs.rightCurrent = m_rightMotorSim.getCurrentDrawAmps(); + inputs.leftVelocityRPS = m_lefMotorSim.getAngularVelocityRPM() / 60.0; + inputs.rightVelocityRPS = m_rightMotorSim.getAngularVelocityRPM() / 60.0; + inputs.position = m_lefMotorSim.getAngularPositionRotations(); + } + + public void setVoltage(double voltage) { + m_voltage = voltage; + m_lefMotorSim.setInputVoltage(voltage); + m_rightMotorSim.setInputVoltage(voltage); + } + + public void setServo(double position) {} +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/coral/Coral.java b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/Coral.java new file mode 100644 index 00000000..713db8d5 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/Coral.java @@ -0,0 +1,139 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.coral; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.CoralLevel; +import frc.robot.Robot; +import frc.robot.RobotObserver; +import frc.robot.subsystems.PassiveSubsystem; +import frc.robot.subsystems.coral.CoralIO.CoralIOInputs; +import frc.robot.utils.LoopTimer; + +public class Coral extends PassiveSubsystem { + private final Logger m_logger = LoggerFactory.getLogger(Coral.class); + + private final LoopTimer m_timer; + + private final CoralIO m_io; + private final CoralIOInputs m_inputs; + private final CoralIOInputsLogger m_inputsLogger; + + public Coral() { + super(); + if (Robot.isReal()) { + m_io = new CoralIOHardware(); + } else { + m_io = new CoralIOSim(); + } + m_inputs = new CoralIOInputs(); + m_inputsLogger = new CoralIOInputsLogger(m_inputs); + RobotObserver.setCoralHeldSupplier(held()); + m_timer = new LoopTimer("Coral"); + } + + private void setVoltage(double voltage) { + take(); + m_io.setVoltage(voltage); + } + + private void setIntake() { + setVoltage(CoralConstants.kIntakeVoltage); + } + + private void setL2Score() { + m_logger.trace("Setting L2 eject"); + setVoltage(CoralConstants.kL2EjectVoltage); + } + + private void setL3Score() { + m_logger.trace("Setting L3 eject"); + setVoltage(CoralConstants.kL3EjectVoltage); + } + + private void setL4Score() { + m_logger.trace("Setting L4 eject"); + setVoltage(CoralConstants.kL4EjectVoltage); + } + + private void setL1Score() { + m_io.setLeftVoltage(CoralConstants.kL1LeftEjectVoltage); + m_io.setRightVoltage(CoralConstants.kL1RightEjectVoltage); + } + + private void stop() { + setVoltage(0); + } + + public Trigger present() { + return new Trigger( + () -> m_inputs.upperDetected || m_inputs.innerDetected || m_inputs.frontDetected); + } + + public Trigger held() { + return new Trigger(() -> m_inputs.frontDetected && !m_inputs.upperDetected); + } + + @Override + public void periodic() { + m_timer.reset(); + m_io.updateInputs(m_inputs); + m_inputsLogger.log(); + m_timer.log(); + } + + protected void passive() { + if (present().getAsBoolean() && !held().getAsBoolean()) { + setIntake(); + } else { + stop(); + } + } + + /** + * Intakes a game piece. The command ends when the piece is fully in the robot. + */ + public Command intake() { + return Commands.sequence( + runOnce(this::setIntake), + Commands.waitUntil(held())) + + .finallyDo(this::stop) + .unless(held()); + } + + public Command score(CoralLevel level) { + return Commands.sequence( + runOnce(() -> { + switch (level) { + case L1, SecondaryL1 -> setL1Score(); + case L2 -> setL2Score(); + case L3 -> setL3Score(); + case L4 -> setL4Score(); + } + }), + Commands.waitUntil(held().negate())) + + .onlyIf(held()); + } + + /** + * Ejects a coral piece + */ + public Command eject() { + return Commands.sequence( + runOnce(() -> setVoltage(CoralConstants.kEjectVoltage)), + Commands.waitUntil(present().negate())) + + .finallyDo(this::stop) + .onlyIf(present()); + } + +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralConstants.java new file mode 100644 index 00000000..d13ec950 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralConstants.java @@ -0,0 +1,76 @@ +package frc.robot.subsystems.coral; + +import com.ctre.phoenix6.configs.CANrangeConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FovParamsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.ProximityParamsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.ToFParamsConfigs; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.UpdateModeValue; + +public class CoralConstants { + protected static final int kLeftMotorID = 55; + protected static final int kRightMotorID = 56; + protected static final int kFrontCANrangeID = 59; + protected static final int kUpperCANrangeID = 58; + protected static final int kInnerCANrangeID = 54; + + protected static final double kIntakeVoltage = 2.4; + protected static final double kEjectVoltage = -6; + + protected static final double kL1EjectVoltage = 2.5; + protected static final double kL2EjectVoltage = 4.0; // 5.1 + protected static final double kL3EjectVoltage = 4.0; // 5.1 + protected static final double kL4EjectVoltage = 5.5; + + protected static final double kL1LeftEjectVoltage = 2; + protected static final double kL1RightEjectVoltage = 4; + + protected static final double kSupplyCurrentLimit = 20.0; + + protected static final InvertedValue kInvertRight = InvertedValue.Clockwise_Positive; + + protected static final TalonFXConfiguration kMotorConfig = new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) + .withInverted(InvertedValue.CounterClockwise_Positive)) + + .withCurrentLimits(new CurrentLimitsConfigs() + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(kSupplyCurrentLimit)); + + protected static final CANrangeConfiguration kFrontCANrangeConfig = new CANrangeConfiguration() + .withFovParams(new FovParamsConfigs() + .withFOVRangeX(6.5) + .withFOVRangeY(6.5)) + .withProximityParams(new ProximityParamsConfigs() + .withMinSignalStrengthForValidMeasurement(15015) + .withProximityThreshold(0.1)) + .withToFParams(new ToFParamsConfigs() + .withUpdateMode(UpdateModeValue.ShortRange100Hz)); + + + protected static final CANrangeConfiguration kUpperCANrangeConfig = new CANrangeConfiguration() + .withFovParams(new FovParamsConfigs() + .withFOVRangeX(6.5) + .withFOVRangeY(15)) + .withProximityParams(new ProximityParamsConfigs() + .withMinSignalStrengthForValidMeasurement(2500) + .withProximityThreshold(0.65)) + .withToFParams(new ToFParamsConfigs() + .withUpdateMode(UpdateModeValue.ShortRange100Hz)); + + protected static final CANrangeConfiguration kInnerCANrangeConfig = new CANrangeConfiguration() + .withFovParams(new FovParamsConfigs() + .withFOVRangeX(27) + .withFOVRangeY(27)) + .withProximityParams(new ProximityParamsConfigs() + .withMinSignalStrengthForValidMeasurement(1500) + .withProximityHysteresis(0) + .withProximityThreshold(0.06)) + .withToFParams(new ToFParamsConfigs() + .withUpdateMode(UpdateModeValue.ShortRange100Hz)); +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIO.java b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIO.java new file mode 100644 index 00000000..4be8577b --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIO.java @@ -0,0 +1,37 @@ +package frc.robot.subsystems.coral; + +public interface CoralIO { + void updateInputs(CoralIOInputs inputs); + + class CoralIOInputs { + public boolean leftMotorConnected = true; + public boolean rightMotorConnected = true; + public boolean frontCANrangeConnected = true; + public boolean upperCANrangeConnected = true; + public boolean innerCANrangeConnected = true; + public double leftVoltage = 0.0; + public double rightVoltage = 0.0; + public double leftCurrent = 0.0; + public double rightCurrent = 0.0; + public double leftTemperature = 0.0; + public double rightTemperature = 0.0; + public boolean frontDetected = false; + public boolean upperDetected = false; + public boolean innerDetected = false; + public double frontDistance = 0.0; + public double upperDistance = 0.0; + public double innerDistance = 0.0; + public double frontStrength = 0.0; + public double upperStreingth = 0.0; + public double innerStrength = 0.0; + } + + void setLeftVoltage(double voltage); + void setRightVoltage(double voltage); + + default void setVoltage(double voltage) { + setRightVoltage(voltage); + setLeftVoltage(voltage); + } +} + diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIOHardware.java b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIOHardware.java new file mode 100644 index 00000000..2e52180b --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIOHardware.java @@ -0,0 +1,152 @@ +package frc.robot.subsystems.coral; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.CANrange; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.utils.StatusSignalUtil; + +public class CoralIOHardware implements CoralIO { + private final TalonFX m_leftMotor; + private final TalonFX m_rightMotor; + + private final CANrange m_frontCANrange; + private final CANrange m_upperCANrange; + private final CANrange m_innerCANrange; + + private double m_leftVoltage; + private double m_rightVoltage; + + private final StatusSignal m_leftVoltageSignal; + private final StatusSignal m_rightVoltageSignal; + private final StatusSignal m_leftCurrentSignal; + private final StatusSignal m_rightCurrentSignal; + private final StatusSignal m_leftTempSignal; + private final StatusSignal m_rightTempSignal; + private final StatusSignal m_leftVelocitySignal; + private final StatusSignal m_rightVelocitySignal; + + private final StatusSignal m_frontDistanceSignal; + private final StatusSignal m_upperDistanceSignal; + private final StatusSignal m_innerDistanceSignal; + private final StatusSignal m_frontDetectedSignal; + private final StatusSignal m_upperDetectedSignal; + private final StatusSignal m_innerDetectedSignal; + private final StatusSignal m_frontStrengthSignal; + private final StatusSignal m_upperStrengthSignal; + private final StatusSignal m_innerStrengthSignal; + + public CoralIOHardware() { + m_leftMotor = new TalonFX(CoralConstants.kLeftMotorID); + m_rightMotor = new TalonFX(CoralConstants.kRightMotorID); + + m_frontCANrange = new CANrange(CoralConstants.kFrontCANrangeID); + m_upperCANrange = new CANrange(CoralConstants.kUpperCANrangeID); + m_innerCANrange = new CANrange(CoralConstants.kInnerCANrangeID); + + m_leftMotor.getConfigurator().apply(CoralConstants.kMotorConfig); + m_rightMotor.getConfigurator().apply(CoralConstants.kMotorConfig.withMotorOutput( + CoralConstants.kMotorConfig.MotorOutput.withInverted(CoralConstants.kInvertRight))); + + m_frontCANrange.getConfigurator().apply(CoralConstants.kFrontCANrangeConfig); + m_upperCANrange.getConfigurator().apply(CoralConstants.kUpperCANrangeConfig); + m_innerCANrange.getConfigurator().apply(CoralConstants.kInnerCANrangeConfig); + + m_leftVoltageSignal = m_leftMotor.getMotorVoltage(); + m_rightVoltageSignal = m_rightMotor.getMotorVoltage(); + m_leftCurrentSignal = m_leftMotor.getSupplyCurrent(); + m_rightCurrentSignal = m_rightMotor.getSupplyCurrent(); + m_leftTempSignal = m_leftMotor.getDeviceTemp(); + m_rightTempSignal = m_rightMotor.getDeviceTemp(); + m_leftVelocitySignal = m_leftMotor.getVelocity(); + m_rightVelocitySignal = m_rightMotor.getVelocity(); + + m_frontDistanceSignal = m_frontCANrange.getDistance(); + m_upperDistanceSignal = m_upperCANrange.getDistance(); + m_innerDistanceSignal = m_innerCANrange.getDistance(); + m_frontStrengthSignal = m_frontCANrange.getSignalStrength(); + m_upperStrengthSignal = m_upperCANrange.getSignalStrength(); + m_innerStrengthSignal = m_innerCANrange.getSignalStrength(); + m_frontDetectedSignal = m_frontCANrange.getIsDetected(); + m_upperDetectedSignal = m_upperCANrange.getIsDetected(); + m_innerDetectedSignal = m_innerCANrange.getIsDetected(); + + StatusSignalUtil.registerRioSignals( + m_leftVoltageSignal, + m_rightVoltageSignal, + m_leftCurrentSignal, + m_rightCurrentSignal, + m_leftTempSignal, + m_rightTempSignal, + m_leftVelocitySignal, + m_rightVelocitySignal, + m_frontDetectedSignal, + m_upperDetectedSignal, + m_innerDetectedSignal, + m_frontDistanceSignal, + m_upperDistanceSignal, + m_innerDistanceSignal, + m_frontStrengthSignal, + m_upperStrengthSignal, + m_innerStrengthSignal); + } + + public void updateInputs(CoralIOInputs inputs) { + inputs.rightMotorConnected = BaseStatusSignal.isAllGood( + m_rightVoltageSignal, + m_rightCurrentSignal, + m_rightTempSignal, + m_rightVelocitySignal); + inputs.leftMotorConnected = BaseStatusSignal.isAllGood( + m_leftVoltageSignal, + m_leftCurrentSignal, + m_leftTempSignal, + m_leftVelocitySignal); + inputs.rightVoltage = m_rightVoltageSignal.getValueAsDouble(); + inputs.leftVoltage = m_leftVoltageSignal.getValueAsDouble(); + inputs.rightCurrent = m_rightCurrentSignal.getValueAsDouble(); + inputs.leftCurrent = m_leftCurrentSignal.getValueAsDouble(); + inputs.rightTemperature = m_rightTempSignal.getValueAsDouble(); + inputs.leftTemperature = m_leftTempSignal.getValueAsDouble(); + inputs.frontCANrangeConnected = BaseStatusSignal.isAllGood( + m_frontDetectedSignal, + m_frontDistanceSignal, + m_frontStrengthSignal); + inputs.upperCANrangeConnected = BaseStatusSignal.isAllGood( + m_upperDetectedSignal, + m_upperDistanceSignal, + m_upperStrengthSignal); + inputs.innerCANrangeConnected = BaseStatusSignal.isAllGood( + m_innerDetectedSignal, + m_innerDistanceSignal, + m_innerStrengthSignal); + inputs.frontDetected = m_frontDetectedSignal.getValue(); + inputs.upperDetected = m_upperDetectedSignal.getValue(); + inputs.innerDetected = m_innerDetectedSignal.getValue(); + inputs.frontDistance = m_frontDistanceSignal.getValueAsDouble(); + inputs.upperDistance = m_upperDistanceSignal.getValueAsDouble(); + inputs.innerDistance = m_innerDistanceSignal.getValueAsDouble(); + inputs.frontStrength = m_frontStrengthSignal.getValueAsDouble(); + inputs.upperStreingth = m_upperStrengthSignal.getValueAsDouble(); + inputs.innerStrength = m_innerStrengthSignal.getValueAsDouble(); + } + + public void setLeftVoltage(double voltage) { + if (m_leftVoltage != voltage) { + m_leftVoltage = voltage; + m_leftMotor.setVoltage(voltage); + } + } + + public void setRightVoltage(double voltage) { + if (m_rightVoltage != voltage) { + m_rightVoltage = voltage; + m_rightMotor.setVoltage(voltage); + } + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIOInputsLogger.java b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIOInputsLogger.java new file mode 100644 index 00000000..2a9a64a0 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIOInputsLogger.java @@ -0,0 +1,36 @@ +package frc.robot.subsystems.coral; + +import frc.robot.subsystems.coral.CoralIO.CoralIOInputs; +import frc.robot.utils.OnboardLogger; + +public class CoralIOInputsLogger { + private final OnboardLogger log; + + public CoralIOInputsLogger(CoralIOInputs inputs) { + log = new OnboardLogger("Coral"); + log.registerBoolean("Left Motor Connected", () -> inputs.leftMotorConnected); + log.registerBoolean("Right Motor Connected", () -> inputs.rightMotorConnected); + log.registerBoolean("Front CANrange Connected", () -> inputs.frontCANrangeConnected); + log.registerBoolean("Upper CANrange Connected", () -> inputs.upperCANrangeConnected); + log.registerBoolean("Inner CANrange Connected", () -> inputs.innerCANrangeConnected); + log.registerDouble("Left Voltage", () -> inputs.leftVoltage); + log.registerDouble("Right Voltage", () -> inputs.rightVoltage); + log.registerDouble("Left Current", () -> inputs.leftCurrent); + log.registerDouble("Right Current", () -> inputs.rightCurrent); + log.registerDouble("Left Temperature", () -> inputs.leftTemperature); + log.registerDouble("Right Temperature", () -> inputs.rightTemperature); + log.registerBoolean("Front Detected", () -> inputs.frontDetected); + log.registerBoolean("Upper Detected", () -> inputs.upperDetected); + log.registerBoolean("Inner Detected", () -> inputs.innerDetected); + log.registerDouble("Front Distance", () -> inputs.frontDistance); + log.registerDouble("Upper Distance", () -> inputs.upperDistance); + log.registerDouble("Inner Distance", () -> inputs.innerDistance); + log.registerDouble("Front Strength", () -> inputs.frontStrength); + log.registerDouble("Upper Strength", () -> inputs.upperStreingth); + log.registerDouble("Inner Strength", () -> inputs.innerStrength); + } + + public void log() { + log.log(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIOSim.java b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIOSim.java new file mode 100644 index 00000000..732c41b9 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralIOSim.java @@ -0,0 +1,52 @@ +package frc.robot.subsystems.coral; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; + +public class CoralIOSim implements CoralIO { + private final DCMotorSim m_leftMotorSim; + private final DCMotorSim m_rightMotorSim; + + private double m_leftVoltage; + private double m_rightVoltage; + + public CoralIOSim() { + m_leftMotorSim = + new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 1, 1), + DCMotor.getKrakenX60(1), 1, 0.1); + m_rightMotorSim = + new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 1, 1), + DCMotor.getKrakenX60(1), 1, 0.1); + } + + public void updateInputs(CoralIOInputs inputs) { + m_leftMotorSim.update(Robot.kDefaultPeriod); + m_rightMotorSim.update(Robot.kDefaultPeriod); + inputs.leftCurrent = m_leftMotorSim.getCurrentDrawAmps(); + inputs.rightCurrent = m_rightMotorSim.getCurrentDrawAmps(); + inputs.leftTemperature = 0.0; + inputs.rightTemperature = 0.0; + inputs.leftVoltage = m_leftVoltage; + inputs.rightVoltage = m_rightVoltage; + inputs.frontDetected = SmartDashboard.getBoolean("Coral/Front CANrange", false); + inputs.upperDetected = SmartDashboard.getBoolean("Coral/Upper CANrange", false); + inputs.innerDetected = SmartDashboard.getBoolean("Coral/Inner CANrange", false); + // Without publishing these values, they will never be able to be read. + SmartDashboard.putBoolean("Coral/Front CANrange", inputs.frontDetected); + SmartDashboard.putBoolean("Coral/Upper CANrange", inputs.upperDetected); + SmartDashboard.putBoolean("Coral/Inner CANrange", inputs.innerDetected); + } + + public void setLeftVoltage(double voltage) { + m_leftVoltage = voltage; + m_leftMotorSim.setInputVoltage(voltage); + } + + public void setRightVoltage(double voltage) { + m_rightVoltage = voltage; + m_rightMotorSim.setInputVoltage(voltage); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java new file mode 100644 index 00000000..6ddd920b --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -0,0 +1,505 @@ +package frc.robot.subsystems.drivetrain; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; +import java.io.IOException; +import java.util.Optional; +import java.util.function.DoubleSupplier; +import org.json.simple.parser.ParseException; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModule; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.ApplyRobotSpeeds; +import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric; +import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentricFacingAngle; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; +import com.ctre.phoenix6.swerve.SwerveRequest.RobotCentric; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.util.DriveFeedforwards; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.pathplanner.lib.util.swerve.SwerveSetpoint; +import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.FFConstants; +import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.SimConstants; +import frc.robot.Robot; +import frc.robot.RobotObserver; +import com.therekrab.autopilot.APTarget; +import com.therekrab.autopilot.Autopilot; +import frc.robot.driveassist.ForceField; +import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.utils.FieldUtils; +import frc.robot.utils.LoopTimer; +import frc.robot.utils.OnboardLogger; +import frc.robot.vision.localization.TimestampedPoseEstimate; +import frc.robot.vision.tracking.AlgaeTracker.ObjectTrackingStatus; + +/** + * Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily + * be used in command-based projects. + */ +public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { + @SuppressWarnings("unused") + private final Logger m_logger = LoggerFactory.getLogger(CommandSwerveDrivetrain.class); + private final OnboardLogger m_ologger; + + private LoopTimer m_timer; + + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + private boolean m_aligned; + + private Trigger m_tippyTrigger = new Trigger(() -> false); + + private boolean m_hasReceivedVisionUpdate; + + private FieldCentric m_teleopRequest = new FieldCentric() + .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) + .withDriveRequestType(DriveRequestType.Velocity); + + private RobotCentric m_trackingRequest = new RobotCentric() + .withDriveRequestType(DriveRequestType.Velocity); + + private FieldCentricFacingAngle m_veloRequest = new FieldCentricFacingAngle() + .withHeadingPID(DriveConstants.HeadingPID.kP, 0, 0) + .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) + .withDriveRequestType(DriveRequestType.Velocity); + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; + + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; + + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; + + /* last known object tracking input */ + private Optional m_objectStatus = Optional.empty(); + + private Pose2d m_estimatedPose = new Pose2d(); + + private SwerveSetpointGenerator setpointGenerator; + private SwerveSetpoint previousSetpoint; + + private final ApplyRobotSpeeds autoRequest = + new ApplyRobotSpeeds().withDriveRequestType(SwerveModule.DriveRequestType.Velocity); + + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { + super(drivetrainConstants, modules); + if (Robot.isSimulation()) { + startSimThread(); + } + m_aligned = false; + m_ologger = new OnboardLogger("Drivetrain"); + m_ologger.registerBoolean("Aligned", aligned()); + m_ologger.registerDouble("Velocity", this::getVelocity); + m_ologger.registerPose("Estimated Pose", this::getPose); + m_ologger.registerBoolean("Received Vision Update", () -> m_hasReceivedVisionUpdate); + + RobotObserver.setVelocitySupplier(this::getVelocity); + RobotObserver.setNoElevatorZoneSupplier(dangerZone()); + RobotObserver.setReefReadySupplier(inReefZone()); + RobotObserver.setAlginedSupplier(aligned()); + + m_timer = new LoopTimer("Drivetrain"); + initializePathPlanner(); + } + + public void initializePathPlanner() { + RobotConfig config; + try { + config = RobotConfig.fromGUISettings(); + AutoBuilder.configure( + this::getPose, // Robot pose supplier + this::resetPose, // Method to reset odometry (will be called if your auto has a + // starting pose) + this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> driveWithChassisSpeeds(speeds), + DriveConstants.kPathplannerHolonomicDriveController, + config, // The robot configuration + () -> { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this); // Reference to this subsystem to set requirements + + initializeSetpointGenerator(config); + + PathPlannerLogging.setLogActivePathCallback( + poses -> RobotObserver.getField().getObject("Pathfind Trajectory").setPoses(poses)); + } catch (IOException | ParseException e) { + e.printStackTrace(); + System.exit(1); + } + } + + public void initializeSetpointGenerator(RobotConfig config) { + setpointGenerator = new SwerveSetpointGenerator(config, + Units.rotationsToRadians(DriveConstants.kMaxRotationalSpeed)); + + // TODO: is this causing problems when the previous setpoint doesn't match the robot speeds? + // We saw issues where it would try to drive the wrong way. This could easily be the cause if + // I'm understanding this system correctly + ChassisSpeeds currSpeeds = getRobotRelativeSpeeds(); + SwerveModuleState[] currStates = getState().ModuleStates; + previousSetpoint = + new SwerveSetpoint(currSpeeds, currStates, DriveFeedforwards.zeros(config.numModules)); + } + + private Translation2d getVelocityComponents() { + double vx = getRobotRelativeSpeeds().vxMetersPerSecond; + double vy = getRobotRelativeSpeeds().vyMetersPerSecond; + Rotation2d theta = getPose().getRotation(); + return new Translation2d(vx, vy).rotateBy(theta); + + } + + private double getVelocity() { + Translation2d velo = getVelocityComponents(); + return velo.getNorm(); + } + + public Pose2d getPose() { + return m_estimatedPose; + } + + /** + * returns the current pose, with red side poses flipped + */ + public Pose2d getBluePose() { + return FieldUtils.getLocalPose(m_estimatedPose); + } + + public void zeroPose() { + setPose(new Pose2d()); + } + + public Command resetHeading() { + // this uses Commands.runOnce() as opposed to this.runOnce() because we don't really want to add + // requirements on this subsystem + return Commands.runOnce(() -> setOperatorPerspectiveForward(getPose().getRotation())); + } + + private void setPose(Pose2d pose) { + resetPose(pose); + } + + private ChassisSpeeds getRobotRelativeSpeeds() { + return getState().Speeds; + } + + private void driveWithChassisSpeeds(ChassisSpeeds speeds) { + previousSetpoint = setpointGenerator.generateSetpoint( + previousSetpoint, // The previous setpoint + speeds, // The desired target speeds + 0.02 // The loop time of the robot code, in seconds + ); + + setControl(autoRequest.withSpeeds(previousSetpoint.robotRelativeSpeeds())); + } + + private void stop() { + setControl(new SwerveRequest.SwerveDriveBrake()); + } + + @Override + public void periodic() { + m_timer.reset(); + m_estimatedPose = this.getState().Pose; + + RobotObserver.getField().setRobotPose(m_estimatedPose); + + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent(allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation); + m_hasAppliedOperatorPerspective = true; + }); + } + m_ologger.log(); + m_hasReceivedVisionUpdate = false; + // Expire old algae tracking data + if (m_objectStatus.isPresent() && m_objectStatus.get().isExpired()) { + m_objectStatus = Optional.empty(); + } + m_timer.log(); + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(SimConstants.k_simPeriodic); + } + + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = + new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = + new SwerveRequest.SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = + new SwerveRequest.SysIdSwerveRotation(); + + private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), + null, + this)); + + private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), + null, + this)); + + private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually radians per second, but SysId only supports "volts" */ + setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + }, + null, + this)); + + public void driveRobotRelative(ChassisSpeeds speeds) { + setControl(new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds)); + } + + public void addPoseEstimate(TimestampedPoseEstimate estimate) { + m_hasReceivedVisionUpdate = true; + // This should NOT run in simulation! + if (Robot.isSimulation()) { + return; + } + addVisionMeasurement( + estimate.pose(), + estimate.timestamp(), + estimate.stdDevs()); + } + + public void addObjectTrackingData(ObjectTrackingStatus status) { + m_objectStatus = Optional.of(status); + } + + public Command sysIdQuasistaticTranslation(SysIdRoutine.Direction direction) { + return m_sysIdRoutineTranslation.quasistatic(direction); + } + + public Command sysIdDynamicTranslation(SysIdRoutine.Direction direction) { + return m_sysIdRoutineTranslation.dynamic(direction); + } + + public Command sysIdQuasistaticSteer(SysIdRoutine.Direction direction) { + return m_sysIdRoutineSteer.quasistatic(direction); + } + + public Command sysIdDynamicSteer(SysIdRoutine.Direction direction) { + return m_sysIdRoutineSteer.dynamic(direction); + } + + public Command sysIdQuasistaticRotation(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.quasistatic(direction); + } + + public Command sysIdDynamicRotation(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.dynamic(direction); + } + + private Trigger dangerZone() { + return new Trigger(() -> { + double distance = Math.abs(m_estimatedPose.getX() - FFConstants.k_bargeX); + return distance < FFConstants.k_radius && !DriverStation.isAutonomous(); + }); + } + + public void setAligned(boolean aligned) { + m_aligned = aligned; + } + + public Trigger aligned() { + return new Trigger(() -> m_aligned); + } + + public Trigger inReefZone() { + return new Trigger(() -> { + double distanceToReef = getBluePose().getTranslation() + .minus(FieldConstants.reefCenter) + .getNorm(); + boolean inRange = + (DriverStation.isAutonomous()) ? distanceToReef <= FieldConstants.kReefReadyAuton + : distanceToReef <= FieldConstants.kReefReady; + return inRange; + }); + } + + /** + * Drives the robot from given x, y, and rotatational velocity suppliers. + */ + public Command teleopDrive(DoubleSupplier x, DoubleSupplier y, DoubleSupplier rot) { + return run(() -> { + double vx = x.getAsDouble() * DriveConstants.kMaxTeleopLinearSpeed; + double vy = y.getAsDouble() * DriveConstants.kMaxTeleopLinearSpeed; + Translation2d operatorRelative = new Translation2d(vx, vy); + Translation2d fieldRelative = operatorRelative.rotateBy(getOperatorForwardDirection()); + double omega = rot.getAsDouble() * DriveConstants.kMaxTeleopAngularSpeed; + + if (Math.hypot(vx, vy) > 1e-3 || Math.abs(omega) > 1e-2) { + setAligned(false); + } + if (RobotObserver.getFFEnabled()) { + double newX = ForceField.calculate( + fieldRelative.getX(), + FFConstants.k_bargeX - m_estimatedPose.getX()); + fieldRelative = new Translation2d(newX, fieldRelative.getY()); + } + setControl(m_teleopRequest + .withVelocityX(fieldRelative.getX()) + .withVelocityY(fieldRelative.getY()) + .withRotationalRate(omega)); + }); + } + + public void setTippyTrigger(Trigger tippyTrigger) { + m_tippyTrigger = tippyTrigger; + } + + private AngularVelocity getMaxRotationalRate() { + if (m_tippyTrigger.getAsBoolean()) { + return DriveConstants.kMaxTippyAngularSpeed; + } else { + return DriveConstants.kMaxAngularSpeed; + } + } + + /** + * Drives to a certain point on the field + */ + public Command align(Autopilot autopilot, APTarget target) { + return Commands.sequence( + runOnce(() -> { + RobotObserver.getField().getObject("reference").setPose(target.getReference()); + setAligned(false); + }), + run(() -> { + Translation2d velocities = getVelocityComponents(); + Transform2d output = autopilot.calculate(m_estimatedPose, velocities, target); + setControl(m_veloRequest + .withVelocityX(output.getX()) + .withVelocityY(output.getY()) + .withTargetDirection(output.getRotation()) + .withMaxAbsRotationalRate(getMaxRotationalRate())); + })) + .until(() -> { + return autopilot.atTarget(m_estimatedPose, target); + }) + .finallyDo(this::stop) + .finallyDo(interrupted -> setAligned(!interrupted)) + .finallyDo(() -> { + RobotObserver.getField().getObject("reference").setPoses(); + }); + } + + public Command seedLocal(Pose2d pose) { + return Commands.runOnce(() -> resetPose(FieldUtils.getLocalPose(pose))) + .ignoringDisable(true); + } + + public Command followObject() { + return run(new Runnable() { + private final PIDController thetaController = new PIDController(8, 0, 0); + private final PIDController driveController = new PIDController(1, 0, 0); + + @Override + public void run() { + m_objectStatus.ifPresentOrElse(status -> { + double output = -this.thetaController.calculate(status.yaw().getRadians(), 0); + double speed = DriveConstants.kObjectTrackSpeed.in(MetersPerSecond); + if (status.pose().isPresent()) { + double distance = m_estimatedPose + .relativeTo(status.pose().get()) + .getTranslation() + .getNorm(); + speed = Math.min( + DriveConstants.kMaxObjectTrackingSpeed.in(MetersPerSecond), + -this.driveController.calculate(distance, 0)); + } + /* + * we invert the calculation because it outputs a negative number, because measurement > + * ref, so err < 0 + */ + double vx = speed * status.yaw().getCos(); + double vy = speed * status.yaw().getSin(); + setControl(m_trackingRequest + .withRotationalRate(output) + .withVelocityX(vx) + .withVelocityY(vy)); + }, () -> stop()); + } + }); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java new file mode 100644 index 00000000..f8e8baa6 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java @@ -0,0 +1,91 @@ +package frc.robot.subsystems.drivetrain; + +import static edu.wpi.first.units.Units.Centimeters; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.therekrab.autopilot.APConstraints; +import com.therekrab.autopilot.APProfile; +import com.therekrab.autopilot.Autopilot; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearAcceleration; +import edu.wpi.first.units.measure.LinearVelocity; +import frc.robot.generated.TunerConstants; + +public class DriveConstants { + protected static final PIDConstants kTranslationPID = new PIDConstants(2, 0.0, 0.0); + protected static final PIDConstants kRotationPID = new PIDConstants(1.5, 0.0, 0.0); + + public static class HeadingPID { + protected static final double kP = 8.0; + } + + private static final APConstraints kTightAutopilotAPConstraints = new APConstraints() + .withAcceleration(15.0) + .withJerk(1.5); + + private static final APProfile kTightProfile = new APProfile(kTightAutopilotAPConstraints) + .withErrorXY(Centimeters.of(1)) + .withErrorTheta(Degrees.of(1)) + .withBeelineRadius(Centimeters.of(10)); + + public static final Autopilot kTightAutopilot = new Autopilot(kTightProfile); + + private static final APConstraints kFastAPConstraints = + new APConstraints() + .withAcceleration(20) + .withJerk(8); + + private static final APProfile kFastProfile = new APProfile(kFastAPConstraints) + .withErrorXY(Centimeters.of(15)) + .withErrorTheta(Degrees.of(5)) + .withBeelineRadius(Centimeters.of(10)); + + public static final Autopilot kFastAutopilot = new Autopilot(kFastProfile); + + private static final APConstraints kSlowAPConstraints = new APConstraints() + .withAcceleration(3) + .withJerk(1.5) + .withVelocity(3); + + private static final APProfile kSlowProfile = new APProfile(kSlowAPConstraints) + .withErrorXY(Inches.of(2)) + .withErrorTheta(Degrees.of(3)) + .withBeelineRadius(Centimeters.of(20)); + + public static final Autopilot kSlowAutopilot = new Autopilot(kSlowProfile); + + protected static final PPHolonomicDriveController kPathplannerHolonomicDriveController = + new PPHolonomicDriveController(kTranslationPID, kRotationPID); + + protected static final double kMaxTeleopLinearSpeed = + TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + protected static final double kMaxTeleopAngularSpeed = + RotationsPerSecond.of(1.5).in(RadiansPerSecond); + + protected static final LinearVelocity kMaxLinearSpeed = MetersPerSecond.of(4); + protected static final LinearAcceleration kMaxLinearAcceleration = + MetersPerSecondPerSecond.of(3); + protected static final AngularVelocity kMaxAngularSpeed = RotationsPerSecond.of(2); + protected static final AngularVelocity kMaxTippyAngularSpeed = RotationsPerSecond.of(0.5); + protected static final AngularAcceleration kMaxAngularAcceleration = + RotationsPerSecondPerSecond.of(2); + + protected static final double kMaxRotationalSpeed = kMaxLinearSpeed.in(MetersPerSecond) + / (TunerConstants.kWheelRadius.in(Meters) * 2 * Math.PI); + + protected static final double k_closedLoopOverrideToleranceTranslation = 0.05; + protected static final double k_closedLoopOverrideToleranceRotation = 0.05; + + protected static final LinearVelocity kObjectTrackSpeed = MetersPerSecond.of(2); + protected static final LinearVelocity kMaxObjectTrackingSpeed = MetersPerSecond.of(4); +} + diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java new file mode 100644 index 00000000..1c0e2615 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,165 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.elevator; + +import static edu.wpi.first.units.Units.Seconds; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.CoralLevel; +import frc.robot.Robot; +import frc.robot.RobotObserver; +import frc.robot.subsystems.PassiveSubsystem; +import frc.robot.subsystems.elevator.ElevatorIO.ElevatorIOInputs; +import frc.robot.utils.LoopTimer; +import frc.robot.utils.OnboardLogger; + +public class Elevator extends PassiveSubsystem { + // we want to have a logger, even if we're not using it... yet + @SuppressWarnings("unused") + private final Logger m_logger = LoggerFactory.getLogger(Elevator.class); + + private final LoopTimer m_timer; + + private final ElevatorIO m_io; + private final ElevatorIOInputs m_inputs; + private final ElevatorIOInputsLogger m_inputsLogger; + + private final OnboardLogger m_ologger; + + private final Debouncer m_debouncer = + new Debouncer(ElevatorConstants.kRangeDebounceTime.in(Seconds)); + + private ElevatorState m_reference = ElevatorState.Stow; + + private Trigger m_prefireReq = new Trigger(() -> false); + private Trigger m_stay = new Trigger(() -> false); + + public Elevator() { + super(); + if (Robot.isReal()) { + m_io = new ElevatorIOHardware(); + } else { + m_io = new ElevatorIOSim(); + } + m_inputs = new ElevatorIOInputs(); + m_inputsLogger = new ElevatorIOInputsLogger(m_inputs); + m_ologger = new OnboardLogger("Elevator"); + m_ologger.registerString("State", () -> m_reference.toString()); + SmartDashboard.putData("Elevator/Lazy Zero", + runOnce(m_io::calibrateZero).ignoringDisable(true).withName("Lazy Zero")); + m_timer = new LoopTimer("Elevator"); + } + + private void setPosition(ElevatorState state) { + take(); + // calculate goal we should go to + double goal = state.position(); + if (RobotObserver.getNoElevatorZone() + && (m_inputs.position > ElevatorConstants.kUnsafeRange || goal > ElevatorConstants.kUnsafeRange)) { + // either trying to reach (or already at) a no-go state given our current position + return; + } + // floor values for the goal between our two extrema for their positions + goal = Math.min(goal, ElevatorConstants.kForwardSoftLimit); + goal = Math.max(goal, ElevatorConstants.kReverseSoftLimit); + m_io.setPosition(goal); + m_reference = state; + } + + public Trigger ready() { + return new Trigger( + () -> Math.abs(m_reference.position() - m_inputs.position) < ElevatorConstants.kTolerance); + } + + public Trigger ready(ElevatorState state) { + return new Trigger(() -> { + if (m_reference.equals(state)) { + return ready().getAsBoolean(); + } + return false; + }); + } + + public Trigger ready(CoralLevel level) { + return ready(level.toElevatorState()); + } + + private boolean atZero() { + return m_debouncer.calculate(m_inputs.zeroCANrangeDetected); + } + + @Override + public void periodic() { + m_timer.reset(); + m_io.updateInputs(m_inputs); + m_inputsLogger.log(); + m_ologger.log(); + m_timer.log(); + } + + /** + * Whether or not the elevator is above the "safe" range + */ + public Trigger unsafe() { + return new Trigger(() -> m_inputs.position > ElevatorConstants.kUnsafeRange + || m_reference.position() > ElevatorConstants.kUnsafeRange); + } + + protected void passive() { + if (m_stay.getAsBoolean()) { + return; + } + if (m_prefireReq.getAsBoolean()) { + setPosition(ElevatorState.L2); + } else { + setPosition(ElevatorState.Stow); + } + } + + public Command go(ElevatorState state) { + return Commands.sequence( + runOnce(() -> setPosition(state)), + Commands.waitUntil(ready())) + .withName("Elevator(" + state.toString() + ")"); + } + + public Command go(CoralLevel level) { + return go(level.toElevatorState()); + } + + /** + * Automatically zeroes the elevator. + */ + public Command autoZero() { + return Commands.waitUntil(this::atZero).deadlineFor( + Commands.sequence( + go(ElevatorState.Zero), + runOnce(m_io::disableLimits), + runOnce(() -> m_io.setVoltage(ElevatorConstants.kZeroVoltage)))) + + .finallyDo(m_io::enableLimits) + .finallyDo(interrupted -> { + if (!interrupted) { + m_io.calibrateZero(); + } + }) + .withName("Autozero"); + } + + public void setPrefireRequirement(Trigger trigger) { + m_prefireReq = trigger; + } + + public void setStayRequirement(Trigger trigger) { + m_stay = trigger; + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 00000000..b258799d --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,110 @@ +package frc.robot.subsystems.elevator; + +import static edu.wpi.first.units.Units.Seconds; +import com.ctre.phoenix6.configs.CANrangeConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.FovParamsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.ProximityParamsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Time; + +public final class ElevatorConstants { + protected static final int kLeftMotorID = 51; + protected static final int kRightMotorID = 52; + protected static final int kCANrangeID = 53; + + protected static final boolean kInvertLeft = true; + + protected static final double kSupplyCurrentLimit = 100; + + protected static final double kRotorToSensorRatio = 5.2; + protected static final double kSensorToMechanismRatio = 1; + + protected static final double kGearRatio = kRotorToSensorRatio * kSensorToMechanismRatio; + + private static final double kDrumRadius = Units.inchesToMeters(2.256 / 2); + + protected static final double kMetersToRotations = 1 / (kDrumRadius * 2 * Math.PI); + + protected static final double kInch = Units.inchesToMeters(1) * kMetersToRotations; + + protected static final double kReefOffset = 3 * kInch; + + protected static final double kForwardSoftLimit = 11.15; + protected static final double kReverseSoftLimit = 0; + + protected static final double kZeroVoltage = -4.0; + + protected static final double kUnsafeRange = ElevatorState.L2.position() + 2 * kInch; + + protected static final double kTolerance = 0.06; + + protected static final double kMaxSpeed = 32; + protected static final double kMaxAcceleration = 48; + protected static final double kMaxJerk = 480; + + protected static final TalonFXConfiguration kMotorConfig = new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive)) + + .withFeedback(new FeedbackConfigs() + .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) + .withSensorToMechanismRatio(kGearRatio)) + + .withCurrentLimits(new CurrentLimitsConfigs() + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(kSupplyCurrentLimit)) + + .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitThreshold(kForwardSoftLimit) + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitThreshold(kReverseSoftLimit) + .withReverseSoftLimitEnable(false)) + + .withSlot0(new Slot0Configs() + .withGravityType(GravityTypeValue.Elevator_Static) + .withKP(20) + .withKI(0) + .withKD(0) + .withKS(0.125) + .withKV(3.59 * (kDrumRadius * 2 * Math.PI)) + .withKA(0.05 * (kDrumRadius * 2 * Math.PI)) + .withKG(0.42)) + + .withMotionMagic(new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(kMaxSpeed) + .withMotionMagicAcceleration(kMaxAcceleration) + .withMotionMagicJerk(kMaxJerk)); + + protected static final CANrangeConfiguration kCANrangeConfig = new CANrangeConfiguration() + .withFovParams(new FovParamsConfigs() + .withFOVRangeX(6.75) + .withFOVRangeY(6.75)) + .withProximityParams(new ProximityParamsConfigs() + .withMinSignalStrengthForValidMeasurement(3500) + .withProximityThreshold(0.13) + .withProximityHysteresis(0)); + + protected static final Time kRangeDebounceTime = Seconds.of(0.06); + + protected static final LinearSystem kPlant = + LinearSystemId.createElevatorSystem(DCMotor.getKrakenX60(1), 1, 1, 1); + public static final double kCalibrationTime = 3.5; // seconds +} + diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java new file mode 100644 index 00000000..816e7f6e --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,35 @@ +package frc.robot.subsystems.elevator; + +public interface ElevatorIO { + void updateInputs(ElevatorIOInputs inputs); + + public class ElevatorIOInputs { + public boolean leftMotorConnected = true; + public boolean rightMotorConnected = true; + public double leftVoltage = 0.0; + public double rightVoltage = 0.0; + public double leftCurrent = 0.0; + public double rightCurrent = 0.0; + public double leftTemp = 0.0; + public double rightTemp = 0.0; + public double leftVelocityRPS = 0.0; + public double rightVelocityRPS = 0.0; + public double leftPosition = 0.0; + public double rightPosition = 0.0; + public double position = 0.0; + public double reference = 0.0; + public boolean zeroCANrangeConnected = true; + public boolean zeroCANrangeDetected = false; + public double zeroCANrangeDistance = 0.0; + public double zeroCANrangeStrength = 0.0; + } + + void setPosition(double position); + + void setVoltage(double voltage); + + void disableLimits(); + void enableLimits(); + + void calibrateZero(); +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIOHardware.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIOHardware.java new file mode 100644 index 00000000..16562cca --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIOHardware.java @@ -0,0 +1,156 @@ +package frc.robot.subsystems.elevator; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.CANrange; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.utils.StatusSignalUtil; + +public class ElevatorIOHardware implements ElevatorIO { + private final TalonFX m_leftMotor; + private final TalonFX m_rightMotor; + + private final CANrange m_CANrange; + + private final DynamicMotionMagicVoltage m_control; + + private double m_reference = Double.NaN; + + private final StatusSignal m_leftVoltageSignal; + private final StatusSignal m_rightVoltageSignal; + private final StatusSignal m_leftCurrentSignal; + private final StatusSignal m_rightCurrentSignal; + private final StatusSignal m_leftTempSignal; + private final StatusSignal m_rightTempSignal; + private final StatusSignal m_leftVelocitySignal; + private final StatusSignal m_rightVelocitySignal; + private final StatusSignal m_leftPositionSignal; + private final StatusSignal m_rightPositionSignal; + + private final StatusSignal m_CANrangeDistanceSignal; + private final StatusSignal m_CANrangeDetectedSignal; + private final StatusSignal m_CANrangeStrengthSignal; + + public ElevatorIOHardware() { + m_rightMotor = new TalonFX(ElevatorConstants.kRightMotorID, "*"); + m_leftMotor = new TalonFX(ElevatorConstants.kLeftMotorID, "*"); + m_rightMotor.getConfigurator().apply(ElevatorConstants.kMotorConfig); + m_leftMotor.getConfigurator().apply(ElevatorConstants.kMotorConfig); + m_leftMotor + .setControl(new Follower(ElevatorConstants.kRightMotorID, ElevatorConstants.kInvertLeft)); + m_rightMotor.setPosition(0.0); + m_leftMotor.setPosition(0.0); + + m_CANrange = new CANrange(ElevatorConstants.kCANrangeID); + m_CANrange.getConfigurator().apply(ElevatorConstants.kCANrangeConfig); + + m_control = new DynamicMotionMagicVoltage( + 0, // no position setpoint yet + ElevatorConstants.kMaxSpeed, + ElevatorConstants.kMaxAcceleration, + ElevatorConstants.kMaxJerk); + + m_leftVoltageSignal = m_leftMotor.getMotorVoltage(); + m_rightVoltageSignal = m_rightMotor.getMotorVoltage(); + m_leftCurrentSignal = m_leftMotor.getSupplyCurrent(); + m_rightCurrentSignal = m_rightMotor.getSupplyCurrent(); + m_leftTempSignal = m_leftMotor.getDeviceTemp(); + m_rightTempSignal = m_rightMotor.getDeviceTemp(); + m_leftVelocitySignal = m_leftMotor.getVelocity(); + m_rightVelocitySignal = m_rightMotor.getVelocity(); + m_leftPositionSignal = m_leftMotor.getPosition(); + m_rightPositionSignal = m_rightMotor.getPosition(); + + m_CANrangeDetectedSignal = m_CANrange.getIsDetected(); + m_CANrangeDistanceSignal = m_CANrange.getDistance(); + m_CANrangeStrengthSignal = m_CANrange.getSignalStrength(); + + StatusSignalUtil.registerCANivoreSignals( + m_leftVoltageSignal, + m_rightVoltageSignal, + m_leftCurrentSignal, + m_rightCurrentSignal, + m_leftTempSignal, + m_rightTempSignal, + m_leftVelocitySignal, + m_rightVelocitySignal, + m_leftPositionSignal, + m_rightPositionSignal); + StatusSignalUtil.registerRioSignals( + m_CANrangeDetectedSignal, + m_CANrangeDistanceSignal, + m_CANrangeStrengthSignal); + } + + public void updateInputs(ElevatorIOInputs inputs) { + inputs.leftMotorConnected = BaseStatusSignal.isAllGood( + m_leftVoltageSignal, + m_leftCurrentSignal, + m_leftTempSignal, + m_leftVelocitySignal, + m_leftPositionSignal); + inputs.rightMotorConnected = BaseStatusSignal.isAllGood( + m_rightVoltageSignal, + m_rightCurrentSignal, + m_rightTempSignal, + m_rightVelocitySignal, + m_rightPositionSignal); + inputs.leftVoltage = m_leftVoltageSignal.getValueAsDouble(); + inputs.rightVoltage = m_rightVoltageSignal.getValueAsDouble(); + inputs.leftCurrent = m_leftCurrentSignal.getValueAsDouble(); + inputs.rightCurrent = m_rightCurrentSignal.getValueAsDouble(); + inputs.leftTemp = m_leftTempSignal.getValueAsDouble(); + inputs.rightTemp = m_rightTempSignal.getValueAsDouble(); + inputs.leftVelocityRPS = m_leftVelocitySignal.getValueAsDouble(); + inputs.rightVelocityRPS = m_rightVelocitySignal.getValueAsDouble(); + inputs.leftPosition = m_leftPositionSignal.getValueAsDouble(); + inputs.rightPosition = m_rightPositionSignal.getValueAsDouble(); + inputs.position = inputs.rightPosition; + + inputs.reference = m_reference; + + inputs.zeroCANrangeConnected = BaseStatusSignal.isAllGood( + m_CANrangeDetectedSignal, + m_CANrangeDistanceSignal, + m_CANrangeStrengthSignal); + inputs.zeroCANrangeDetected = m_CANrangeDetectedSignal.getValue(); + inputs.zeroCANrangeDistance = m_CANrangeDistanceSignal.getValueAsDouble(); + inputs.zeroCANrangeStrength = m_CANrangeStrengthSignal.getValueAsDouble(); + } + + public void setPosition(double reference) { + m_rightMotor.setControl(m_control.withPosition(reference)); + m_reference = reference; + } + + public void setVoltage(double voltage) { + m_rightMotor.setVoltage(voltage); + } + + public void enableLimits() { + m_rightMotor.getConfigurator().apply(ElevatorConstants.kMotorConfig.SoftwareLimitSwitch); + m_leftMotor.getConfigurator().apply(ElevatorConstants.kMotorConfig.SoftwareLimitSwitch); + } + + public void disableLimits() { + SoftwareLimitSwitchConfigs noLimits = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(false) + .withReverseSoftLimitEnable(false); + m_rightMotor.getConfigurator().apply(noLimits); + m_leftMotor.getConfigurator().apply(noLimits); + } + + public void calibrateZero() { + m_rightMotor.setPosition(0.0); + m_leftMotor.setPosition(0.0); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIOInputsLogger.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIOInputsLogger.java new file mode 100644 index 00000000..9adc7455 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIOInputsLogger.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems.elevator; + +import frc.robot.subsystems.elevator.ElevatorIO.ElevatorIOInputs; +import frc.robot.utils.OnboardLogger; + +public class ElevatorIOInputsLogger { + private final OnboardLogger log; + + public ElevatorIOInputsLogger(ElevatorIOInputs inputs) { + log = new OnboardLogger("Elevator"); + log.registerBoolean("Left Motor Connected", () -> inputs.leftMotorConnected); + log.registerBoolean("Right Motor Connected", () -> inputs.rightMotorConnected); + log.registerDouble("Left Voltage", () -> inputs.leftVoltage); + log.registerDouble("Right Voltage", () -> inputs.rightVoltage); + log.registerDouble("Left Current", () -> inputs.leftCurrent); + log.registerDouble("Right Current", () -> inputs.rightCurrent); + log.registerDouble("Left Temperature", () -> inputs.leftTemp); + log.registerDouble("Right Tempterature", () -> inputs.rightTemp); + log.registerDouble("Left Velocity RPS", () -> inputs.leftVelocityRPS); + log.registerDouble("Right Velocity RPS", () -> inputs.rightVelocityRPS); + log.registerDouble("Left Position", () -> inputs.leftPosition); + log.registerDouble("Right Position", () -> inputs.rightPosition); + log.registerDouble("Position", () -> inputs.position); + log.registerDouble("Reference", () -> inputs.reference); + log.registerBoolean("Zero CANrange Connected", () -> inputs.zeroCANrangeConnected); + log.registerBoolean("Zero CANrange Detected", () -> inputs.zeroCANrangeDetected); + log.registerDouble("Zero CANrange Distance", () -> inputs.zeroCANrangeDistance); + log.registerDouble("Zero CANrange Strength", () -> inputs.zeroCANrangeStrength); + } + + public void log() { + log.log(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java new file mode 100644 index 00000000..95877ab5 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.elevator; + +public class ElevatorIOSim implements ElevatorIO { + private double m_position = 0; + private double m_reference = 0; + + public void updateInputs(ElevatorIOInputs inputs) { + m_position = 0.9 * m_position + 0.1 * m_reference; + inputs.position = m_position; + inputs.reference = m_reference; + inputs.zeroCANrangeDetected = m_position < 1e-3; + } + + public void setPosition(double position) { + m_reference = position; // that's an awesome elevator! it's instant! + } + + public void setVoltage(double voltage) {} + + public void enableLimits() {} + public void disableLimits() {} + + public void calibrateZero() {} + +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java new file mode 100644 index 00000000..1df67e79 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.elevator; + +public enum ElevatorState { + /** Elevator at lowest position */ + Zero(0), + /** Elevator at ground algae intake height*/ + Ground(0), + /** Height for ground algae intake */ + HighGround(0.60), + /** Regular "home" position - also intake position */ + Stow(0.31), + /** A little higher than stow to eject a coral */ + Eject(ElevatorState.Stow.position() + 2 * ElevatorConstants.kInch), + /** Height to score processor */ + Processor(0), + /** L1 height */ + L1(2.63), + /** Secondary L1 height for when a coral is already present */ + SecondaryL1(ElevatorState.L1.position() + 8 * ElevatorConstants.kInch), + /** L2 height */ + L2(4.016 + 3 * ElevatorConstants.kInch), + /** L3 height */ + L3(7.257 - 4 * ElevatorConstants.kInch), + /** L4 height */ + L4(9.757 + 0.3 * ElevatorConstants.kInch), + /** Height to score net */ + Net(9.31 + 4 * ElevatorConstants.kInch), + /** Height to intake algae from lower reef */ + LowerReef(2.0), + /** Height to intake algae from upper reef */ + UpperReef(4.5); + + private final double m_position; + + private ElevatorState(double position) { + m_position = position; + } + + protected double position() { + return m_position; + } +} + diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/Pivot.java b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/Pivot.java new file mode 100644 index 00000000..aefbf2f6 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/Pivot.java @@ -0,0 +1,80 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.pivot; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Robot; +import frc.robot.RobotObserver; +import frc.robot.subsystems.PassiveSubsystem; +import frc.robot.subsystems.pivot.PivotIO.PivotIOInputs; +import frc.robot.utils.LoopTimer; +import frc.robot.utils.OnboardLogger; + +public class Pivot extends PassiveSubsystem { + @SuppressWarnings("unused") + private final Logger m_logger = LoggerFactory.getLogger(Pivot.class); + private final OnboardLogger m_ologger; + + private final LoopTimer m_timer; + + private final PivotIO m_io; + private final PivotIOInputs m_inputs; + private final PivotIOInputsLogger m_inputsLogger; + + private PivotState m_reference; + + public Pivot() { + super(); + if (Robot.isReal()) { + m_io = new PivotIOHardware(); + } else { + m_io = new PivotIOSim(); + } + m_inputs = new PivotIOInputs(); + m_inputsLogger = new PivotIOInputsLogger(m_inputs); + m_reference = PivotState.Stow; + m_ologger = new OnboardLogger("Pivot"); + m_ologger.registerString("State", () -> m_reference.toString()); + m_ologger.registerBoolean("Ready", ready()); + m_timer = new LoopTimer("Pivot"); + } + + + private void setPosition(PivotState state) { + take(); + m_io.setPosition(state.position(), RobotObserver.getAlgaePieceHeld()); + m_reference = state; + } + + public Trigger ready() { + return new Trigger(() -> Math.abs(m_reference.position() - m_inputs.position) < PivotConstants.kTolerance); + } + + @Override + public void periodic() { + m_timer.reset(); + m_io.updateInputs(m_inputs); + m_inputsLogger.log(); + m_ologger.log(); + m_timer.log(); + } + + protected void passive() { + setPosition(PivotState.Stow); + } + + /** + * Drives the pivot to the given PivotState This command ends when the state is reached. + */ + public Command go(PivotState state) { + return Commands.sequence( + runOnce(() -> setPosition(state)), + Commands.waitUntil(ready())); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotConstants.java new file mode 100644 index 00000000..8ac291ac --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotConstants.java @@ -0,0 +1,90 @@ +package frc.robot.subsystems.pivot; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.LinearSystemId; +import frc.robot.Constants.TalonFXConstants; + +public final class PivotConstants { + protected static final int kMotorID = 57; + + protected static final double kRotorOffset = 0.344; + + protected static final double kRotorToSensorRatio = 64.0 / 14.0; + protected static final double kSensorToMechanismRatio = 32.0 / 14.0; + + protected static final double kForwardSoftLimit = 0.359; + protected static final double kReverseSoftLimit = 0.0; + + protected static final double kSupplyCurrentLimit = 40; + + protected static final double kTolerance = 0.03; + + protected static final double kMomentOfIntertia = 0.14622; + protected static final double kGearRatio = kRotorToSensorRatio * kSensorToMechanismRatio; + + protected static final LinearSystem kPlant = LinearSystemId + .createSingleJointedArmSystem(TalonFXConstants.TalonFXDCMotor, kMomentOfIntertia, kGearRatio); + + protected static final double kMaxSpeed = 1.5; // cancoder rotations per secon + protected static final double kMaxAcceleration = 3.0; + protected static final double kMaxJerk = 30.0; + + protected static final TalonFXConfiguration kMotorConfig = new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive)) + + .withFeedback(new FeedbackConfigs() + .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) + .withSensorToMechanismRatio(kGearRatio)) + + .withCurrentLimits(new CurrentLimitsConfigs() + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(kSupplyCurrentLimit)) + + .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitThreshold(kForwardSoftLimit) + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitThreshold(kReverseSoftLimit) + .withReverseSoftLimitEnable(true)) + + .withSlot0(new Slot0Configs() + .withGravityType(GravityTypeValue.Arm_Cosine) + .withKP(25) + .withKI(0) + .withKD(0) + .withKS(0) + .withKV(1.3) + .withKA(0.12) + .withKG(0.625)) + .withSlot1(new Slot1Configs() + .withGravityType(GravityTypeValue.Arm_Cosine) + .withKP(30) + .withKI(0) + .withKD(0) + .withKS(0) + .withKV(1.3) + .withKA(0.12) + .withKG(0.85)) + + .withMotionMagic(new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(kMaxSpeed) + .withMotionMagicAcceleration(kMaxAcceleration) + .withMotionMagicJerk(kMaxJerk)); + + protected static final double kArmLength = 0.443; +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIO.java new file mode 100644 index 00000000..6aefe954 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIO.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.pivot; + +public interface PivotIO { + void updateInputs(PivotIOInputs inputs); + + class PivotIOInputs { + public boolean motorConnected = true; + public double current = 0.0; + public double voltage = 0.0; + public double temperatue = 0.0; + public double position = 0.0; + } + + void setPosition(double position, boolean holdingAlgae); +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIOHardware.java b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIOHardware.java new file mode 100644 index 00000000..2e5ffe5b --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIOHardware.java @@ -0,0 +1,62 @@ +package frc.robot.subsystems.pivot; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.utils.StatusSignalUtil; + +public class PivotIOHardware implements PivotIO { + private final TalonFX m_motor; + + private final MotionMagicVoltage m_control; + + private final StatusSignal m_voltageSignal; + private final StatusSignal m_currentSignal; + private final StatusSignal m_tempSignal; + private final StatusSignal m_velocitySignal; + private final StatusSignal m_positionSignal; + + public PivotIOHardware() { + m_motor = new TalonFX(PivotConstants.kMotorID); + m_motor.getConfigurator().apply(PivotConstants.kMotorConfig); + m_motor.setPosition(PivotConstants.kRotorOffset); + + m_voltageSignal = m_motor.getMotorVoltage(); + m_currentSignal = m_motor.getSupplyCurrent(); + m_tempSignal = m_motor.getDeviceTemp(); + m_velocitySignal = m_motor.getVelocity(); + m_positionSignal = m_motor.getPosition(); + + m_control = new MotionMagicVoltage(0.0); + + StatusSignalUtil.registerRioSignals( + m_voltageSignal, + m_currentSignal, + m_tempSignal, + m_velocitySignal, + m_positionSignal); + } + + public void updateInputs(PivotIOInputs inputs) { + inputs.motorConnected = BaseStatusSignal.isAllGood( + m_voltageSignal, + m_currentSignal, + m_tempSignal, + m_velocitySignal, + m_positionSignal); + inputs.voltage = m_voltageSignal.getValueAsDouble(); + inputs.current = m_currentSignal.getValueAsDouble(); + inputs.temperatue = m_tempSignal.getValueAsDouble(); + inputs.position = m_positionSignal.getValueAsDouble(); + } + + public void setPosition(double position, boolean holdingAlgae) { + m_motor.setControl(m_control.withPosition(position).withSlot(holdingAlgae ? 1 : 0)); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIOInputsLogger.java b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIOInputsLogger.java new file mode 100644 index 00000000..1b8b9fb8 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIOInputsLogger.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.pivot; + +import frc.robot.subsystems.pivot.PivotIO.PivotIOInputs; +import frc.robot.utils.OnboardLogger; + +public class PivotIOInputsLogger { + private final OnboardLogger log; + + public PivotIOInputsLogger(PivotIOInputs inputs) { + log = new OnboardLogger("Pivot"); + log.registerBoolean("Motor Connected", () -> inputs.motorConnected); + log.registerDouble("Current", () -> inputs.current); + log.registerDouble("Voltage", () -> inputs.voltage); + log.registerDouble("Temperature", () -> inputs.temperatue); + log.registerDouble("Position", () -> inputs.position); + } + + public void log() { + log.log(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java new file mode 100644 index 00000000..60c9e362 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.pivot; + +public class PivotIOSim implements PivotIO { + private double m_position = 0; + private double m_reference = 0; + + public void updateInputs(PivotIOInputs inputs) { + m_position = (m_position + m_reference) / 2.0; + inputs.position = m_position; + } + + public void setPosition(double position, boolean holdingAlgae) { + m_reference = position; + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotState.java b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotState.java new file mode 100644 index 00000000..cad2f8dd --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotState.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.pivot; + +public enum PivotState { + /** The pivot angle for intaking algae off the ground */ + Ground(0.0669), + /** The angle for scoring in the processor */ + Processor(0.085), + /** The angle for intaking algae off the reef */ + ReefIntake(0.2), + /** The angle for removing algae off the reef after intake */ + ReefExtract(0.281), + /** The angle to score at the net */ + Net(0.342), + /** The "home" angle for the pivot */ + Stow(0.343), + /** The angle for lollipop intake */ + HighGround(0.15); + + private double m_position; + + private PivotState(double position) { + m_position = position; + } + + protected double position() { + return m_position; + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/EnterableState.java b/ThriftyTest/src/main/java/frc/robot/superstructure/EnterableState.java new file mode 100644 index 00000000..c190db60 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/EnterableState.java @@ -0,0 +1,18 @@ +package frc.robot.superstructure; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.Superstructure.Subsystems; + +/** + * A robot has a lot of possible states. This class represents a single one. + * + * A state is a possible configuration of the robot. The EnterableState has a method + * build(subsystems) which builds a command that, if possible, directs the robot to + * enter the state that the class represents. + */ +public interface EnterableState { + /** + * Builds a command to enter this state. + */ + Command build(Subsystems subsystems); +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/PassiveModifier.java b/ThriftyTest/src/main/java/frc/robot/superstructure/PassiveModifier.java new file mode 100644 index 00000000..7e9440cf --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/PassiveModifier.java @@ -0,0 +1,8 @@ +package frc.robot.superstructure; + +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.superstructure.Superstructure.Subsystems; + +public interface PassiveModifier { + void modify(Subsystems subsystems, Trigger trigger); +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/Superstructure.java b/ThriftyTest/src/main/java/frc/robot/superstructure/Superstructure.java new file mode 100644 index 00000000..84f3ceca --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/Superstructure.java @@ -0,0 +1,134 @@ +package frc.robot.superstructure; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.RobotObserver; +import frc.robot.subsystems.LedFeedback; +import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; +import frc.robot.subsystems.algae.Algae; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.coral.Coral; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.pivot.Pivot; +import frc.robot.vision.tracking.AlgaeTracker; +import frc.robot.vision.localization.AprilTagVisionHandler; + +public class Superstructure { + private final Subsystems m_subsystems; + + /** + * Constructs a new superstructure given the individual subsystems + */ + public Superstructure( + Algae algae, + Coral coral, + Pivot pivot, + Elevator elevator, + Climber climber, + CommandSwerveDrivetrain drivetrain, + LedFeedback leds) { + + m_subsystems = new Subsystems( + algae, + coral, + pivot, + elevator, + climber, + drivetrain, + leds); + + drivetrain.setTippyTrigger(tippy()); + + RobotObserver.setFFEnabledSupplier(elevator.unsafe().and(() -> !DriverStation.isAutonomous())); + } + + /** + * Sets a specified EnterableState as reference state. This also sets that command to + * run as a proxied command. + */ + public Command enter(EnterableState state) { + return state.build(m_subsystems) + .withName(state.getClass().getSimpleName()) + .asProxy(); + } + + /** + * The same thing as enter(), except this is NOT a proxied command. This should be + * used for default commands, where the command needs to explicity list its subsystems. However, + * other than that, there aren't many uses for this method, so use with care!. + */ + public Command enterWithoutProxy(EnterableState state) { + return state.build(m_subsystems) + .withName(state.getClass().getSimpleName()); // avoid poorly named commands + } + + /** + * Uses a PassiveModifier to change passive behavior + */ + public void modify(PassiveModifier modifier, Trigger trigger) { + modifier.modify(m_subsystems, trigger); + } + + /** + * Sets the provided command to be a default command for the drivetrain + */ + public void setDrive(Command driveCommand) { + m_subsystems.drivetrain().setDefaultCommand(driveCommand); + } + + /** + * Returns an AprilTagVisionHandler. + */ + public AprilTagVisionHandler buildVision() { + return new AprilTagVisionHandler( + m_subsystems.drivetrain()::getPose, + m_subsystems.drivetrain()::addPoseEstimate); + } + + public static record Subsystems( + Algae algae, + Coral coral, + Pivot pivot, + Elevator elevator, + Climber climber, + CommandSwerveDrivetrain drivetrain, + LedFeedback leds) { + } + + public Trigger aligned() { + return m_subsystems.drivetrain().aligned(); + } + + public Trigger inReefZone() { + return m_subsystems.drivetrain().inReefZone(); + } + + public Trigger holdingCoral() { + return m_subsystems.coral().held(); + } + + public Trigger holdingAlgae() { + return m_subsystems.algae.holdingAlgae(); + } + + public Trigger tippy() { + return m_subsystems.elevator.unsafe().or(m_subsystems.algae.holdingAlgae()); + } + + /** + * Returns a runnable that can be used for tracking algae + * + * If algae tracking is disabled, then this Runnable does nothing. + */ + public Runnable buildAlgaeTracker() { + if (AlgaeTracker.enabled) { + return new AlgaeTracker( + m_subsystems.drivetrain()::getPose, + m_subsystems.drivetrain()::addObjectTrackingData); + } else { + return () -> { + }; + } + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/modifiers/AlgaeElevatorHold.java b/ThriftyTest/src/main/java/frc/robot/superstructure/modifiers/AlgaeElevatorHold.java new file mode 100644 index 00000000..e5c41fee --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/modifiers/AlgaeElevatorHold.java @@ -0,0 +1,18 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.superstructure.modifiers; + +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.superstructure.PassiveModifier; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class AlgaeElevatorHold implements PassiveModifier { + /** Tells the elevator not to move when holding algae and in reef zone */ + public AlgaeElevatorHold() {} + + public void modify(Subsystems subsystems, Trigger trigger) { + subsystems.elevator().setStayRequirement(trigger); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/modifiers/ElevatorPrep.java b/ThriftyTest/src/main/java/frc/robot/superstructure/modifiers/ElevatorPrep.java new file mode 100644 index 00000000..46e4dbf1 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/modifiers/ElevatorPrep.java @@ -0,0 +1,16 @@ +package frc.robot.superstructure.modifiers; + +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.superstructure.PassiveModifier; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class ElevatorPrep implements PassiveModifier { + /** + * Modifies the behavior of the elevator's prefire. + */ + public ElevatorPrep() {} + + public void modify(Subsystems subsystems, Trigger trigger) { + subsystems.elevator().setPrefireRequirement(trigger); + } + } diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/AlgaeStow.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/AlgaeStow.java new file mode 100644 index 00000000..2fe7b996 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/AlgaeStow.java @@ -0,0 +1,25 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class AlgaeStow implements EnterableState { + /** + * A state that holds an algae in a disable-safe position. What this means is that if the robot is + * disabled while in this state, an algae present will not fall out. + */ + public AlgaeStow() {} + + public Command build(Subsystems subsystems) { + return Commands.parallel( + subsystems.elevator().go(ElevatorState.Stow), + subsystems.pivot().go(PivotState.Ground)) + + .onlyIf(subsystems.algae().holdingAlgae()); + // Neither subsystem is released because this is a "persistent" state. + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Align.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Align.java new file mode 100644 index 00000000..3c028779 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Align.java @@ -0,0 +1,67 @@ +package frc.robot.superstructure.states; + +import java.util.Set; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.DeferredCommand; +import com.therekrab.autopilot.APTarget; +import com.therekrab.autopilot.Autopilot; +import frc.robot.subsystems.drivetrain.DriveConstants; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; +import frc.robot.utils.FieldUtils; + +public class Align implements EnterableState { + @SuppressWarnings("unused") + private final Logger m_logger = LoggerFactory.getLogger(Align.class); + + private Autopilot m_autopilot; + private final APTarget m_target; + private boolean m_flip; + + /** + * A state that controls the drivetrain and drives to a certain, given point on the field + */ + public Align(Autopilot autopilot, APTarget target) { + m_autopilot = autopilot; + m_target = target; + } + + /** + * A state in which the robot is aligned with a given target + * + * This approach uses the tight Autopilot configuration from DriveConstants + */ + public Align(APTarget target) { + m_autopilot = DriveConstants.kTightAutopilot; + m_target = target; + } + + public Command build(Subsystems subsystems) { + return new DeferredCommand(() -> subsystems.drivetrain().align(m_autopilot, target()), + Set.of(subsystems.drivetrain())); + } + + public Align allianceRelative() { + m_flip = true; + return this; + } + + public Align fast() { + m_autopilot = DriveConstants.kFastAutopilot; + return this; + } + + public Align slow() { + m_autopilot = DriveConstants.kSlowAutopilot; + return this; + } + + private APTarget target() { + if (m_flip) { + return FieldUtils.flipTargetConditionally(m_target); + } + return m_target; + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Climb.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Climb.java new file mode 100644 index 00000000..596cc3b4 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Climb.java @@ -0,0 +1,16 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class Climb implements EnterableState { + /** + * A state in which the robot has lowered the climber to the climb setpoint + */ + public Climb() {} + + public Command build(Subsystems subsystems) { + return subsystems.climber().climb(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/ClimbStowed.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ClimbStowed.java new file mode 100644 index 00000000..94299bf5 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ClimbStowed.java @@ -0,0 +1,15 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class ClimbStowed { + /** + * A state that represents the climber at its "stow" position + */ + public ClimbStowed() {} + + public Command build(Subsystems subsystems) { + return subsystems.climber().lower(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CompleteCoralIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CompleteCoralIntake.java new file mode 100644 index 00000000..a35634cc --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CompleteCoralIntake.java @@ -0,0 +1,21 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class CompleteCoralIntake implements EnterableState { + /** + * A state to complete a coral intake if it has been started
+ *
+ * This is different from the passive behavior on the coral subsystem itself because it also + * requires the elevator, which makes it more of a "blocking" operation. + */ + public CompleteCoralIntake() {} + + public Command build(Subsystems subsystems) { + return new CoralIntake().build(subsystems) + .finallyDo(subsystems.coral()::release) + .onlyIf(subsystems.coral().present()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CompleteCoralScore.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CompleteCoralScore.java new file mode 100644 index 00000000..6187a509 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CompleteCoralScore.java @@ -0,0 +1,22 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.CoralLevel; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class CompleteCoralScore implements EnterableState { + private final CoralLevel m_level; + + /** + * A state that completes a coral intake cycle, if the robot is actively ready. + */ + public CompleteCoralScore(CoralLevel level) { + m_level = level; + } + + public Command build(Subsystems subsystems) { + return new CoralScore(m_level).build(subsystems) + .onlyIf(subsystems.elevator().ready(m_level)); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralEject.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralEject.java new file mode 100644 index 00000000..080b7107 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralEject.java @@ -0,0 +1,24 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class CoralEject implements EnterableState { + /** + * A state that represents a present coral being ejected + */ + public CoralEject() {} + + public Command build(Subsystems subsystems) { + return Commands.sequence( + subsystems.elevator().go(ElevatorState.Eject), + subsystems.coral().eject()) + + .finallyDo(subsystems.elevator()::release) + .finallyDo(subsystems.coral()::release) + .onlyIf(subsystems.coral().present()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralIntake.java new file mode 100644 index 00000000..c6ff5842 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralIntake.java @@ -0,0 +1,24 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class CoralIntake implements EnterableState { + /** + * A state to intake coral + */ + public CoralIntake() {} + + public Command build(Subsystems subsystems) { + return Commands.sequence( + subsystems.elevator().go(ElevatorState.Stow), + subsystems.coral().intake()) + + .finallyDo(subsystems.elevator()::release) + .finallyDo(subsystems.coral()::release) + .unless(subsystems.coral().held()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralScore.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralScore.java new file mode 100644 index 00000000..16ba3636 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralScore.java @@ -0,0 +1,28 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.CoralLevel; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class CoralScore implements EnterableState { + private CoralLevel m_level; + + /** + * A state that scores a coral on a specified level on the reef. + */ + public CoralScore(CoralLevel level) { + m_level = level; + } + + public Command build(Subsystems subsystems) { + return Commands.sequence( + subsystems.elevator().go(m_level), + subsystems.coral().score(m_level)) + + .finallyDo(subsystems.elevator()::release) + .finallyDo(subsystems.coral()::release) + .onlyIf(subsystems.coral().held()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralScorePrep.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralScorePrep.java new file mode 100644 index 00000000..1860646a --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralScorePrep.java @@ -0,0 +1,23 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.CoralLevel; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class CoralScorePrep implements EnterableState { + private final CoralLevel m_level; + + /** + * A state that prepares the robot to score at the desired level + */ + public CoralScorePrep(CoralLevel level) { + m_level = level; + } + + public Command build(Subsystems subsystems) { + return subsystems.elevator().go(m_level) + .finallyDo(subsystems.elevator()::conditionalRelease) + .onlyIf(subsystems.coral().held()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralWait.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralWait.java new file mode 100644 index 00000000..aae780b3 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralWait.java @@ -0,0 +1,18 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class CoralWait implements EnterableState { + /** + * A state that puts the robot into a "waiting" state until a coral can be acquired. + * The state is NOT a coral itake state, however, and does not actually change any mechanisms + */ + public CoralWait() {} + + public Command build(Subsystems subsystems) { + return Commands.waitUntil(subsystems.coral().present()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java new file mode 100644 index 00000000..b3adfd06 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java @@ -0,0 +1,66 @@ +package frc.robot.superstructure.states; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.Set; +import java.util.stream.Collectors; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.ScoringLocationsCenter; +import frc.robot.Constants.ScoringLocationsLeft; +import frc.robot.Constants.ScoringLocationsRight; +import com.therekrab.autopilot.APTarget; +import frc.robot.subsystems.drivetrain.DriveConstants; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; +import frc.robot.utils.FieldUtils; + +public class DeferredAlign implements EnterableState { + private List m_locations; + + /** + * Represents a state where the robot aligns to a nonstatic location and then waits there + */ + public DeferredAlign(AlignLocation side) { + switch (side) { + case Left -> { + m_locations = Arrays.stream(ScoringLocationsLeft.values()) + .map(location -> location.value) + .collect(Collectors.toList()); + } + case Right -> { + m_locations = Arrays.stream(ScoringLocationsRight.values()) + .map(location -> location.value) + .collect(Collectors.toList()); + } + case Center -> { + m_locations = Arrays.stream(ScoringLocationsCenter.values()) + .map(location -> location.value) + .collect(Collectors.toList()); + } + case Intake -> { + m_locations = List.of(FieldConstants.kLeftIntake, FieldConstants.kRightIntake); + } + } + } + + public Command build(Subsystems subsystems) { + return Commands.defer(() -> { + List locations = new ArrayList<>(); + m_locations.forEach(location -> locations.add(FieldUtils.getLocalPose(location))); + Pose2d closest = subsystems.drivetrain().getPose().nearest(locations); + APTarget target = new APTarget(closest) + .withEntryAngle(closest.getRotation()); + return Commands.sequence( + subsystems.drivetrain().align(DriveConstants.kTightAutopilot, target), + Commands.idle(subsystems.drivetrain())); + }, Set.of(subsystems.drivetrain())); + } + + public enum AlignLocation { + Left, Right, Center, Intake; + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/ElevatorZero.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ElevatorZero.java new file mode 100644 index 00000000..18fdcb7b --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ElevatorZero.java @@ -0,0 +1,20 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorConstants; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class ElevatorZero implements EnterableState { + /** + * A state that calibrates the elevator to its zero position + */ + public ElevatorZero() {} + + public Command build(Subsystems subsystems) { + return subsystems.elevator().autoZero() + .withTimeout(ElevatorConstants.kCalibrationTime) + .finallyDo(subsystems.elevator()::release) + .unless(subsystems.coral().held()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/GroundAlgaeIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/GroundAlgaeIntake.java new file mode 100644 index 00000000..a349122f --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/GroundAlgaeIntake.java @@ -0,0 +1,27 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class GroundAlgaeIntake implements EnterableState { + /** + * A state to intake algae from the ground + */ + public GroundAlgaeIntake() {} + + public Command build(Subsystems subsystems) { + return Commands.parallel( + subsystems.elevator().go(ElevatorState.Ground), + subsystems.pivot().go(PivotState.Ground), + subsystems.algae().intake()) + + .finallyDo(subsystems.elevator()::release) + .finallyDo(subsystems.pivot()::release) + .finallyDo(subsystems.algae()::release) + .unless(subsystems.algae().holdingAlgae()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/HeadingReset.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/HeadingReset.java new file mode 100644 index 00000000..5010fe26 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/HeadingReset.java @@ -0,0 +1,16 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class HeadingReset implements EnterableState { + /** + * An instant state that resets the operator forward perspective for teleop control + */ + public HeadingReset() {} + + public Command build(Subsystems subsystems) { + return subsystems.drivetrain().resetHeading(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/HighGroundAlgaeIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/HighGroundAlgaeIntake.java new file mode 100644 index 00000000..59f751d9 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/HighGroundAlgaeIntake.java @@ -0,0 +1,29 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class HighGroundAlgaeIntake implements EnterableState { + /** + * A state to intake algae off the "lollipops" + */ + public HighGroundAlgaeIntake() {} + + public Command build(Subsystems subsystems) { + return Commands.sequence( + subsystems.elevator().go(ElevatorState.HighGround), + Commands.parallel( + subsystems.pivot().go(PivotState.HighGround), + subsystems.algae().intake())) + + .finallyDo(subsystems.elevator()::release) + .finallyDo(subsystems.pivot()::release) + .finallyDo(subsystems.algae()::release) + .unless(subsystems.algae().holdingAlgae()); + } +} + diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/IntakeComplete.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/IntakeComplete.java new file mode 100644 index 00000000..a07cb68e --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/IntakeComplete.java @@ -0,0 +1,20 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class IntakeComplete implements EnterableState { + /** + * A state that waits for a coral to complete intaking as long as there is a coral in the robot + * + * This command will end if a coral is no longer detected. + */ + public IntakeComplete() {} + + public Command build(Subsystems subsystems) { + return Commands.waitUntil(subsystems.coral().held()) + .onlyWhile(subsystems.coral().present()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java new file mode 100644 index 00000000..5d4e9e41 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java @@ -0,0 +1,29 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class LowerReefAlgaeIntake implements EnterableState { + /** + * A state to intake the algae off of the reef + */ + public LowerReefAlgaeIntake() {} + + public Command build(Subsystems subsystems) { + return Commands.sequence( + subsystems.elevator().go(ElevatorState.LowerReef), + Commands.parallel( + subsystems.pivot().go(PivotState.ReefIntake), + subsystems.algae().intake()), + subsystems.pivot().go(PivotState.ReefExtract)) + + .finallyDo(subsystems.elevator()::release) + .finallyDo(subsystems.algae()::release) + .finallyDo(subsystems.pivot()::release) + .unless(subsystems.algae().holdingAlgae()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java new file mode 100644 index 00000000..b0130924 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java @@ -0,0 +1,28 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class Net implements EnterableState { + /** + * A state that scores an algae in the net + */ + public Net() {} + + public Command build(Subsystems subsystems) { + return Commands.sequence( + Commands.parallel( + subsystems.elevator().go(ElevatorState.Net), + subsystems.pivot().go(PivotState.Net)), + subsystems.algae().net()) + + .finallyDo(subsystems.elevator()::release) + .finallyDo(subsystems.pivot()::release) + .finallyDo(subsystems.algae()::release) + .onlyIf(subsystems.algae().holdingAlgae()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/NetPrep.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/NetPrep.java new file mode 100644 index 00000000..d5a7a453 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/NetPrep.java @@ -0,0 +1,25 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class NetPrep implements EnterableState { + /** + * A state that sets the robot up to score in the net + */ + public NetPrep() {} + + public Command build(Subsystems subsystems) { + return Commands.parallel( + subsystems.elevator().go(ElevatorState.Net), + subsystems.pivot().go(PivotState.Net)) + + .finallyDo(subsystems.elevator()::conditionalRelease) + .finallyDo(subsystems.pivot()::conditionalRelease) + .onlyIf(subsystems.algae().holdingAlgae()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/OpenFunnel.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/OpenFunnel.java new file mode 100644 index 00000000..ee953534 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/OpenFunnel.java @@ -0,0 +1,16 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class OpenFunnel implements EnterableState { + /** + * A state that represents the funnel in the open state + */ + public OpenFunnel() {} + + public Command build(Subsystems subsystems) { + return subsystems.climber().openFunnel(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Processor.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Processor.java new file mode 100644 index 00000000..224c2d68 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Processor.java @@ -0,0 +1,28 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class Processor implements EnterableState { + /** + * A state that scores an algae in the processor + */ + public Processor() {} + + public Command build(Subsystems subsystems) { + return Commands.sequence( + Commands.parallel( + subsystems.elevator().go(ElevatorState.Processor), + subsystems.pivot().go(PivotState.Processor)), + subsystems.algae().processorScore()) + + .finallyDo(subsystems.elevator()::release) + .finallyDo(subsystems.pivot()::release) + .finallyDo(subsystems.algae()::release) + .onlyIf(subsystems.algae().holdingAlgae()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/ProcessorPrep.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ProcessorPrep.java new file mode 100644 index 00000000..572c40ed --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ProcessorPrep.java @@ -0,0 +1,25 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class ProcessorPrep implements EnterableState { + /** + * A state that prepares the robot for a processor score + */ + public ProcessorPrep() {} + + public Command build(Subsystems subsystems) { + return Commands.parallel( + subsystems.elevator().go(ElevatorState.Processor), + subsystems.pivot().go(PivotState.Processor)) + + .finallyDo(subsystems.elevator()::conditionalRelease) + .finallyDo(subsystems.pivot()::conditionalRelease) + .onlyIf(subsystems.algae().holdingAlgae()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/RaiseClimb.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/RaiseClimb.java new file mode 100644 index 00000000..1c0e2d64 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/RaiseClimb.java @@ -0,0 +1,16 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class RaiseClimb implements EnterableState { + /** + * A state with the climber at the raised position + */ + public RaiseClimb() {} + + public Command build(Subsystems subsystems) { + return subsystems.climber().raise(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/SeedPose.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/SeedPose.java new file mode 100644 index 00000000..82c77a08 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/SeedPose.java @@ -0,0 +1,34 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.FieldConstants; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class SeedPose implements EnterableState { + private final Pose2d m_pose; + + /** + * Sets the robot's position to the center starting position + */ + public SeedPose(Pose2d pose) { + m_pose = pose; + } + + public Command build(Subsystems subsystems) { + return subsystems.drivetrain().seedLocal(m_pose); + } + + public static SeedPose center() { + return new SeedPose(FieldConstants.kStartCenter); + } + + public static SeedPose left() { + return new SeedPose(FieldConstants.kStartLeft); + } + + public static SeedPose right() { + return new SeedPose(FieldConstants.kStartRight); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Stow.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Stow.java new file mode 100644 index 00000000..72d8bb74 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Stow.java @@ -0,0 +1,22 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class Stow implements EnterableState { + /** + * Stows all the stowables on the robot + */ + public Stow() {} + + public Command build(Subsystems subsystems) { + return Commands.parallel( + subsystems.elevator().go(ElevatorState.Stow), + subsystems.pivot().go(PivotState.Stow)); + } +} + diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/TeleopDrive.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/TeleopDrive.java new file mode 100644 index 00000000..8acb222f --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/TeleopDrive.java @@ -0,0 +1,25 @@ +package frc.robot.superstructure.states; + +import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class TeleopDrive implements EnterableState { + private final DoubleSupplier m_x; + private final DoubleSupplier m_y; + private final DoubleSupplier m_rot; + + /** + * A state that is used to drive the robot given suppliers for x, y, and rotational velocities. + */ + public TeleopDrive(DoubleSupplier x, DoubleSupplier y, DoubleSupplier rot) { + m_x = x; + m_y = y; + m_rot = rot; + } + + public Command build(Subsystems subsystems) { + return subsystems.drivetrain().teleopDrive(m_x, m_y, m_rot); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Test.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Test.java new file mode 100644 index 00000000..6cef5536 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Test.java @@ -0,0 +1,22 @@ +package frc.robot.superstructure.states; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class Test implements EnterableState { + @SuppressWarnings("unused") + private final Logger m_logger = LoggerFactory.getLogger(Test.class); + + /** + * A test state + */ + public Test() {} + + public Command build(Subsystems subsystems) { + return Commands.none(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/TrackAlgae.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/TrackAlgae.java new file mode 100644 index 00000000..0e8a9c66 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/TrackAlgae.java @@ -0,0 +1,11 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class TrackAlgae implements EnterableState { + public Command build(Subsystems subsystems) { + return subsystems.drivetrain().followObject(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java new file mode 100644 index 00000000..c3e9edae --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java @@ -0,0 +1,28 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class UpperReefAlgaeIntake implements EnterableState { + /** + * A state that intakes algae off of the higher level of the reef + */ + public UpperReefAlgaeIntake() {} + + public Command build(Subsystems subsystems) { + return Commands.sequence( + subsystems.elevator().go(ElevatorState.UpperReef), + subsystems.pivot().go(PivotState.ReefIntake), + subsystems.algae().intake(), + subsystems.pivot().go(PivotState.ReefExtract)) + + .finallyDo(subsystems.pivot()::release) + .finallyDo(subsystems.elevator()::release) + .finallyDo(subsystems.algae()::release) + .unless(subsystems.algae().holdingAlgae()); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/utils/AutonomousUtil.java b/ThriftyTest/src/main/java/frc/robot/utils/AutonomousUtil.java deleted file mode 100644 index d3e91470..00000000 --- a/ThriftyTest/src/main/java/frc/robot/utils/AutonomousUtil.java +++ /dev/null @@ -1,160 +0,0 @@ -package frc.robot.utils; - -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.function.Supplier; - -import org.json.simple.parser.ParseException; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.IdealStartingState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.path.Waypoint; -import com.pathplanner.lib.util.PathPlannerLogging; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.AutonConstants; -import frc.robot.Constants.DriveConstants; -import frc.robot.RobotContainer; -import frc.robot.RobotObserver; -import frc.robot.commands.DriveToPointCommand; -import frc.robot.subsystems.CommandSwerveDrivetrain; - -public class AutonomousUtil { - @SuppressWarnings("unused") - private static final Logger m_logger = LoggerFactory.getLogger(RobotContainer.class); - - private static CommandSwerveDrivetrain m_drivetrain; - - public static void initializePathPlanner(CommandSwerveDrivetrain drivetrain) { - RobotConfig config; - try { - config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - drivetrain::getPose, // Robot pose supplier - drivetrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) - drivetrain::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> drivetrain.driveWithChassisSpeeds(speeds), - DriveConstants.k_pathplannerHolonomicDriveController, - config, // The robot configuration - () -> { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - drivetrain); // Reference to this subsystem to set requirements - - drivetrain.initializeSetpointGenerator(config); - - m_drivetrain = drivetrain; - - PathPlannerLogging.setLogActivePathCallback(poses -> RobotObserver.getField().getObject("Pathfind Trajectory").setPoses(poses)); - } catch (IOException | ParseException e) { - e.printStackTrace(); - System.exit(1); - } - } - - private static final PathConstraints pathFindConstraints = new PathConstraints(DriveConstants.k_maxLinearSpeed, DriveConstants.k_maxLinearAcceleration, DriveConstants.k_maxAngularSpeed, DriveConstants.k_maxAngularAcceleration); - private static final PathConstraints finalAlignConstraints = new PathConstraints(DriveConstants.k_maxAlignLinearSpeed, DriveConstants.k_maxAlignLinearAcceleration, DriveConstants.k_maxAlignAngularSpeed, DriveConstants.k_maxAlignAngularAcceleration); - - private static Command pathFindThenPreciseAlign(Pose2d pose) { - Pose2d startPose = new Pose2d( - (Math.cos(pose.getRotation().getRadians()) * -AutonConstants.stage2Distance) + pose.getX(), - (Math.sin(pose.getRotation().getRadians()) * -AutonConstants.stage2Distance) + pose.getY(), - pose.getRotation() - ); - - List waypoints = PathPlannerPath.waypointsFromPoses(startPose, pose); - PathPlannerPath path = new PathPlannerPath( - waypoints, - finalAlignConstraints, - new IdealStartingState(0/*DriveConstants.k_maxAlignLinearSpeed.in(MetersPerSecond)*/, pose.getRotation()), - new GoalEndState(0, pose.getRotation()), - false); - - return AutoBuilder.pathfindThenFollowPath(path, pathFindConstraints); - } - - private static Command pathFinder(Pose2d pose) { - return new SequentialCommandGroup( - new InstantCommand(() -> RobotObserver.setReefMode(true)), - pathFindThenPreciseAlign(pose), - // preciseAlign(pose), - new InstantCommand(() -> RobotObserver.setReefMode(false)) - ); - } - - private static Command driveToPoint(Pose2d pose, CommandSwerveDrivetrain drivetrain) { - return new SequentialCommandGroup( - new InstantCommand(() -> RobotObserver.setReefMode(true)), - new DriveToPointCommand(FieldUtils.flipPose(pose), drivetrain), - new InstantCommand(() -> RobotObserver.setReefMode(false)) - ); - } - - public static Command generateRoutineWithCommands(CommandSwerveDrivetrain drivetrain, Pose2d desiredPickupLocation, Pose2d[] poses, Command[] scoringCommands, Supplier intakeCommand) { - SequentialCommandGroup routine = new SequentialCommandGroup(); - for (int i = 0; i < scoringCommands.length; i++) { - if (i != 0) { - routine.addCommands(pathFinder(desiredPickupLocation)); - routine.addCommands(intakeCommand.get()); - } - routine.addCommands(pathFinder(poses[i])); - routine.addCommands(scoringCommands[i]); - routine.addCommands(pathFinder(desiredPickupLocation)); - routine.addCommands(intakeCommand.get()); - } - - return routine; - } - - public static Command pathThenRunCommand(Pose2d pose, Supplier command) { - return new SequentialCommandGroup( - // pathFinder(pose), - driveToPoint(pose, m_drivetrain), - command.get() - ); - } - - public static Command closestPathThenRunCommand(Supplier scoreSupplier, List scoringLocationList) { - return pathThenRunCommand(clip(scoringLocationList), scoreSupplier); - } - - // queue stuff - - private static ArrayList onTheFlyCommands = new ArrayList<>(); - - public static void queuePathWithCommand(Pose2d pose, Supplier command) { - onTheFlyCommands.add(pathThenRunCommand(pose, command)); - } - - public static void queueClosest(Supplier scoreSupplier, List scoringLocationList) { - queuePathWithCommand(clip(scoringLocationList), scoreSupplier); - } - - private static Pose2d clip(List list) { - return FieldUtils.flipPose(RobotObserver.getPose()).nearest(list); - } - - // ****** HANDLE THE QUEUE ******** - - public static void clearQueue() { - for (Command command : onTheFlyCommands) { - command.cancel(); - } - onTheFlyCommands = new ArrayList<>(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/utils/FieldUtils.java b/ThriftyTest/src/main/java/frc/robot/utils/FieldUtils.java index 86000fdc..ed8545b3 100644 --- a/ThriftyTest/src/main/java/frc/robot/utils/FieldUtils.java +++ b/ThriftyTest/src/main/java/frc/robot/utils/FieldUtils.java @@ -5,21 +5,37 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import com.therekrab.autopilot.APTarget; public class FieldUtils { - /** - * If the driver station reports that we are on the red alliance, flip the given pose. - * In other words: - * Flips a pose either FROM alliance relative TO field relative or FROM field relative TO alliance relative. - * Defaults to blue alliance. - *

IMPORTANT

- * This depends on the current Driver Station settinng for alliance. When this function is called, the driver station pose is read. - */ - public static Pose2d flipPose(Pose2d pose) { - if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { - return FlippingUtil.flipFieldPose(pose); - } else { - return pose; - } + /** + * If the driver station reports that we are on the red alliance, flip the given pose. In other + * words: Flips a pose either FROM alliance relative TO field relative or FROM field relative TO + * alliance relative. Defaults to blue alliance. + *

IMPORTANT

This depends on the current Driver Station settinng for alliance. When this + * function is called, the driver station pose is read. + */ + public static Pose2d getLocalPose(Pose2d localPose) { + if (DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Red)) { + return FlippingUtil.flipFieldPose(localPose); + } else { + return localPose; } + } + + /** + * Flips the target if the driver station reports being on the red alliance + */ + public static APTarget flipTargetConditionally(APTarget target) { + if (DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Red)) { + APTarget flipped = target.withReference(FlippingUtil.flipFieldPose(target.getReference())); + // Use a weird thing + if (target.getEntryAngle().isPresent()) { + flipped = flipped.withEntryAngle(FlippingUtil.flipFieldRotation(target.getEntryAngle().get())); + } + return flipped; + } else { + return target; + } + } } diff --git a/ThriftyTest/src/main/java/frc/robot/utils/LoopTimer.java b/ThriftyTest/src/main/java/frc/robot/utils/LoopTimer.java new file mode 100644 index 00000000..bb1b659c --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/utils/LoopTimer.java @@ -0,0 +1,53 @@ +package frc.robot.utils; + +public class LoopTimer { + private long m_lastUpdateTime; + + private final OnboardLogger m_ologger; + + public LoopTimer(String identifier) { + m_ologger = new OnboardLogger(identifier); + m_ologger.registerDouble("Loop Time (ms)", this::getElapsedTime); + } + + /** + * Resets the timer + */ + public void reset() { + m_lastUpdateTime = System.nanoTime(); + } + + private double getElapsedTime() { + long end = System.nanoTime(); + long elapsed = end - m_lastUpdateTime; + double elapsedMillis = (double) elapsed * 1e-6; + return elapsedMillis; + } + + /** + * Logs the time since last timer reset + * + * This does not reset the timer. A call to reset() is necessary to reset the timer. + */ + public void log() { + m_ologger.log(); + } + + /** + * Runs the given runnable, and tracks how long it takes. + */ + public void time(Runnable runnable) { + reset(); + runnable.run(); + log(); + } + + /** + * Returns a runnable that runs and tracks the given runnable. + */ + public Runnable timed(Runnable runnable) { + return () -> { + time(runnable); + }; + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/utils/MonitoredSupplier.java b/ThriftyTest/src/main/java/frc/robot/utils/MonitoredSupplier.java new file mode 100644 index 00000000..4ce161dd --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/utils/MonitoredSupplier.java @@ -0,0 +1,36 @@ +package frc.robot.utils; + +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class MonitoredSupplier implements Supplier { + private T m_prev; + + private final Supplier m_supplier; + + public MonitoredSupplier(Supplier supplier) { + m_supplier = supplier; + } + + public void ifChanged(Consumer action) { + T curr = m_supplier.get(); + if (!curr.equals(m_prev)) { + m_prev = curr; + action.accept(curr); + } + } + + public static MonitoredSupplier fromDoubleSuplier(DoubleSupplier supplier) { + return new MonitoredSupplier<>(supplier::getAsDouble); + } + + public static MonitoredSupplier fromBooleanSupplier(BooleanSupplier supplier) { + return new MonitoredSupplier(supplier::getAsBoolean); + } + + public T get() { + return m_supplier.get(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java b/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java new file mode 100644 index 00000000..5f2add24 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java @@ -0,0 +1,86 @@ +package frc.robot.utils; + +import java.util.ArrayList; +import java.util.List; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.util.datalog.BooleanLogEntry; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.util.datalog.StructArrayLogEntry; +import edu.wpi.first.util.datalog.StructLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; + +/** + * A utility class to help log structured data to a DataLog without worrying about extra writes or + * other nasty situations. + */ +public class OnboardLogger { + private static final DataLog dataLog = DataLogManager.getLog(); + + private final String m_name; + + private final List, DoubleLogEntry>> m_doubleEntries; + private final List, BooleanLogEntry>> m_booleanEntries; + private final List, StringLogEntry>> m_stringEntries; + private final List, StructLogEntry>> m_poseEntries; + private final List, StructArrayLogEntry>> m_poseListEntries; + + public OnboardLogger(String name) { + m_name = name; + m_doubleEntries = new ArrayList<>(); + m_booleanEntries = new ArrayList<>(); + m_stringEntries = new ArrayList<>(); + m_poseEntries = new ArrayList<>(); + m_poseListEntries = new ArrayList<>(); + } + + public void registerDouble(String name, DoubleSupplier supplier) { + DoubleLogEntry entry = new DoubleLogEntry(dataLog, m_name + "/" + name); + m_doubleEntries.add(new Pair<>(MonitoredSupplier.fromDoubleSuplier(supplier), entry)); + } + + public void registerBoolean(String name, BooleanSupplier supplier) { + BooleanLogEntry entry = new BooleanLogEntry(dataLog, m_name + "/" + name); + m_booleanEntries.add(new Pair<>(MonitoredSupplier.fromBooleanSupplier(supplier), entry)); + } + + public void registerString(String name, Supplier supplier) { + StringLogEntry entry = new StringLogEntry(dataLog, m_name + "/" + name); + m_stringEntries.add(new Pair<>(new MonitoredSupplier<>(supplier), entry)); + } + + public void registerPose(String name, Supplier supplier) { + StructLogEntry entry = + StructLogEntry.create(dataLog, m_name + "/" + name, Pose2d.struct); + m_poseEntries.add(new Pair<>(new MonitoredSupplier<>(supplier), entry)); + } + + public void registerPoses(String name, Supplier supplier) { + StructArrayLogEntry entry = + StructArrayLogEntry.create(dataLog, m_name + "/" + name, Pose2d.struct); + m_poseListEntries.add(new Pair<>(new MonitoredSupplier<>(supplier), entry)); + } + + public void log() { + for (Pair, DoubleLogEntry> pair : m_doubleEntries) { + pair.getFirst().ifChanged(d -> pair.getSecond().append(d)); + } + for (Pair, BooleanLogEntry> pair : m_booleanEntries) { + pair.getFirst().ifChanged(b -> pair.getSecond().append(b)); + } + for (Pair, StringLogEntry> pair : m_stringEntries) { + pair.getFirst().ifChanged(s -> pair.getSecond().append(s)); + } + for (Pair, StructLogEntry> pair : m_poseEntries) { + pair.getFirst().ifChanged(p -> pair.getSecond().append(p)); + } + for (Pair, StructArrayLogEntry> pair : m_poseListEntries) { + pair.getFirst().ifChanged(ps -> pair.getSecond().append(ps)); + } + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/utils/Shape.java b/ThriftyTest/src/main/java/frc/robot/utils/Shape.java deleted file mode 100644 index 0eeabc23..00000000 --- a/ThriftyTest/src/main/java/frc/robot/utils/Shape.java +++ /dev/null @@ -1,99 +0,0 @@ -package frc.robot.utils; - -import java.util.ArrayList; -import java.util.Comparator; -import java.util.List; -import java.util.stream.Collectors; - -import edu.wpi.first.math.geometry.Translation2d; - -import static edu.wpi.first.units.Units.Meters; -import frc.robot.Constants.FieldConstants; -import frc.robot.RobotObserver; - -public class Shape { - private final List m_vertices; - private final String m_name; - - public Shape(List vertices, String name) { - this.m_vertices = vertices; - this.m_name = name; - } - - public static Shape fromUnsortedVertices(List unsortedVertices, String name) { - if (unsortedVertices.size() < 3) { - throw new IllegalArgumentException("Polygon must be atleat 3 points."); - } - - List mutableList = new ArrayList<>(unsortedVertices); - - double centerX = 0, centerY = 0; - for (Translation2d vertex : unsortedVertices) { - centerX += vertex.getX(); - centerY += vertex.getY(); - } - - centerX /= unsortedVertices.size(); - centerY /= unsortedVertices.size(); - - final double cx = centerX; - final double cy = centerY; - - mutableList.sort(Comparator.comparingDouble(point -> Math.atan2(point.getY() - cy, point.getX() - cx))); - - return new Shape(mutableList, name); - } - - // ray cast: if crosses odd times, its inside - public boolean isPointInside (Translation2d point) { - int crossings = 0; - int numVertices = m_vertices.size(); - - // horizontal ray pointing right, counts num vertices it crosses - for (int i = 0; i < numVertices; i++) { - Translation2d start = m_vertices.get(i); - Translation2d end = m_vertices.get((i+1) % numVertices); // rolls over to 1 after last vertex - - // converted to doubles for ez math - double x = point.getX(), y = point.getY(); - double x1 = start.getX(), y1 = start.getY(); - double x2 = end.getX(), y2 = end.getY(); - - if ((y > y1) != (y > y2)) { // if both true? not within y1-y2 bounds - // calculates the x coordinate of where it intersects the line - double intersectionPoint = x1 + (y - y1) * (x2 - x1) / (y2 - y1); - if (x < intersectionPoint) { // we only care if its to the RIGHT (when point less than intersection point) - crossings++; - } - } - } - - return (crossings % 2 == 1); - } - - public boolean isActive() { - if (!RobotObserver.getVisionValid()) { // vision off = don't run shape logic - return true; - } - boolean isInside = isPointInside(FieldUtils.flipPose(RobotObserver.getPose()).getTranslation()); - return isInside; - } - - public List getVertices() { - return m_vertices; - } - - // flips hotdog style (up down) - ex. reef pose - public static Shape flipHotdog(Shape shape, String name) { - // flips the y axis by doing field width - old y - return new Shape(shape.getVertices().stream() - .map(oldTranslation -> new Translation2d(oldTranslation.getX(), FieldConstants.k_fieldWidth.in(Meters) - oldTranslation.getY())) - .collect(Collectors.toList()), name); - } - - public Shape flip() { - return new Shape(this.getVertices().stream() - .map(oldTranslation -> new Translation2d(FieldConstants.k_fieldLength.in(Meters) - oldTranslation.getX(), FieldConstants.k_fieldWidth.in(Meters) - oldTranslation.getY())) - .collect(Collectors.toList()), this.m_name); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/utils/StatusSignalUtil.java b/ThriftyTest/src/main/java/frc/robot/utils/StatusSignalUtil.java new file mode 100644 index 00000000..20d1f52e --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/utils/StatusSignalUtil.java @@ -0,0 +1,33 @@ +package frc.robot.utils; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; + +@SuppressWarnings("rawtypes") +public class StatusSignalUtil { + private static StatusSignal[] m_rioSignals = new StatusSignal[0]; + private static StatusSignal[] m_canivoreSignals = new StatusSignal[0]; + + public static void registerRioSignals(StatusSignal... signals) { + StatusSignal[] newSignals = new StatusSignal[m_rioSignals.length + signals.length]; + System.arraycopy(m_rioSignals, 0, newSignals, 0, m_rioSignals.length); + System.arraycopy(signals, 0, newSignals, m_rioSignals.length, signals.length); + m_rioSignals = newSignals; + } + + public static void registerCANivoreSignals(StatusSignal... signals) { + StatusSignal[] newSignals = new StatusSignal[m_canivoreSignals.length + signals.length]; + System.arraycopy(m_canivoreSignals, 0, newSignals, 0, m_canivoreSignals.length); + System.arraycopy(signals, 0, newSignals, m_canivoreSignals.length, signals.length); + m_canivoreSignals = newSignals; + } + + public static void refreshAll() { + if (m_rioSignals.length > 0) { + BaseStatusSignal.refreshAll(m_rioSignals); + } + if (m_canivoreSignals.length > 0) { + BaseStatusSignal.refreshAll(m_canivoreSignals); + } + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/CameraIO.java b/ThriftyTest/src/main/java/frc/robot/vision/CameraIO.java new file mode 100644 index 00000000..76dbd99b --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/CameraIO.java @@ -0,0 +1,14 @@ +package frc.robot.vision; + +import java.util.ArrayList; +import java.util.List; +import org.photonvision.targeting.PhotonPipelineResult; + +public interface CameraIO { + void updateInputs(CameraIOInputs inputs); + + public class CameraIOInputs { + public boolean connected = true; + public List unreadResults = new ArrayList<>(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/CameraIOHardware.java b/ThriftyTest/src/main/java/frc/robot/vision/CameraIOHardware.java new file mode 100644 index 00000000..645ca264 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/CameraIOHardware.java @@ -0,0 +1,22 @@ +package frc.robot.vision; + +import org.photonvision.PhotonCamera; + +public class CameraIOHardware implements CameraIO { + private final PhotonCamera m_camera; + private final String m_name; + + public CameraIOHardware(String name) { + m_camera = new PhotonCamera(name); + m_name = name; + } + + public void updateInputs(CameraIOInputs inputs) { + inputs.connected = m_camera.isConnected(); + inputs.unreadResults = m_camera.getAllUnreadResults(); + } + + public String getName() { + return m_name; + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/CameraIOInputsLogger.java b/ThriftyTest/src/main/java/frc/robot/vision/CameraIOInputsLogger.java new file mode 100644 index 00000000..093ec3d5 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/CameraIOInputsLogger.java @@ -0,0 +1,17 @@ +package frc.robot.vision; + +import frc.robot.utils.OnboardLogger; +import frc.robot.vision.CameraIO.CameraIOInputs; + +public class CameraIOInputsLogger { + private final OnboardLogger log; + + public CameraIOInputsLogger(CameraIOInputs inputs, String name) { + log = new OnboardLogger("Cameras"); + log.registerBoolean(name + "Connected", () -> inputs.connected); + } + + public void log() { + log.log(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/CameraOffset.java b/ThriftyTest/src/main/java/frc/robot/vision/CameraOffset.java deleted file mode 100644 index 6429af0e..00000000 --- a/ThriftyTest/src/main/java/frc/robot/vision/CameraOffset.java +++ /dev/null @@ -1,10 +0,0 @@ -package frc.robot.vision; - -import java.util.Map; - -import edu.wpi.first.math.geometry.Transform3d; - -public record CameraOffset ( - String source, - Map offsets -) {} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/LogBuilder.java b/ThriftyTest/src/main/java/frc/robot/vision/LogBuilder.java deleted file mode 100644 index 52a7cbcb..00000000 --- a/ThriftyTest/src/main/java/frc/robot/vision/LogBuilder.java +++ /dev/null @@ -1,50 +0,0 @@ -package frc.robot.vision; - -import java.util.ArrayList; -import java.util.List; - -import edu.wpi.first.math.geometry.Pose2d; -import frc.robot.Constants.VisionConstants; -import frc.robot.RobotObserver; - -public class LogBuilder { - private List m_estimates; - private List m_logs; - - public LogBuilder() { - m_estimates = new ArrayList<>(20); - m_logs = new ArrayList<>(20); - } - - public void addEstimate(TimestampedPoseEstimate estimate) { - m_estimates.add(estimate); - } - - private void buildLogs() { - m_logs.clear(); - for (TimestampedPoseEstimate est : m_estimates) { - Pose2d robot = RobotObserver.getPose(); - double distance = est.pose().minus(robot) - .getTranslation() - .getNorm(); - m_logs.add( - new VisionLog(est, distance, robot) - ); - } - // consume each processed value - m_estimates.clear(); - } - - - public void log() { - buildLogs(); - if (VisionConstants.k_enableLogging) VisionLogger.record(m_logs); - } - - /* a helper record to handle logs */ - public record VisionLog( - TimestampedPoseEstimate estimate, - double error, - Pose2d robot - ) {} -} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/MultiInputFilter.java b/ThriftyTest/src/main/java/frc/robot/vision/MultiInputFilter.java deleted file mode 100644 index ce2558c2..00000000 --- a/ThriftyTest/src/main/java/frc/robot/vision/MultiInputFilter.java +++ /dev/null @@ -1,86 +0,0 @@ -package frc.robot.vision; - -import java.util.HashMap; -import java.util.HashSet; -import java.util.Optional; -import java.util.Set; -import java.util.Map.Entry; - -import org.slf4j.Logger; - -import org.slf4j.LoggerFactory; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import frc.robot.Constants.VisionConstants; - -public class MultiInputFilter { - private final Logger m_logger = LoggerFactory.getLogger(MultiInputFilter.class); - - private HashMap> m_tags = new HashMap<>(); - - /** - * Returns whether a camera at the source is able to "see" the tag with the - * specified ID, using the known camera horizontal field of view. - */ - private boolean verifyTarget(Pose2d source, int tag) { - Optional tagPose = VisionConstants.k_layout.getTagPose(tag); - if (tagPose.isEmpty()) return false; - Transform2d relative = tagPose.get().toPose2d().minus(source); - double angleToTag = Math.abs(Math.atan2(relative.getY(), relative.getX())); - boolean angleToTagOk = angleToTag < VisionConstants.kHorizontalFov.getRadians() / 2; - double tagFacingAngle = Math.abs(relative.getRotation().getRotations()); - boolean tagFacingAngleOk = tagFacingAngle > 0.25; - if (angleToTagOk && tagFacingAngleOk) { - return true; - } else { - return false; - } - } - - /** - * Returns whether all targets provided COULD be seen with this camera - */ - private boolean verifyTargets(Pose2d source, Set targets) { - for (int target : targets) { - if (!verifyTarget(source, target)) return false; - } - return true; - } - - public void addEstimate(TimestampedPoseEstimate est) { - if (!m_tags.containsKey(est.source())) { - m_tags.put(est.source(), new HashSet<>()); - } - Set visible = m_tags.get(est.source()); - visible.addAll(est.tags()); - } - - public void clear() { - m_tags.clear(); - } - - public boolean verify(Pose2d estimate) { - for (Entry> entry : m_tags.entrySet()) { - String sourceName = entry.getKey(); - Set tags = entry.getValue(); - if (!VisionConstants.cameras.containsKey(sourceName)) { - m_logger.warn("Detected target not on field layout, ignoring"); - continue; - } - Transform3d offset = VisionConstants.cameras.get(sourceName); - Transform2d offset2d = new Transform2d( - offset.getX(), - offset.getY(), - offset.getRotation().toRotation2d() - ); - Pose2d source = estimate.plus(offset2d); - if (!verifyTargets(source, tags)) return false; - } - // no checks have failed, i.e. all checks have passed. - return true; - } - -} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/SingleInputPoseEstimator.java b/ThriftyTest/src/main/java/frc/robot/vision/SingleInputPoseEstimator.java deleted file mode 100644 index f964e50e..00000000 --- a/ThriftyTest/src/main/java/frc/robot/vision/SingleInputPoseEstimator.java +++ /dev/null @@ -1,309 +0,0 @@ -package frc.robot.vision; - -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import java.util.function.Consumer; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import com.ctre.phoenix6.Utils; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import static edu.wpi.first.units.Units.Seconds; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants.FieldConstants; -import frc.robot.Constants.VisionConstants; -import frc.robot.RobotObserver; -import frc.robot.vision.TimestampedPoseEstimate.EstimationAlgorithm; - -public class SingleInputPoseEstimator implements Runnable { - private final Logger m_logger = LoggerFactory.getLogger(SingleInputPoseEstimator.class); - - private final PhotonCamera m_camera; - private final Consumer m_reporter; - - // Estimators - private final PhotonPoseEstimator m_estimator; - - private final String m_name; - - private final Transform3d m_robotToCamera; - - public SingleInputPoseEstimator( - PhotonCamera camera, - Transform3d robotToCamera, - Consumer updateCallback - ) { - m_camera = camera; - m_name = camera.getName(); - m_reporter = updateCallback; - m_robotToCamera = robotToCamera; - m_estimator = new PhotonPoseEstimator( - VisionConstants.k_layout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - robotToCamera - ); - m_estimator.setMultiTagFallbackStrategy( - PoseStrategy.PNP_DISTANCE_TRIG_SOLVE - ); - } - - @Override - public void run() { - if (!m_camera.isConnected()) { - SmartDashboard.putBoolean(m_name + " Connected", false); - m_logger.error("Unable to read data from {}", m_name); - } else { - SmartDashboard.putBoolean(m_name + " Connected", true); - } - // Pull the latest data from the camera. - List results = m_camera.getAllUnreadResults(); - if (results.size() > VisionConstants.k_expectedResults) { - /* - Rationale for this warning: - This run() method should be running on a loop. It should run fast. - Ideally, it runs WAY faster than the camera and always receives - either 0 or 1 new result. - We may want to know if we are being bombarded with too many results, - i.e. the camera is running faster than we are, which could suggest - that we are running slow. - Also, we assume that the time that we see the result minus the time - the result took to get sent to us is the time that it was sent. - But if we are running slowly, it's possible there would be some - time between when a result was sent and when we "see" it. This would - mess up the timestamping logic. - */ - m_logger.trace("Possibly too many results: {} ({})", results.size(), m_camera.getName()); - } - m_estimator.addHeadingData( - RobotController.getMeasureTime().in(Seconds), - RobotObserver.getPose().getRotation() - ); - /* take many */ - for (PhotonPipelineResult result : results) { - //headingHandleResult(result); - //handleResult(result); - combinedHandleResult(result); - } - } - - private void combinedHandleResult(PhotonPipelineResult result) { - // some prechecks before we do anything - if (!precheckValidity(result)) return; - // we can now assume that we have targets - List targets = result.getTargets(); - // use solvePnP every time - EstimationAlgorithm algorithm = (targets.size() > 1) ? - EstimationAlgorithm.PnP - : EstimationAlgorithm.Trig; - - Optional est = m_estimator.update(result); - if (est.isPresent()) { - Optional processed = process(result, est.get().estimatedPose, algorithm); - if (processed.isPresent()) { - m_reporter.accept(processed.get()); - return; - } - } - PhotonTrackedTarget target = targets.get(0); - int fidId = target.getFiducialId(); - Optional targetPosition = VisionConstants.k_layout - .getTagPose(fidId); - if (targetPosition.isEmpty()) { - m_logger.error("Tag {} detected not in field layout", fidId); - return; - } - - Pose3d targetPosition3d = targetPosition.get(); - Transform3d best3d = target.getBestCameraToTarget(); - Transform3d alt3d = target.getAlternateCameraToTarget(); - Pose3d best = targetPosition3d - .plus(best3d.inverse()) - .plus(m_robotToCamera.inverse()); - Pose3d alt = targetPosition3d - .plus(alt3d.inverse()) - .plus(m_robotToCamera.inverse()); - if (RobotBase.isSimulation()) { - RobotObserver.getField().getObject("alt").setPose(alt.toPose2d()); - RobotObserver.getField().getObject("best").setPose(best.toPose2d()); - } - double bestHeading = best.getRotation().getZ(); - double altHeading = alt.getRotation().getZ(); - Pose2d pose = RobotObserver.getPose(); - double heading = pose.getRotation().getRadians(); - Transform2d bestDiff = best.toPose2d().minus(pose); - Transform2d altDiff = alt.toPose2d().minus(pose); - double bestRotErr = Math.abs(bestHeading - heading); - double altRotErr = Math.abs(altHeading - heading); - double bestXYErr = bestDiff.getTranslation().getNorm(); - double altXYErr = altDiff.getTranslation().getNorm(); - Pose3d estimate; - - if (Math.abs(bestRotErr - altRotErr) >= VisionConstants.k_headingThreshold) { - estimate = (bestRotErr <= altRotErr) ? best : alt; - } else { - estimate = (bestXYErr <= altXYErr) ? best : alt; - } - - process(result, estimate, EstimationAlgorithm.Heading).ifPresent(m_reporter); - - } - - private boolean precheckValidity(PhotonPipelineResult result) { - double latency = result.metadata.getLatencyMillis() / 1.0e+3; - // too old -> don't count it - if (latency > VisionConstants.k_latencyThreshold) { - // this is interesting, so let's report it - m_logger.warn("({}) Refused old vision data, latency of {}", m_name, latency); - return false; - } - // check if we are in reef mode - //if (RobotObserver.getReefMode()) { - // switch (RobotObserver.getReefClipLocation()) { - // case LEFT: - // if (m_name.equals(VisionConstants.k_leftAlignName)) return false; - // case RIGHT: - // if (m_name.equals(VisionConstants.k_rightAlignName)) return false; - // } - //} - // no targets -> no pose - return result.hasTargets(); - } - - private Optional process( - PhotonPipelineResult result, - Pose3d pose, - EstimationAlgorithm algorithm - ) { - double latency = result.metadata.getLatencyMillis() / 1.0e+3; - double timestamp = Utils.getCurrentTimeSeconds() - latency; - double ambiguity = getAmbiguity(result); - Pose2d flatPose = pose.toPose2d(); - Matrix stdDevs = calculateStdDevs(result, latency, flatPose); - // if in reef mode, disregard data that doesn't use the reef. - if (RobotObserver.getReefMode()) { - int id = result.getBestTarget().getFiducialId(); - if (!VisionConstants.k_reefIds.contains(id)) { - return Optional.empty(); - } - } - // check validity again - if (!checkValidity(pose, ambiguity)) return Optional.empty(); - List tags = new ArrayList<>(); - for (PhotonTrackedTarget target : result.getTargets()) { - tags.add(target.getFiducialId()); - } - return Optional.of( - new TimestampedPoseEstimate(flatPose, m_name, timestamp, stdDevs, algorithm, tags) - ); - } - - private boolean checkValidity( - Pose3d pose, - double ambiguity - ) { - if (ambiguity >= VisionConstants.k_AmbiguityThreshold) return false; - return !isOutsideField(pose); - } - - private boolean isOutsideField(Pose3d pose) { - double x = pose.getX(); - double y = pose.getY(); - double z = pose.getZ(); - double xMax = VisionConstants.k_XYMargin.magnitude() - + FieldConstants.k_fieldLength.magnitude(); - double yMax = VisionConstants.k_XYMargin.magnitude() - + FieldConstants.k_fieldWidth.magnitude(); - double xyMin = -VisionConstants.k_XYMargin.magnitude(); - double zMax = VisionConstants.k_ZMargin.magnitude(); - double zMin = -VisionConstants.k_ZMargin.magnitude(); - return x < xyMin - || x > xMax - || y < xyMin - || y > yMax - || z > zMax - || z < zMin; - } - - private Matrix calculateStdDevs( - PhotonPipelineResult result, - double latency, - Pose2d pose - ) { - double multiplier = calculateStdDevMultiplier(result, latency, pose); - Matrix stdDevs = VecBuilder.fill( - multiplier * VisionConstants.k_translationCoefficient, - multiplier * VisionConstants.k_translationCoefficient, - multiplier * VisionConstants.k_rotationCoefficient - ); - return stdDevs; - } - - private double calculateStdDevMultiplier( - PhotonPipelineResult result, - double latency, - Pose2d pose - ) { - double averageTagDistance = 0; - for (PhotonTrackedTarget tag : result.getTargets()) { - averageTagDistance += tag - .getBestCameraToTarget() - .getTranslation() - .getNorm(); - } - averageTagDistance /= result.getTargets().size(); - // calculate tag distance factor - double distanceFactor = Math.max(1, - VisionConstants.k_distanceMultiplier - * (averageTagDistance - VisionConstants.k_noisyDistance) - ); - // calculate an (average) ambiguity real quick: - double ambiguity = getAmbiguity(result); - // ambiguity factor - double ambiguityFactor = Math.max(1, - VisionConstants.k_ambiguityMultiplier * ambiguity - + VisionConstants.k_ambiguityShifter - ); - // tag divisor - double tags = result.getTargets().size(); - double tagDivisor = 1 + (tags - 1) * VisionConstants.k_targetMultiplier; - // distance from last pose - double poseDifferenceError = Math.max(0, - RobotObserver.getPose().minus(pose).getTranslation().getNorm() - - VisionConstants.k_differenceThreshold * RobotObserver.getVelocity() - ); - double diffMultiplier = Math.max(1, - poseDifferenceError * VisionConstants.k_differenceMultiplier - ); - double timeMultiplier = Math.max(1, latency * VisionConstants.k_latencyMultiplier); - // final calculation - double stdDevMultiplier = ambiguityFactor - * distanceFactor - * diffMultiplier - * timeMultiplier - / tagDivisor; - return stdDevMultiplier; - } - - private double getAmbiguity(PhotonPipelineResult result) { - return result.getBestTarget().getPoseAmbiguity(); - } -} - diff --git a/ThriftyTest/src/main/java/frc/robot/vision/TimestampedPoseEstimate.java b/ThriftyTest/src/main/java/frc/robot/vision/TimestampedPoseEstimate.java deleted file mode 100644 index e5bdc58c..00000000 --- a/ThriftyTest/src/main/java/frc/robot/vision/TimestampedPoseEstimate.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.vision; - -import java.util.List; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; - -public record TimestampedPoseEstimate ( - Pose2d pose, - String source, - double timestamp, - Matrix stdDevs, - EstimationAlgorithm algorithm, - List tags -) { - public enum EstimationAlgorithm { - Trig, PnP, Ambiguity, Heading; - - @Override - public String toString() { - String s = "?"; - switch (this) { - case Trig -> s = "T"; - case PnP -> s = "P"; - case Ambiguity -> s = "A"; - case Heading -> s = "H"; - } - return s; - } - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/VisionHandler.java b/ThriftyTest/src/main/java/frc/robot/vision/VisionHandler.java deleted file mode 100644 index bd959e3f..00000000 --- a/ThriftyTest/src/main/java/frc/robot/vision/VisionHandler.java +++ /dev/null @@ -1,144 +0,0 @@ -package frc.robot.vision; - -import static edu.wpi.first.units.Units.Milliseconds; - -import java.util.ArrayList; -import java.util.List; -import java.util.Map; - -import org.photonvision.PhotonCamera; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import frc.robot.Constants.VisionConstants; -import frc.robot.Robot; -import frc.robot.RobotObserver; -import frc.robot.subsystems.CommandSwerveDrivetrain; - -public class VisionHandler implements AutoCloseable { - @SuppressWarnings("unused") - private final Logger m_logger = LoggerFactory.getLogger(VisionHandler.class); - private CommandSwerveDrivetrain m_drivetrain; - private final Notifier m_notifier; - private List m_estimators = new ArrayList<>(); - private List m_estimates = new ArrayList<>(); - - private VisionSystemSim m_visionSim = new VisionSystemSim("main"); - private SimCameraProperties m_simProps = new SimCameraProperties(); - - private final Field2d m_field; - - private LogBuilder m_logBuilder = new LogBuilder(); - - private final MultiInputFilter m_filter = new MultiInputFilter(); - - public VisionHandler(CommandSwerveDrivetrain drivetrain) { - m_drivetrain = drivetrain; - m_visionSim.addAprilTags(VisionConstants.k_layout); - setupProps(); - setupCameras(); - m_notifier = new Notifier(this::updateEstimators); - m_field = m_visionSim.getDebugField(); - RobotObserver.setField(m_field); - } - - /** - * Sets up the simulated camera properties with values that should - * reflect the real world situation. - */ - private void setupProps() { - m_simProps.setCalibration( - VisionConstants.k_resWidth, - VisionConstants.k_resHeight, - VisionConstants.k_fov - ); - m_simProps.setAvgLatencyMs( - VisionConstants.k_avgLatency.in(Milliseconds) - ); - m_simProps.setLatencyStdDevMs( - VisionConstants.k_latencyStdDev.in(Milliseconds) - ); - m_simProps.setCalibError( - VisionConstants.k_avgErr, - VisionConstants.k_errStdDev - ); - } - - private void setupCameras() { - for (Map.Entry entry : VisionConstants.cameras.entrySet()) { - // it's easier to read this way: - String cameraName = entry.getKey(); - Transform3d robotToCamera = entry.getValue(); - // initialze both real and simulated cameras - PhotonCamera realCamera = new PhotonCamera(cameraName); - if (Robot.isSimulation()) { - PhotonCameraSim simCamera = new PhotonCameraSim(realCamera, m_simProps); - m_visionSim.addCamera(simCamera, robotToCamera); - - // This is somewhat intensive (especially the first one) so we only - // enable if the robot is in simulation mode. - simCamera.enableDrawWireframe(true); - } - // we always need to add a vision estimator - SingleInputPoseEstimator estimator = new SingleInputPoseEstimator( - realCamera, - robotToCamera, - this::addEstimate - ); - m_estimators.add(estimator); - } - } - - private void updateEstimators() { - m_estimates.clear(); - // clear previous output from the estimators. - m_field.getObject(VisionConstants.k_estimationName).setPoses(); - m_field.getObject("best").setPoses(); - m_field.getObject("alt").setPoses(); - // setup filter - m_filter.clear(); - for (SingleInputPoseEstimator estimator : m_estimators) { - estimator.run(); - } - List poses = new ArrayList<>(); - List rejected = new ArrayList<>(); - for (TimestampedPoseEstimate estimate : m_estimates) { - if (m_filter.verify(estimate.pose())) { - m_drivetrain.addPoseEstimate(estimate); - poses.add(estimate.pose()); - } else { - rejected.add(estimate.pose()); - } - } - m_field.getObject(VisionConstants.kRejectedName).setPoses(rejected); - m_field.getObject(VisionConstants.k_estimationName).setPoses(poses); - Pose2d currPose = m_drivetrain.getPose(); - m_visionSim.update(currPose); - // finish logging - m_logBuilder.log(); - } - - public void startThread() { - m_notifier.startPeriodic(VisionConstants.k_periodic); - } - - private void addEstimate(TimestampedPoseEstimate estimate) { - m_filter.addEstimate(estimate); - m_estimates.add(estimate); - - // pose logging - m_logBuilder.addEstimate(estimate); - } - - @Override - public void close() { - m_notifier.close(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/VisionLogger.java b/ThriftyTest/src/main/java/frc/robot/vision/VisionLogger.java deleted file mode 100644 index ab5a4fd7..00000000 --- a/ThriftyTest/src/main/java/frc/robot/vision/VisionLogger.java +++ /dev/null @@ -1,97 +0,0 @@ -package frc.robot.vision; - -import java.io.BufferedWriter; -import java.io.FileWriter; -import java.io.IOException; -import java.util.List; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import frc.robot.Constants.VisionConstants; -import frc.robot.Robot; -import frc.robot.vision.LogBuilder.VisionLog; - -public class VisionLogger implements AutoCloseable { - private static VisionLogger instance; - - private synchronized static VisionLogger getInstance() { - if (instance == null) { - instance = new VisionLogger(); - } - return instance; - } - - private Logger logger = LoggerFactory.getLogger(VisionLogger.class); - - private final StringBuilder m_builder; - - private FileWriter m_writer; - private BufferedWriter m_buffer; - - private VisionLogger() { - m_builder = new StringBuilder(); - String filepath = VisionConstants.k_logPath + Long.toHexString(System.currentTimeMillis() / 1000) + ".log"; - if (Robot.isSimulation()) { - filepath = VisionConstants.k_simLogPath + Long.toHexString(System.currentTimeMillis() / 1000) + ".log"; - } - try { - m_writer = new FileWriter(filepath); - } catch (IOException e) { - logger.error("failed to open vision log file : {}", e.toString()); - return; - } - m_buffer = new BufferedWriter(m_writer); - - try { - m_buffer.write("time, source, x, y, err\n"); - } catch (IOException e) { - logger.error("failed to write to vision log: {}", e.toString()); - } - } - - private synchronized void recordLogs(List logs) { - if (m_buffer == null) { - logger.warn("Not saving vision logs! Check your console for more info"); - return; - } - - m_builder.setLength(0); - - for (VisionLog log : logs) { - m_builder.append(log.estimate().timestamp()); - m_builder.append(","); - m_builder.append(log.estimate().source()); - m_builder.append(","); - m_builder.append(log.robot().getX()); - m_builder.append(","); - m_builder.append(log.robot().getY()); - m_builder.append(","); - m_builder.append(log.error()); - m_builder.append(","); - m_builder.append(log.estimate().algorithm().toString()); - m_builder.append("\n"); - } - - try { - m_buffer.write(m_builder.toString()); - m_buffer.flush(); - } catch (IOException e) { - logger.error("Failed writing to vision log file: {}", e.toString()); - } - - } - - public static void record(List logs) { - getInstance().recordLogs(logs); - } - - @Override - public void close() { - try { - m_buffer.close(); - } catch (IOException e) { - logger.error("failed closing file: {}", e.toString()); - } - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/AprilTagVisionHandler.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/AprilTagVisionHandler.java new file mode 100644 index 00000000..4ba381f3 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/AprilTagVisionHandler.java @@ -0,0 +1,97 @@ +package frc.robot.vision.localization; + +import java.util.ArrayList; +import java.util.List; +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.Supplier; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Notifier; +import frc.robot.Robot; +import frc.robot.utils.LoopTimer; +import frc.robot.vision.CameraIO; +import frc.robot.vision.CameraIOHardware; + +public class AprilTagVisionHandler implements AutoCloseable { + @SuppressWarnings("unused") + private final Logger m_logger = LoggerFactory.getLogger(AprilTagVisionHandler.class); + + private final LoopTimer m_loopTimer; + + private final Supplier m_poseSupplier; + private final Consumer m_consumer; + + private final Notifier m_notifier; + private final List m_estimators = new ArrayList<>(); + private final MultiInputFilter m_filter; + + private final AprilTagVisionLogger m_esimateLogger; + + public AprilTagVisionHandler(Supplier poseSupplier, Consumer callback) { + m_poseSupplier = poseSupplier; + m_consumer = callback; + m_filter = new MultiInputFilter(); + m_esimateLogger = new AprilTagVisionLogger(); + setupCameras(); + m_notifier = new Notifier(this::updateEstimators); + m_loopTimer = new LoopTimer("Vision"); + } + + private void setupCameras() { + for (Map.Entry entry : LocalizationConstants.kCameras.entrySet()) { + String cameraName = entry.getKey(); + Transform3d robotToCamera = entry.getValue(); + CameraIO io; + if (Robot.isSimulation()) { + io = new CameraIOAprilTagSim(cameraName, robotToCamera, m_poseSupplier); + } else { + io = new CameraIOHardware(cameraName); + } + SingleInputPoseEstimator estimator = new SingleInputPoseEstimator( + m_filter, + io, + cameraName, + robotToCamera, + this::addEstimate); + m_estimators.add(estimator); + } + } + + private void updateEstimators() { + m_loopTimer.reset(); + m_filter.clear(); + for (SingleInputPoseEstimator estimator : m_estimators) { + estimator.refresh(m_poseSupplier.get()); + } + + for (SingleInputPoseEstimator estimator : m_estimators) { + estimator.run(); + } + // finish logging + m_esimateLogger.log(); + m_loopTimer.log(); + } + + public void startThread() { + m_notifier.startPeriodic(LocalizationConstants.kPeriodic); + } + + private void addEstimate(TimestampedPoseEstimate estimate) { + if (DriverStation.isDisabled()) { + if (!m_filter.verify(estimate.pose())) { + return; + } + } + m_consumer.accept(estimate); + m_esimateLogger.addEstimate(estimate); + } + + @Override + public void close() { + m_notifier.close(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/AprilTagVisionLogger.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/AprilTagVisionLogger.java new file mode 100644 index 00000000..bcf78368 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/AprilTagVisionLogger.java @@ -0,0 +1,37 @@ +package frc.robot.vision.localization; + +import java.util.ArrayList; +import java.util.List; +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.utils.OnboardLogger; + +public class AprilTagVisionLogger { + private List m_estimates; + + private final OnboardLogger m_logger; + + public AprilTagVisionLogger() { + m_estimates = new ArrayList<>(20); + m_logger = new OnboardLogger("Vision"); + m_logger.registerPoses("Estimates", () -> { + Pose2d[] estimates = new Pose2d[m_estimates.size()]; + for (int i = 0; i < m_estimates.size(); i++) { + estimates[i] = m_estimates.get(i); + } + return estimates; + }); + } + + public void addEstimate(TimestampedPoseEstimate estimate) { + if (LocalizationConstants.kEnableLogging) { + m_estimates.add(estimate.pose()); + } + } + + public void log() { + if (LocalizationConstants.kEnableLogging) { + m_logger.log(); + m_estimates.clear(); + } + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/CameraIOAprilTagSim.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/CameraIOAprilTagSim.java new file mode 100644 index 00000000..f919e246 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/CameraIOAprilTagSim.java @@ -0,0 +1,69 @@ +package frc.robot.vision.localization; + +import static edu.wpi.first.units.Units.Milliseconds; +import java.util.function.Supplier; +import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotObserver; +import frc.robot.vision.CameraIO; + +public class CameraIOAprilTagSim implements CameraIO { + private static boolean setupComplete; + + private static final VisionSystemSim simSystem = new VisionSystemSim("localization"); + + private static final SimCameraProperties simProps = new SimCameraProperties(); + + private static final Field2d simField = simSystem.getDebugField(); + + private final PhotonCamera m_camera; + private final PhotonCameraSim m_cameraSim; + private final Transform3d m_robotToCamera; + + private final Supplier m_poseSupplier; + + private final String m_name; + + public CameraIOAprilTagSim(String name, Transform3d robotToCamera, Supplier poseSupplier) { + setupSimProps(); + m_name = name; + m_poseSupplier = poseSupplier; + m_camera = new PhotonCamera(name); + m_robotToCamera = robotToCamera; + m_cameraSim = new PhotonCameraSim(m_camera, simProps); + simSystem.addCamera(m_cameraSim, m_robotToCamera); + SmartDashboard.putBoolean("Vision/" + m_name + " connected", true); + } + + public void updateInputs(CameraIOInputs inputs) { + inputs.connected = SmartDashboard.getBoolean("Vision/" + m_name + " connected", true); + simSystem.update(m_poseSupplier.get()); + inputs.unreadResults = m_camera.getAllUnreadResults(); + } + + public String getName() { + return m_camera.getName(); + } + + private static void setupSimProps() { + if (setupComplete) { + return; + } + setupComplete = true; + simSystem.addAprilTags(LocalizationConstants.kTagLayout); + simProps.setCalibration( + LocalizationConstants.kResWidth, + LocalizationConstants.kResHeight, + LocalizationConstants.kFOV); + simProps.setAvgLatencyMs(LocalizationConstants.kAvgLatency.in(Milliseconds)); + simProps.setLatencyStdDevMs(LocalizationConstants.kLatencyStdDev.in(Milliseconds)); + simProps.setCalibError(LocalizationConstants.kAvgErr, LocalizationConstants.kErrStdDevs); + RobotObserver.setField(simField); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java new file mode 100644 index 00000000..f00c15e9 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java @@ -0,0 +1,108 @@ +package frc.robot.vision.localization; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Milliseconds; +import java.util.Map; +import java.util.Set; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Time; +import frc.robot.Robot; + +public class LocalizationConstants { + protected static final boolean kEnableLogging = Robot.isSimulation(); + + protected static final double kRotationCoefficient = Math.PI * 0.5; + protected static final double kTranslationCoefficient = 0.06; + + protected static final Vector kBaseStdDevs = + VecBuilder.fill(kTranslationCoefficient, kTranslationCoefficient, kRotationCoefficient); + + protected static final AprilTagFieldLayout kTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + protected static final String kEstimationName = "estimation"; + protected static final String kRejectName = "rejected"; + + protected static final String kLogPath = "/home/lvuser/logs/vision"; + protected static final String kSimLogPath = "logs/vision"; + + private static final double kCameraHeight = 0.190; + private static final double kTightPitch = -Units.degreesToRadians(22.5); + private static final double kWidePitch = -Units.degreesToRadians(25.0); + private static final double kTightYaw = Units.degreesToRadians(37.0); // this doesn't seem + private static final double kWideYaw = Units.degreesToRadians(-7.0); + + @SuppressWarnings("unused") + private static Map kFakeCameras = Map.ofEntries( + Map.entry("test", new Transform3d(0, 0, 0, new Rotation3d()))); + + private static Map kRealCameras = Map.ofEntries( + Map.entry("cam1", new Transform3d( // left tight + new Translation3d(0.256, 0.289, kCameraHeight), + new Rotation3d(0, kTightPitch, -kTightYaw + Units.degreesToRadians(0.9)))), + Map.entry("cam2", new Transform3d( // left wide + new Translation3d(0.337, 0.331, kCameraHeight), + new Rotation3d(0, kWidePitch, -kWideYaw))), + Map.entry("cam3", new Transform3d( // right wide + new Translation3d(0.337, -0.331, kCameraHeight), + new Rotation3d(0, kWidePitch, kWideYaw))), + Map.entry("cam4", new Transform3d( // right tight + new Translation3d(0.256, -0.289, kCameraHeight), + new Rotation3d(0, kTightPitch, kTightYaw)))); + + protected static Map kCameras = kRealCameras; + + /** The tick time for each pose estimator to run */ + protected static final double kPeriodic = 0.02; + /** The maximum tolerated latency, in seconds. */ + protected static final double kLatencyThreshold = 0.75; + /** The maximum tolerated ambiguity value. */ + protected static final double kAmbiguityThreshold = 0.2; + /** The farthest out off a field a pose estimate can say we are (in each dimension separately) */ + protected static final Distance kXYMargin = Meters.of(0.5); + /** The maximum height from that a camera's pose can reasonably report */ + protected static final Distance kZMargin = Meters.of(1.5); + + // Some configuration variables: + protected static final double kDistanceMultiplier = 5.0; + protected static final double kNoisyDistance = 0.8; + protected static final double kAmbiguityMultiplier = 0.4; + protected static final double kAmbiguityShifter = 0.2; + protected static final double kTargetMultiplier = 80; + protected static final double kDifferenceThreshold = 0.10; + protected static final double kDifferenceMultiplier = 200.0; + protected static final double kLatencyMultiplier = 1.3; + + protected static final double kHeadingThreshold = Units.degreesToRadians(3); + + // Stats about the camera for simulation + protected static final int kResWidth = 320; + protected static final int kResHeight = 240; + protected static final Rotation2d kFOV = Rotation2d.fromDegrees(82.0); + protected static final Rotation2d kHorizontalFov = Rotation2d.fromDegrees(70.0); + + + // Simulated error: + protected static final Time kAvgLatency = Milliseconds.of(18); + protected static final Time kLatencyStdDev = Milliseconds.of(2); + protected static final double kAvgErr = 0.08; + protected static final double kErrStdDevs = 0.02; + + public static final boolean kEnableReefFilter = false; + + protected static final Set kReefIds = Set.of( + 6, 7, 8, 9, 10, 11, // red tags + 17, 18, 19, 20, 21, 22 // blue tags + ); +} + diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/MultiInputFilter.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/MultiInputFilter.java new file mode 100644 index 00000000..fcb93a17 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/MultiInputFilter.java @@ -0,0 +1,87 @@ +package frc.robot.vision.localization; + +import java.util.HashMap; +import java.util.HashSet; +import java.util.Map.Entry; +import java.util.Optional; +import java.util.Set; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; + +public class MultiInputFilter { + private final Logger m_logger = LoggerFactory.getLogger(MultiInputFilter.class); + + private HashMap> m_tags = new HashMap<>(); + + /** + * Returns whether a camera at the source is able to "see" the tag with the specified ID, using + * the known camera horizontal field of view. + */ + private boolean verifyTarget(Pose2d source, int tag) { + Optional tagPose = LocalizationConstants.kTagLayout.getTagPose(tag); + if (tagPose.isEmpty()) { + return false; + } + Pose2d tagPose2d = tagPose.get().toPose2d(); + Transform2d sourceRelative = tagPose2d.minus(source); + Transform2d tagRelative = source.minus(tagPose2d); + Rotation2d sourceAngle = new Rotation2d(sourceRelative.getX(), sourceRelative.getY()); + Rotation2d tagAngle = new Rotation2d(tagRelative.getX(), tagRelative.getY()); + boolean sourceAngleOk = + Math.abs(sourceAngle.getRadians()) <= LocalizationConstants.kHorizontalFov.getRadians() / 2.0; + boolean tagAngleOk = Math.abs(tagAngle.getRadians()) <= Math.PI / 2.0; + return sourceAngleOk && tagAngleOk; + } + + /** + * Returns whether all targets provided COULD be seen with this camera + */ + private boolean verifyTargets(Pose2d source, Set targets) { + for (int target : targets) { + if (!verifyTarget(source, target)) { + return false; + } + } + return true; + } + + public void addInput(String source, Set tags) { + if (!m_tags.containsKey(source)) { + m_tags.put(source, new HashSet<>()); + } + Set visible = m_tags.get(source); + visible.addAll(tags); + } + + public void clear() { + m_tags.clear(); + } + + public boolean verify(Pose2d estimate) { + for (Entry> entry : m_tags.entrySet()) { + String sourceName = entry.getKey(); + Set tags = entry.getValue(); + if (!LocalizationConstants.kCameras.containsKey(sourceName)) { + m_logger.warn("Detected target not on field layout, ignoring"); + continue; + } + Transform3d offset = LocalizationConstants.kCameras.get(sourceName); + Transform2d offset2d = new Transform2d( + offset.getX(), + offset.getY(), + offset.getRotation().toRotation2d()); + Pose2d source = estimate.plus(offset2d); + if (!verifyTargets(source, tags)) { + return false; + } + } + // no checks have failed, i.e. all checks have passed. + return true; + } + +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java new file mode 100644 index 00000000..ca8f08d3 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java @@ -0,0 +1,268 @@ +package frc.robot.vision.localization; + +import static edu.wpi.first.units.Units.Seconds; +import java.util.List; +import java.util.Optional; +import java.util.Set; +import java.util.function.Consumer; +import java.util.stream.Collectors; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import com.ctre.phoenix6.Utils; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.Constants.FieldConstants; +import frc.robot.RobotObserver; +import frc.robot.vision.CameraIO; +import frc.robot.vision.CameraIO.CameraIOInputs; +import frc.robot.vision.CameraIOInputsLogger; + +public class SingleInputPoseEstimator implements Runnable { + private final Logger m_logger = LoggerFactory.getLogger(SingleInputPoseEstimator.class); + + private final CameraIO m_io; + private final CameraIOInputs m_inputs; + private final CameraIOInputsLogger m_inputsLogger; + + private final Consumer m_reporter; + private Pose2d m_lastPose; + + private final PhotonPoseEstimator m_estimator; + private final MultiInputFilter m_filter; + + private final Alert m_disconnectedAlert; + + private final String m_name; + private final Transform3d m_robotToCamera; + + public SingleInputPoseEstimator( + MultiInputFilter fitler, + CameraIO io, + String name, + Transform3d robotToCamera, + Consumer updateCallback) { + m_io = io; + m_name = name; + m_inputs = new CameraIOInputs(); + m_inputsLogger = new CameraIOInputsLogger(m_inputs, name); + m_reporter = updateCallback; + m_robotToCamera = robotToCamera; + m_filter = fitler; + m_disconnectedAlert = + new Alert("Vision/Camera Status", name + " disconnected", AlertType.kError); + m_estimator = new PhotonPoseEstimator( + LocalizationConstants.kTagLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + m_robotToCamera); + m_estimator.setMultiTagFallbackStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + } + + public void refresh(Pose2d robotPose) { + m_lastPose = robotPose; + m_io.updateInputs(m_inputs); + m_inputsLogger.log(); + m_disconnectedAlert.set(!m_inputs.connected); + if (!m_inputs.connected) { + return; + } + for (PhotonPipelineResult result : m_inputs.unreadResults) { + Set tags = result.getTargets().stream() + .map(target -> target.getFiducialId()) + .collect(Collectors.toSet()); + m_filter.addInput(m_name, tags); + } + } + + @Override + public void run() { + if (!m_inputs.connected) { + return; + } + // Pull the latest data from the camera. + List results = m_inputs.unreadResults; + m_estimator.addHeadingData( + RobotController.getMeasureTime().in(Seconds), + m_lastPose.getRotation()); + /* take many */ + for (PhotonPipelineResult result : results) { + combinedHandleResult(result); + } + } + + private void combinedHandleResult(PhotonPipelineResult result) { + // some prechecks before we do anything + if (!precheckValidity(result)) { + return; + } + // we can now assume that we have targets + List targets = result.getTargets(); + // use solvePnP every time if we can + Optional est = m_estimator.update(result); + if (est.isPresent()) { + Pose3d estimatedPose = est.get().estimatedPose; + process(result, estimatedPose).ifPresent(m_reporter); + } + PhotonTrackedTarget target = targets.get(0); + int fidId = target.getFiducialId(); + Optional targetPosition = LocalizationConstants.kTagLayout + .getTagPose(fidId); + if (targetPosition.isEmpty()) { + m_logger.error("Tag {} detected not in field layout", fidId); + return; + } + + Pose3d targetPosition3d = targetPosition.get(); + Transform3d best3d = target.getBestCameraToTarget(); + Transform3d alt3d = target.getAlternateCameraToTarget(); + Pose3d best = targetPosition3d + .plus(best3d.inverse()) + .plus(m_robotToCamera.inverse()); + Pose3d alt = targetPosition3d + .plus(alt3d.inverse()) + .plus(m_robotToCamera.inverse()); + // final decision maker + double bestHeading = best.getRotation().getZ(); + double altHeading = alt.getRotation().getZ(); + Pose2d pose = m_lastPose; + double heading = pose.getRotation().getRadians(); + Transform2d bestDiff = best.toPose2d().minus(pose); + Transform2d altDiff = alt.toPose2d().minus(pose); + double bestRotErr = Math.abs(bestHeading - heading); + double altRotErr = Math.abs(altHeading - heading); + double bestXYErr = bestDiff.getTranslation().getNorm(); + double altXYErr = altDiff.getTranslation().getNorm(); + Pose3d estimate; + + if (Math.abs(bestRotErr - altRotErr) >= LocalizationConstants.kHeadingThreshold) { + estimate = (bestRotErr <= altRotErr) ? best : alt; + } else { + estimate = (bestXYErr <= altXYErr) ? best : alt; + } + + process(result, estimate).ifPresent(m_reporter); + } + + private boolean precheckValidity(PhotonPipelineResult result) { + double latency = result.metadata.getLatencyMillis() * 1e-3; + if (latency > LocalizationConstants.kLatencyThreshold) { + m_logger.warn("({}) Refused old vision data, latency of {}", m_name, latency); + return false; + } + // Ensure we only accept reef-focused estimates + return result.hasTargets() + && (!LocalizationConstants.kEnableReefFilter + || LocalizationConstants.kReefIds.contains(result.getBestTarget().getFiducialId())); + } + + private Optional process(PhotonPipelineResult result, Pose3d pose) { + double latency = result.metadata.getLatencyMillis() / 1.0e+3; + double timestamp = Utils.getCurrentTimeSeconds() - latency; + double ambiguity = getAmbiguity(result); + Pose2d flatPose = pose.toPose2d(); + Matrix stdDevs = calculateStdDevs(result, flatPose); + + // check validity + if (!checkValidity(pose, ambiguity)) { + return Optional.empty(); + } + return Optional.of( + new TimestampedPoseEstimate(flatPose, timestamp, stdDevs)); + } + + private boolean checkValidity( + Pose3d pose, + double ambiguity) { + if (ambiguity >= LocalizationConstants.kAmbiguityThreshold) { + return false; + } + return !isOutsideField(pose); + } + + private boolean isOutsideField(Pose3d pose) { + double x = pose.getX(); + double y = pose.getY(); + double z = pose.getZ(); + double xMax = LocalizationConstants.kXYMargin.magnitude() + + FieldConstants.kFieldLength.magnitude(); + double yMax = LocalizationConstants.kXYMargin.magnitude() + + FieldConstants.kFieldWidth.magnitude(); + double xyMin = -LocalizationConstants.kXYMargin.magnitude(); + double zMax = LocalizationConstants.kZMargin.magnitude(); + double zMin = -LocalizationConstants.kZMargin.magnitude(); + return x < xyMin + || x > xMax + || y < xyMin + || y > yMax + || z > zMax + || z < zMin; + } + + private Matrix calculateStdDevs(PhotonPipelineResult result, Pose2d pose) { + double latency = result.metadata.getLatencyMillis() * 1e-3; + double multiplier = calculateStdDevMultiplier(result, latency, pose); + return LocalizationConstants.kBaseStdDevs.times(multiplier); + } + + private double calculateStdDevMultiplier( + PhotonPipelineResult result, + double latency, + Pose2d pose) { + double averageTagDistance = 0; + for (PhotonTrackedTarget tag : result.getTargets()) { + averageTagDistance += tag + .getBestCameraToTarget() + .getTranslation() + .getNorm(); + } + averageTagDistance /= result.getTargets().size(); + // calculate tag distance factor + double distanceFactor = Math.max(1, + LocalizationConstants.kDistanceMultiplier + * (averageTagDistance - LocalizationConstants.kNoisyDistance)); + // calculate an (average) ambiguity real quick: + double ambiguity = getAmbiguity(result); + // ambiguity factor + double ambiguityFactor = Math.max(1, + LocalizationConstants.kAmbiguityMultiplier * ambiguity + + LocalizationConstants.kAmbiguityShifter); + // tag divisor + double tags = result.getTargets().size(); + double tagDivisor = 1 + (tags - 1) * LocalizationConstants.kTargetMultiplier; + // distance from last pose + double poseDifferenceError = Math.max(0, + m_lastPose.minus(pose).getTranslation().getNorm() + - LocalizationConstants.kDifferenceThreshold * RobotObserver.getVelocity()); + double diffMultiplier = Math.max(1, + poseDifferenceError * LocalizationConstants.kDifferenceMultiplier); + double timeMultiplier = Math.max(1, latency * LocalizationConstants.kLatencyMultiplier); + // final calculation + double stdDevMultiplier = ambiguityFactor + * distanceFactor + * diffMultiplier + * timeMultiplier + / tagDivisor; + return stdDevMultiplier; + } + + private double getAmbiguity(PhotonPipelineResult result) { + return result.getBestTarget().getPoseAmbiguity(); + } + + public boolean isConnected() { + return m_inputs.connected; + } +} + diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/TimestampedPoseEstimate.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/TimestampedPoseEstimate.java new file mode 100644 index 00000000..97fb3954 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/TimestampedPoseEstimate.java @@ -0,0 +1,12 @@ +package frc.robot.vision.localization; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +public record TimestampedPoseEstimate( + Pose2d pose, + double timestamp, + Matrix stdDevs) { +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java new file mode 100644 index 00000000..c987005b --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java @@ -0,0 +1,115 @@ +package frc.robot.vision.tracking; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Seconds; + +import java.util.List; +import java.util.Optional; +import java.util.function.Consumer; +import java.util.function.Supplier; + +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Robot; +import frc.robot.vision.CameraIO; +import frc.robot.vision.CameraIO.CameraIOInputs; +import frc.robot.vision.CameraIOHardware; +import frc.robot.vision.CameraIOInputsLogger; + +public class AlgaeTracker implements Runnable { + + public static final boolean enabled = TrackingConstants.kEnabled; + + public record ObjectTrackingStatus(Rotation2d yaw, double time, Optional pose) { + public boolean isExpired() { + return Timer.getTimestamp() - time() >= TrackingConstants.kExpirationTime.in(Seconds); + } + } + + @SuppressWarnings("unused") + private final Logger m_logger = LoggerFactory.getLogger(AlgaeTracker.class); + + private final CameraIO m_io; + private final CameraIOInputs m_inputs = new CameraIOInputs(); + private final CameraIOInputsLogger m_inputsLogger; + + private final Consumer m_action; + + private final Supplier m_robotPose; + + public AlgaeTracker(Supplier robotPose, Consumer action) { + if (Robot.isSimulation()) { + m_io = new CameraIOTrackingSim( + TrackingConstants.kCameraName, + TrackingConstants.kRobotToCamera, + robotPose); + } else { + m_io = new CameraIOHardware(TrackingConstants.kCameraName); + } + m_action = action; + m_robotPose = robotPose; + m_inputsLogger = new CameraIOInputsLogger(m_inputs, TrackingConstants.kCameraName); + } + + public void run() { + m_io.updateInputs(m_inputs); + m_inputsLogger.log(); + + List results = m_inputs.unreadResults; + + results.forEach(result -> { + if (!result.hasTargets()) { + return; + } + PhotonTrackedTarget target = result.getBestTarget(); + Rotation2d yaw = Rotation2d.fromDegrees(-result.getBestTarget().yaw); + + Optional estimatedDistance = estimateDistance(target); + + estimatedDistance.ifPresentOrElse(dist -> { + Transform2d robotOffset = + new Transform2d(new Translation2d(yaw.getCos(), yaw.getSin()).times(dist), Rotation2d.kZero); + Pose2d algae = m_robotPose.get().transformBy(robotOffset); + m_action.accept(new ObjectTrackingStatus( + yaw, + Timer.getTimestamp(), + Optional.of(algae))); + }, () -> { + m_action.accept(new ObjectTrackingStatus( + yaw, + Timer.getTimestamp(), + Optional.empty())); + }); + + }); + } + + private Optional estimateDistance(PhotonTrackedTarget target) { + if (!TrackingConstants.kDistanceEstimationEnabled) { + return Optional.empty(); + } + if (target.pitch < 0) { + return estimateDistance(target.pitch, TrackingConstants.kGroundAlgaeHeight); + } + if (target.pitch > 0) { + return estimateDistance(target.pitch, TrackingConstants.kLollipopAlgaeHeight); + } + // The algae could not be detected + return Optional.empty(); + } + + private Optional estimateDistance(double pitch, Distance targetHeight) { + double diff = TrackingConstants.kRobotToCamera.getZ() - targetHeight.in(Meters); + return Optional.of(Math.abs(diff / Math.sin(Units.degreesToRadians(pitch)))); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java new file mode 100644 index 00000000..117eb9cf --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java @@ -0,0 +1,77 @@ +package frc.robot.vision.tracking; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Seconds; +import java.util.function.Supplier; +import org.photonvision.PhotonCamera; +import org.photonvision.estimation.TargetModel; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.simulation.VisionTargetSim; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotObserver; +import frc.robot.vision.CameraIO; + +public class CameraIOTrackingSim implements CameraIO { + private final String m_name; + + private final PhotonCamera m_camera; + + private final Supplier m_robotPose; + + private final static Transform3d offset = + new Transform3d(0, 0, TrackingConstants.kGroundAlgaeHeight.in(Meters), Rotation3d.kZero); + + // simulation stuff only happens once + private static final VisionSystemSim visionSim = new VisionSystemSim("tracking"); + private static final TargetModel targetModel = new TargetModel(0.5); + private static final Pose3d initialTargetPose = new Pose3d(2, 2, 0, Rotation3d.kZero); + private static final VisionTargetSim targetSim = + new VisionTargetSim(initialTargetPose, targetModel); + private static final SimCameraProperties simProps = new SimCameraProperties() + .setFPS(TrackingConstants.kFPS) + .setCalibration( + TrackingConstants.kResWidth, + TrackingConstants.kResHeight, + TrackingConstants.kFOVDiag) + .setCalibError( + TrackingConstants.kCalibError, + TrackingConstants.kCalibStdDevs) + .setAvgLatencyMs(TrackingConstants.kLatency.in(Seconds)) + .setLatencyStdDevMs(TrackingConstants.kLatencyStdDevs.in(Seconds)); + + // Add the target to the simulation + static { + visionSim.addVisionTargets(targetSim); + } + + public CameraIOTrackingSim( + String cameraName, + Transform3d robotToCamera, + Supplier robotPose) { + m_robotPose = robotPose; + m_name = cameraName; + SmartDashboard.putBoolean("Vision/" + m_name + " connected", true); + + m_camera = new PhotonCamera(cameraName); + PhotonCameraSim cameraSim = new PhotonCameraSim(m_camera, simProps); + visionSim.addCamera(cameraSim, robotToCamera); + + RobotObserver.getField().getObject("Tracked Object").setPose(initialTargetPose.toPose2d()); + } + + public void updateInputs(CameraIOInputs inputs) { + inputs.connected = SmartDashboard.getBoolean("Vision/" + m_name + " connected", true); + + targetSim.setPose( + new Pose3d(RobotObserver.getField().getObject("Tracked Object").getPose()).plus(offset)); + visionSim.update(m_robotPose.get()); + + inputs.unreadResults = m_camera.getAllUnreadResults(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java new file mode 100644 index 00000000..deda9daf --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java @@ -0,0 +1,39 @@ +package frc.robot.vision.tracking; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Milliseconds; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Time; + +class TrackingConstants { + public static final boolean kEnabled = false; + + protected static final String kCameraName = "camera"; + /* it is very important that the robot's position be ON THE GROUND (z = 0) */ + protected static final Transform3d kRobotToCamera = new Transform3d(0, 0, 0.3, Rotation3d.kZero); + + /* simulation */ + protected static final double kFPS = 14; + protected static final int kResWidth = 640; + protected static final int kResHeight = 480; + protected static final Rotation2d kFOVDiag = Rotation2d.fromDegrees(100); + + protected static final double kCalibError = 0.5; + protected static final double kCalibStdDevs = 0.3; + protected static final Time kLatency = Milliseconds.of(17); + protected static final Time kLatencyStdDevs = Milliseconds.of(6); + + /* + * algae location constants - these measure the distance from the ground to the center of the + * algae + */ + protected static final Distance kGroundAlgaeHeight = Inches.of(8.125); + protected static final Distance kLollipopAlgaeHeight = Inches.of(14.0625); + + protected static final boolean kDistanceEstimationEnabled = false; + + protected static final Time kExpirationTime = Milliseconds.of(300); +} diff --git a/ThriftyTest/src/main/resources/logback.xml b/ThriftyTest/src/main/resources/logback.xml index b95fdb92..6582a1b1 100644 --- a/ThriftyTest/src/main/resources/logback.xml +++ b/ThriftyTest/src/main/resources/logback.xml @@ -6,17 +6,26 @@ + + + + + + + - - - - - + + + + + + + diff --git a/ThriftyTest/vendordeps/Autopilot.json b/ThriftyTest/vendordeps/Autopilot.json new file mode 100644 index 00000000..115430ef --- /dev/null +++ b/ThriftyTest/vendordeps/Autopilot.json @@ -0,0 +1,20 @@ +{ + "name": "Autopilot", + "uuid": "4e922d47-9954-4db5-929f-c708cc62e152", + "fileName": "Autopilot.json", + "version": "1.2.1", + "frcYear": "2025", + "jsonUrl": "https://therekrab.github.io/autopilot/vendordep.json", + "mavenUrls": [ + "https://jitpack.io" + ], + "javaDependencies": [ + { + "groupId": "com.github.therekrab", + "artifactId": "autopilot", + "version": "1.2.1" + } + ], + "cppDependencies": [], + "jniDependencies": [] +} diff --git a/ThriftyTest/vendordeps/PathplannerLib-2025.2.6.json b/ThriftyTest/vendordeps/PathplannerLib-2025.2.7.json similarity index 87% rename from ThriftyTest/vendordeps/PathplannerLib-2025.2.6.json rename to ThriftyTest/vendordeps/PathplannerLib-2025.2.7.json index 95ba2033..d3f84e53 100644 --- a/ThriftyTest/vendordeps/PathplannerLib-2025.2.6.json +++ b/ThriftyTest/vendordeps/PathplannerLib-2025.2.7.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.6.json", + "fileName": "PathplannerLib-2025.2.7.json", "name": "PathplannerLib", - "version": "2025.2.6", + "version": "2025.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.6" + "version": "2025.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.6", + "version": "2025.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/ThriftyTest/vendordeps/Phoenix6-25.3.2.json b/ThriftyTest/vendordeps/Phoenix6-frc2025-latest.json similarity index 86% rename from ThriftyTest/vendordeps/Phoenix6-25.3.2.json rename to ThriftyTest/vendordeps/Phoenix6-frc2025-latest.json index 29187a8b..6f40c840 100644 --- a/ThriftyTest/vendordeps/Phoenix6-25.3.2.json +++ b/ThriftyTest/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.3.2.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.3.2", + "version": "25.4.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.3.2" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +382,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +398,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +446,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -444,6 +458,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file diff --git a/ThriftyTest/vendordeps/photonlib.json b/ThriftyTest/vendordeps/photonlib.json index 1219919e..2d7b1d8e 100644 --- a/ThriftyTest/vendordeps/photonlib.json +++ b/ThriftyTest/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.2.1", + "version": "v2025.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.2.1", + "version": "v2025.3.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.2.1", + "version": "v2025.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.2.1", + "version": "v2025.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.2.1" + "version": "v2025.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.2.1" + "version": "v2025.3.1" } ] } \ No newline at end of file diff --git a/adantage_scope_custom_assets/Joystick_DragonReins/config.json b/adantage_scope_custom_assets/Joystick_DragonReins/config.json new file mode 100644 index 00000000..317c8772 --- /dev/null +++ b/adantage_scope_custom_assets/Joystick_DragonReins/config.json @@ -0,0 +1,43 @@ +{ + "name": "Dragon Reins", + "components": [ + { + "type": "button", + "isYellow": true, + "isEllipse": false, + "centerPx": [1600,600], + "sizePx": [200,200], + "sourceIndex": 4 + }, + { + "type": "button", + "isYellow": true, + "isEllipse": false, + "centerPx": [2400, 600], + "sizePx": [200,200], + "sourceIndex": 3 + }, + { + "type": "joystick", + "isYellow": false, + "centerPx": [1300,1300], + "radiusPx": 200, + "xSourceIndex": 0, + "xSourceInverted": false, + "ySourceIndex": 1, + "ySourceInverted": false, + "buttonSourceIndex": 2 + }, + { + "type": "joystick", + "isYellow": false, + "centerPx": [2900,1200], + "radiusPx": 200, + "xSourceIndex": 3, + "ySourceIndex": 2, + "xSourceInverted": true, + "ySourceInverted": false, + "buttonSourceIndex": 1 + } + ] +} diff --git a/adantage_scope_custom_assets/Joystick_DragonReins/image.png b/adantage_scope_custom_assets/Joystick_DragonReins/image.png new file mode 100644 index 00000000..a2e1d532 Binary files /dev/null and b/adantage_scope_custom_assets/Joystick_DragonReins/image.png differ diff --git a/resources/algae.jpg b/resources/algae.jpg new file mode 100644 index 00000000..27f83f74 Binary files /dev/null and b/resources/algae.jpg differ