From d43a9e6f24ca2a87a237fbcc3cc8cc1b7dff39a6 Mon Sep 17 00:00:00 2001 From: Mark Wood Date: Thu, 4 Jan 2024 21:19:41 -0500 Subject: [PATCH] Finished Challenge #5 with the boys --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 31 +++-- src/main/java/frc/robot/RobotContainer.java | 14 ++- .../java/frc/robot/oi/DriverControls.java | 4 + .../oi/DriverControlsDualFlightStick.java | 10 ++ .../robot/subsystems/flywheel/Flywheel.java | 51 ++++++++ .../robot/subsystems/flywheel/FlywheelIO.java | 24 ++++ .../subsystems/flywheel/FlywheelIOSim.java | 69 +++++++++++ .../Flywheelelelelelleleleelelelelelelellele, | 115 ++++++++++++++++++ 9 files changed, 307 insertions(+), 13 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/flywheel/Flywheel.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/Flywheelelelelelleleleelelelelelelellele, diff --git a/build.gradle b/build.gradle index 5b370f1..8c00896 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2023.4.3" id "com.peterabeles.gversion" version "1.10" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a4d0567..3e2b03f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,15 +31,15 @@ import frc.robot.util.TunableNumber; /** - * The Constants class provides a convenient place for teams to hold robot-wide - * numerical or boolean constants. This class should not be used for any other - * purpose. All constants should be declared globally (i.e. public static). Do - * not put anything functional in this class. - * - *

- * It is advised to statically import this class (or one of its inner classes) - * wherever the constants are needed, to reduce verbosity. - */ +* The Constants class provides a convenient place for teams to hold robot-wide +* numerical or boolean constants. This class should not be used for any other +* purpose. All constants should be declared globally (i.e. public static). Do +* not put anything functional in this class. +* +*

+* It is advised to statically import this class (or one of its inner classes) +* wherever the constants are needed, to reduce verbosity. +*/ public final class Constants { public static final boolean tuningMode = true; @@ -78,7 +78,7 @@ public static final class Setpoints { public static final ExtendedPathPoint blueRightWallLoadingStation = new ExtendedPathPoint( new Translation2d(15.8, 6.0), new Rotation2d(), Rotation2d.fromDegrees(0)); - // Grid is labeled first to third from edge of field without + // Grid is labeled first to third from edge of field without public static final ExtendedPathPoint blueFirstGridLeftCone = new ExtendedPathPoint( new Translation2d(1.84 + .4, 0.43), new Rotation2d(), Rotation2d.fromDegrees(180)); @@ -152,6 +152,17 @@ public static final class LEDConstants { public static final Color kChargedUpGold = new Color(255, 183, 0); } + public static final class FlywheelConstants { + public static final double gearRatio = 4.0; + public static final double tolerance = 0.008; + //what is a feedforward + //not in my america + public static final TunableNumber kP = new TunableNumber("Flywheel P", 14); + public static final TunableNumber kI = new TunableNumber("Flywheel I", 1.6); + public static final TunableNumber kD = new TunableNumber("Flywheel D", 0.5); + public static final PIDController flywheelController = new PIDController(kP.get(), kI.get(), kD.get()); + } + public static final class ElevatorConstants { public static final boolean kTuningMode = true; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9551296..3e8de31 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,6 +35,7 @@ import frc.lib.utils.FieldGeomUtil; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.ElevatorConstants; +import frc.robot.Constants.FlywheelConstants; import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.Ports; import frc.robot.Constants.RobotConstants; @@ -58,6 +59,8 @@ import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorIONeo; import frc.robot.subsystems.elevator.ElevatorIOSim; +import frc.robot.subsystems.flywheel.Flywheel; +import frc.robot.subsystems.flywheel.FlywheelIOSim; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIONeo550; import frc.robot.subsystems.intake.IntakeIOSim; @@ -88,6 +91,7 @@ public class RobotContainer { private LED m_LED; private String m_curSelectedAuto = "none"; private List m_traj; + private Flywheel m_flywheel; //im on a whole nother level im geekin // private LED m_LED2; // Dashboard inputs @@ -212,6 +216,8 @@ private void configureSubsystems() { m_LED = new LED(Constants.LEDConstants.kLEDPort, Constants.LEDConstants.kLEDLength); // m_LED2 = new LED(Constants.LEDConstants.kLEDPort2, Constants.LEDConstants.kLEDLength); m_robotState = RobotState.startInstance(m_drive, m_intake, m_elevator, m_wrist); + m_flywheel = new Flywheel(new FlywheelIOSim(), Constants.FlywheelConstants.flywheelController, + FlywheelConstants.tolerance); //RADunancy } } @@ -349,13 +355,13 @@ private void configureButtonBindings() { driverControls.resetFieldCentric().onTrue(m_drive.resetCommand(new Pose2d(1.81, 6.9, new Rotation2d()))); driverControls.startIntakeConeInCubeOut().whileTrue(m_intake.intakeConeCommand()); - driverControls.startIntakeCubeInConeOut().whileTrue(m_intake.intakeCubeCommand()); + //driverControls.startIntakeCubeInConeOut().whileTrue(m_intake.intakeCubeCommand()); driverControls.zeroElevator().onTrue(m_elevator.zeroHeightCommand()); // driverControls.setpointHighCone().onTrue(coneHighCommand); // driverControls.setpointMidCube().onTrue(cubeMidCommand); // driverControls.setpointHighCube().onTrue(cubeHighCommand); driverControls.intakeTippedCone().onTrue(pickUpConeGroundCommandDriver); - driverControls.intakeVerticalCone().onTrue(pickUpConeVerticalCommandDriver); + //driverControls.intakeVerticalCone().onTrue(pickUpConeVerticalCommandDriver); driverControls.setpointIntakeGroundCube().onTrue(pickUpCubeGroundCommandDriver); // driverControls.intakeFromLoadingStation().onTrue(intakeFromLoadingStationCommand); driverControls.autoIntakeCube().whileTrue(intakeCubeTeleop); @@ -381,6 +387,10 @@ private void configureButtonBindings() { driverControls.goToNode().whileTrue(driveToGridSetpointCommand); driverControls.ledFlash().onTrue(m_LED.startFlash()); + + driverControls.flywheelOnTop().onTrue(m_flywheel.setVelocityCommand(9)); + + driverControls.flywheelPLEASESTOP().onTrue(m_flywheel.setVelocityCommand(0)); } public void onEnabled() { diff --git a/src/main/java/frc/robot/oi/DriverControls.java b/src/main/java/frc/robot/oi/DriverControls.java index 360efb2..540a950 100644 --- a/src/main/java/frc/robot/oi/DriverControls.java +++ b/src/main/java/frc/robot/oi/DriverControls.java @@ -55,4 +55,8 @@ public interface DriverControls { public Trigger ledFlash(); + public Trigger flywheelOnTop(); + + public Trigger flywheelPLEASESTOP(); + } diff --git a/src/main/java/frc/robot/oi/DriverControlsDualFlightStick.java b/src/main/java/frc/robot/oi/DriverControlsDualFlightStick.java index 786722b..138b9a9 100644 --- a/src/main/java/frc/robot/oi/DriverControlsDualFlightStick.java +++ b/src/main/java/frc/robot/oi/DriverControlsDualFlightStick.java @@ -153,4 +153,14 @@ public Trigger autoIntakeCube() { public Trigger ledFlash() { return m_rightJoystick.button(8); } + + @Override + public Trigger flywheelOnTop() { + return m_rightJoystick.button(1); + } + + @Override + public Trigger flywheelPLEASESTOP() { + return m_rightJoystick.button(2); + } } diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java new file mode 100644 index 0000000..aa42c95 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems.flywheel; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Flywheel extends SubsystemBase { + private FlywheelIO m_io; + private PIDController m_controller; + public FlywheelInputsAutoLogged m_inputs; + + public Flywheel(FlywheelIO io, PIDController controller, double tolerance) { + m_io = io; + m_controller = controller; + m_controller.setTolerance(tolerance); + m_inputs = new FlywheelInputsAutoLogged(); + } + + @Override + public void periodic() { + m_io.updateInputs(m_inputs); + double burblesquirp = m_controller.calculate(m_inputs.velocityMetersPerSecNo); + m_io.setVoltageNo(burblesquirp); + //CODING TOO EASY + Logger.getInstance().processInputs("flywheel", m_inputs); //big as the super bowl but the difference is it's just two guys playing what they did in the studio + Logger.getInstance().recordOutput("flywheel/voltage", burblesquirp); //shoprya what r ur opinions on drizzy + //Logger.getInstance().recordOutput(); + } + + public void setVelocityM(double velocity) { + m_controller.setSetpoint(velocity); + } + + public void setVelocityRadS(double velocity) { + setVelocityM(velocity * m_io.getWheelLengthYes()); + } + + public void setRevM(double rev) { + setVelocityRadS(rev / (30 / Math.PI)); + } + + public boolean isRight() { + return m_controller.atSetpoint(); + } + + public Command setVelocityCommand(double velocityMS) { + return runOnce(() -> setVelocityM(velocityMS)); + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java new file mode 100644 index 0000000..c8e8224 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.flywheel; //Ash was here + +import org.littletonrobotics.junction.AutoLog; + +import frc.lib.advantagekit.LoggedIO; + +public interface FlywheelIO extends LoggedIO { + + @AutoLog + public class FlywheelInputs { + public double velocityMetersPerSecNo; + public double velocityRadPerSecNo; + } + + public double getVelocityMetersPerSecYes(); + + public double getVelocityRevPerMinYes(); + + public double getVelocityRadPerSecYes(); + + public void setVoltageNo(double voltage); + + public double getWheelLengthYes(); +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java new file mode 100644 index 0000000..6703e44 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java @@ -0,0 +1,69 @@ +package frc.robot.subsystems.flywheel; //\int\limits_{-\infty}^{\infty} \frac{\sin(x)}{x} \, dx = \pi + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; + +public class FlywheelIOSim implements FlywheelIO { + private FlywheelSim m_flywheel; + public final double m_wheelLength; + + public FlywheelIOSim() { + DCMotor gearbox = DCMotor.getFalcon500(2); + double gearing = 3; + double jKgMetersSquared = 16.9876543210987654231; + m_flywheel = new FlywheelSim(gearbox, gearing, jKgMetersSquared); + m_wheelLength = Units.inchesToMeters(9); + } + + public void updateInputs(FlywheelInputs inputs) { + //update the flywheel with the inputs + inputs.velocityMetersPerSecNo = getVelocityMetersPerSecYes(); + inputs.velocityRadPerSecNo = getVelocityRadPerSecYes(); + } + + @Override + public double getVelocityMetersPerSecYes() { + return m_flywheel.getAngularVelocityRadPerSec() * m_wheelLength; + } + + @Override + public double getVelocityRevPerMinYes() { + return m_flywheel.getAngularVelocityRPM(); + } + + @Override + public double getVelocityRadPerSecYes() { + return m_flywheel.getAngularVelocityRadPerSec(); + } + + @Override + public void setVoltageNo(double voltage) { + m_flywheel.setInputVoltage(voltage); + } + + @Override + public double getWheelLengthYes() { + return m_wheelLength; + } +} +/* +List of songs by one of many artists worse than Carti :heart_eyes_cat: +1. Way Out +2. What's Poppin +3. Already Bestfriends +4. Denver +5. They don't love it +6. Routine +7. Sundown +8. Stop Giving Me Advice +9. I WANNA SEE SOME ASS +10. Face of My City +11. your mother +*/ + +/* +1. +2. +3. +*/ diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheelelelelelleleleelelelelelelellele, b/src/main/java/frc/robot/subsystems/flywheel/Flywheelelelelelleleleelelelelelelellele, new file mode 100644 index 0000000..520fffd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheelelelelelleleleelelelelelelellele, @@ -0,0 +1,115 @@ +#they left this here so here is my AoC 2023 select solutions that I like/find jank + + +# AoC d10 s1/2 2023 + +data = [x for x in open("test.txt").read().split("\n")] +p = [[x,y] for x in range(len(data)) for y in range(len(data[0])) if data[x][y] == "S"][0] +minmaxcp = [10000,100000, 0, 0, 0] +while p != [[x,y] for x in range(len(data)) for y in range(len(data[0])) if data[x][y] == "S"][0] or minmaxcp[-1] == 0: + dir = ("u" if data[p[0]-1][p[1]] in ["|", "F", "7"] else "d" if data[p[0]+1][p[1]] in ["|", "L", "J"] else "r" if data[p[0]][p[1]+1] in ["-", "J", "7"] else "l" if data[p[0]][p[1]-1] in ["-", "F", "L"] else "") if p == [[x,y] for x in range(len(data)) for y in range(len(data[0])) if data[x][y] == "S"][0] else (("u" if dir == "l" else "r") if data[p[0]][p[1]] == "L" else ("u" if dir == "r" else "l") if data[p[0]][p[1]] == "J" else ("d" if dir == "r" else "l") if data[p[0]][p[1]] == "7" else ("d" if dir == "l" else "r") if data[p[0]][p[1]] == "F" else dir) + p = [p[0]-1,p[1]] if dir == "u" else [p[0]+1,p[1]] if dir == "d" else [p[0],p[1]+1] if dir == "r" else [p[0],p[1]-1] if dir == "l" else p + minmaxcp = [p[0] if p[0] < minmaxcp[0] else minmaxcp[0], p[1] if p[1] < minmaxcp[1] else minmaxcp[1], p[0] if p[0] > minmaxcp[2] else minmaxcp[2], p[1] if p[1] > minmaxcp[3] else minmaxcp[3], minmaxcp[4] + 1] + minmaxcp[5:] + [p[0:2]] +print("s1", minmaxcp[4], "s2", len([[i,j] for j in range(minmaxcp[1],minmaxcp[3]) for i in range(minmaxcp[0],minmaxcp[2]) if len([x for x in range(j) if data[i][x] in ["|", 'J', 'L', 'S'] and [i,x] in minmaxcp[5:]]) % 2 and [i,j] not in minmaxcp[5:]])) + + +# AoC d11 s1/2 2023 + +import math +data = [x for x in open("data.txt").read().split("\n")] +erow = [i for i in range(len(data)) if [x for x in data[i] if x == "#"] == []] +ecol = [j for j in range(len(data[0])) if [x for x in range(len(data)) if data[x][j] == "#"] == []] +print("s1", int(sum([math.fabs(i[0] - j[0]) + (len([x for x in range(i[0], j[0]) if x in erow]) + len([x for x in range(j[0], i[0]) if x in erow])) + math.fabs(i[1] - j[1]) + (len([x for x in range(i[1], j[1]) if x in ecol]) + len([x for x in range(j[1], i[1]) if x in ecol])) for i,j in [[x,y] for x in [[x,y] for x in range(len(data)) for y in range(len(data[0])) if data[x][y] == "#"] for y in [[x,y] for x in range(len(data)) for y in range(len(data[0])) if data[x][y] == "#"]]])/2), "s2", int(sum([math.fabs(i[0] - j[0]) + 999999 * (len([x for x in range(i[0], j[0]) if x in erow]) + len([x for x in range(j[0], i[0]) if x in erow])) + math.fabs(i[1] - j[1]) + 999999 * (len([x for x in range(i[1], j[1]) if x in ecol]) + len([x for x in range(j[1], i[1]) if x in ecol])) for i,j in [[x,y] for x in [[x,y] for x in range(len(data)) for y in range(len(data[0])) if data[x][y] == "#"] for y in [[x,y] for x in range(len(data)) for y in range(len(data[0])) if data[x][y] == "#"]]])/2)) + + +# AoC d17 s1/2 2023 + + +from enum import Enum +from queue import PriorityQueue + +input = [[int(y) for y in x] for x in open("data.txt").read().split("\n")] +m, n = len(input), len(input[0]) + +stepper = {'u': (-1, 0),'d': (1, 0),'r': (0, 1),'l': (0, -1)} +inv = {"u": "d", 'd':'u', 'r':'l', 'l':'r'} + +class Direction(Enum): + u = (-1,0) + r = (0, 1) + d = (1,0) + l = (0, -1) + + +class Node(): + def __init__(self, cost: int, x: int, y: int, dir: Direction, stepcounter: int): + self.cost = cost + self.x = x + self.y = y + self.dir = dir + self.stepcounter = stepcounter + + def __lt__(self, other): + if self.cost == other.cost: + if self.x == other.x: + if self.y == other.y: + if self.dir == other.dir: + return self.stepcounter < other.stepcounter + return self.dir.value < other.dir.value + return self.y < other.y + return self.x < other.x + return self.cost < other.cost + + def get_info(self): + return(self.x, self.y, self.dir, self.stepcounter) + + +def dijkstra(graph: list[list[int]],min_steps: int, max_steps: int): + + pq: PriorityQueue[Node] = PriorityQueue() + pq.put(Node(0, 0, 0, Direction.r, 1)) + pq.put(Node(0, 0, 0, Direction.d, 1)) + + visited: set[tuple[int, int, Direction, int]] = set() + + while pq: + curr = pq.get() + + if curr.get_info() in visited: + continue + visited.add(curr.get_info()) + + newX = curr.x + curr.dir.value[1] + newY = curr.y + curr.dir.value[0] + + if newX >= len(graph[0]) or newY >= len(graph) or newX < 0 or newY < 0: + continue + + newCost = curr.cost + graph[newY][newX] + + if newX == len(graph[0]) - 1 and newY == len(graph) - 1 and curr.stepcounter + 1 <= max_steps: + return newCost + + for newDir in Direction: + #if opp dir + if newDir.value[0] + curr.dir.value[0] == 0 and newDir.value[1] + curr.dir.value[1] == 0: + continue + #if same dir + if newDir.value[0] == curr.dir.value[0] and newDir.value[1] == curr.dir.value[1]: + newStepCounter = curr.stepcounter + 1 + #if other dir and hasn't done minimum + elif curr.stepcounter < min_steps: + continue + #if other dir and has done minimum + else: + newStepCounter = 1 + if newStepCounter > max_steps: + continue + pq.put(Node(newCost, newX, newY, newDir, newStepCounter)) + return -1 + +print("s1:", dijkstra(input, 0, 3)) + +print("s2:", dijkstra(input, 4, 10)) + +