From 8f1e7f476c64b8c7722e80781133da99c7b40d2f Mon Sep 17 00:00:00 2001 From: Gregory Kelly Date: Wed, 9 Apr 2025 23:59:59 -0400 Subject: [PATCH 1/8] Add basic annotation logging structure --- src/main/java/frc/robot/Robot.java | 7 +++++++ src/main/java/frc/robot/RobotContainer.java | 2 ++ 2 files changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 687a0a0..79c9890 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,9 @@ package frc.robot; +import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -14,9 +17,11 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ +@Logged(strategy = Strategy.OPT_IN) public class Robot extends TimedRobot { private Command m_autonomousCommand; + @Logged(name="Container") private RobotContainer m_robotContainer; /** @@ -25,6 +30,8 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { + Epilogue.bind(this); + // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f10e4cb..18406d9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -18,6 +19,7 @@ import frc.robot.commands.auto.DriveTowardsRing; +@Logged public class RobotContainer { /** CREATE SUBSYSTEMS */ From 7b9eae152760bb17799b00be23d36e3380d8c5f9 Mon Sep 17 00:00:00 2001 From: Gregory Kelly Date: Thu, 10 Apr 2025 00:00:38 -0400 Subject: [PATCH 2/8] Add TalonSRX for crusher --- src/main/java/frc/robot/RobotMap.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index faeb8bf..a684c7d 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -1,11 +1,9 @@ package frc.robot; import com.ctre.phoenix.motorcontrol.can.BaseMotorController; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.VictorSPX; -import edu.wpi.first.wpilibj.motorcontrol.MotorController; -import edu.wpi.first.wpilibj.motorcontrol.VictorSP; - public class RobotMap { private static final int LEFT_DRIVE_1 = 4; @@ -13,11 +11,15 @@ public class RobotMap { private static final int RIGHT_DRIVE_1 = 1; private static final int RIGHT_DRIVE_2 = 2; + private static final int CRUSHER_MOTOR = 5; + public static final BaseMotorController leftDrive1; public static final BaseMotorController leftDrive2; public static final BaseMotorController rightDrive1; public static final BaseMotorController rightDrive2; + public static final TalonSRX crusherMotor; + static { leftDrive1 = new VictorSPX(LEFT_DRIVE_1); leftDrive2 = new VictorSPX(LEFT_DRIVE_2); @@ -26,5 +28,7 @@ public class RobotMap { rightDrive1 = new VictorSPX(RIGHT_DRIVE_1); rightDrive2 = new VictorSPX(RIGHT_DRIVE_2); rightDrive2.follow(rightDrive1); + + crusherMotor = new TalonSRX(CRUSHER_MOTOR); } } From e88c153e0404801b2c608897bf518a48bccda83c Mon Sep 17 00:00:00 2001 From: Gregory Kelly Date: Thu, 10 Apr 2025 00:01:28 -0400 Subject: [PATCH 3/8] Add subsystem and a dummy command for crusher --- src/main/java/frc/robot/RobotContainer.java | 6 +- .../robot/commands/CrushDefaultCommand.java | 43 ++++++++++++ .../java/frc/robot/subsystems/Crusher.java | 70 +++++++++++++++++++ 3 files changed, 117 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/CrushDefaultCommand.java create mode 100644 src/main/java/frc/robot/subsystems/Crusher.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18406d9..e8c08e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,8 @@ /* IMPORT SUBSYSTEMS */ import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Camera; - +import frc.robot.subsystems.Crusher; +import frc.robot.commands.CrushDefaultCommand; /* IMPORT COMMANDS */ import frc.robot.commands.DriveCommand; @@ -26,7 +27,7 @@ public class RobotContainer { private final Drivetrain drivetrain = new Drivetrain(RobotMap.leftDrive1, RobotMap.rightDrive1); private final Camera camera = new Camera(); - + public final Crusher crusher = new Crusher(RobotMap.crusherMotor); /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -35,6 +36,7 @@ public RobotContainer() { configureBindings(); drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.leftDriveSupplier, OI.rightDriveSupplier)); + crusher.setDefaultCommand(new CrushDefaultCommand(crusher)); } /** diff --git a/src/main/java/frc/robot/commands/CrushDefaultCommand.java b/src/main/java/frc/robot/commands/CrushDefaultCommand.java new file mode 100644 index 0000000..dd859d5 --- /dev/null +++ b/src/main/java/frc/robot/commands/CrushDefaultCommand.java @@ -0,0 +1,43 @@ +// 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.Crusher; + +/* 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 CrushDefaultCommand extends Command { + + Crusher crusher; + + /** Creates a new CrushDefaultCommand. */ + public CrushDefaultCommand(Crusher crusher) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(crusher); + + this.crusher = crusher; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + crusher.setOutput(0.5); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Crusher.java b/src/main/java/frc/robot/subsystems/Crusher.java new file mode 100644 index 0000000..48c5652 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Crusher.java @@ -0,0 +1,70 @@ +// 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.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.TalonSRXSimCollection; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.math.numbers.N1; +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.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; + +@Logged(strategy = Strategy.OPT_IN) +public class Crusher extends SubsystemBase { + + TalonSRX talon; + + // 12.75:1 Gearbox, 4:1 belt reduction to flywheel + private static final double GEAR_REDUCTION = 12.75 * 2; + // 11cm ID to 15cm OD radius ring, 20lbs + private static final double MOMENT_OF_INERTIA = 0.5 * Units.lbsToKilograms(20) * (0.11 * 0.11 + 0.15 * 0.15); + + LinearSystem sys = LinearSystemId.createFlywheelSystem(DCMotor.getCIM(1), MOMENT_OF_INERTIA, GEAR_REDUCTION); + FlywheelSim flywheel = new FlywheelSim(sys, DCMotor.getCIM(1)); + TalonSRXSimCollection simTalon; + + /** Creates a new Crusher. */ + public Crusher(TalonSRX talon) { + this.talon = talon; + if (Robot.isSimulation()){ + simTalon = talon.getSimCollection(); + } + } + + public void setOutput(double output){ + talon.set(ControlMode.PercentOutput, output); + } + + @Logged(name="Curent (A)") + public double getCurrent(){ + return talon.getStatorCurrent(); + } + + @Logged(name="Motor Volts (V)") + public double getOutput(){ + return talon.getMotorOutputVoltage(); + } + + @Logged(name="RPM [SIMULATION]") + public double simRPM(){ + return flywheel.getAngularVelocityRPM(); + } + + @Override + public void simulationPeriodic() { + simTalon.setBusVoltage(12); + flywheel.setInputVoltage(simTalon.getMotorOutputLeadVoltage()); + flywheel.update(0.02); + simTalon.setStatorCurrent(flywheel.getCurrentDrawAmps()); + } +} From b214f493b8bb68dc0ab75747edbf274375269580 Mon Sep 17 00:00:00 2001 From: Gregory Kelly Date: Thu, 10 Apr 2025 00:14:22 -0400 Subject: [PATCH 4/8] Switch to WPI_TalonSRX for better sim support --- src/main/java/frc/robot/RobotMap.java | 5 +++-- src/main/java/frc/robot/subsystems/Crusher.java | 8 +++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index a684c7d..fd186f0 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -3,6 +3,7 @@ import com.ctre.phoenix.motorcontrol.can.BaseMotorController; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.VictorSPX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; public class RobotMap { @@ -18,7 +19,7 @@ public class RobotMap { public static final BaseMotorController rightDrive1; public static final BaseMotorController rightDrive2; - public static final TalonSRX crusherMotor; + public static final WPI_TalonSRX crusherMotor; static { leftDrive1 = new VictorSPX(LEFT_DRIVE_1); @@ -29,6 +30,6 @@ public class RobotMap { rightDrive2 = new VictorSPX(RIGHT_DRIVE_2); rightDrive2.follow(rightDrive1); - crusherMotor = new TalonSRX(CRUSHER_MOTOR); + crusherMotor = new WPI_TalonSRX(CRUSHER_MOTOR); } } diff --git a/src/main/java/frc/robot/subsystems/Crusher.java b/src/main/java/frc/robot/subsystems/Crusher.java index 48c5652..57643a4 100644 --- a/src/main/java/frc/robot/subsystems/Crusher.java +++ b/src/main/java/frc/robot/subsystems/Crusher.java @@ -7,6 +7,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.TalonSRXSimCollection; import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Strategy; @@ -22,10 +23,11 @@ @Logged(strategy = Strategy.OPT_IN) public class Crusher extends SubsystemBase { - TalonSRX talon; + // Use WPI_TalonSRX for better simulation support + WPI_TalonSRX talon; // 12.75:1 Gearbox, 4:1 belt reduction to flywheel - private static final double GEAR_REDUCTION = 12.75 * 2; + private static final double GEAR_REDUCTION = 12.75; // 11cm ID to 15cm OD radius ring, 20lbs private static final double MOMENT_OF_INERTIA = 0.5 * Units.lbsToKilograms(20) * (0.11 * 0.11 + 0.15 * 0.15); @@ -34,7 +36,7 @@ public class Crusher extends SubsystemBase { TalonSRXSimCollection simTalon; /** Creates a new Crusher. */ - public Crusher(TalonSRX talon) { + public Crusher(WPI_TalonSRX talon) { this.talon = talon; if (Robot.isSimulation()){ simTalon = talon.getSimCollection(); From 462ec7299ed0d946c0297d72ec7c92be55707e9f Mon Sep 17 00:00:00 2001 From: Gregory Kelly Date: Thu, 10 Apr 2025 21:21:28 -0400 Subject: [PATCH 5/8] Create Mechanism2D object to represent a flywheel/roller --- .../java/frc/robot/MechanismFlywheel2d.java | 74 +++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 src/main/java/frc/robot/MechanismFlywheel2d.java diff --git a/src/main/java/frc/robot/MechanismFlywheel2d.java b/src/main/java/frc/robot/MechanismFlywheel2d.java new file mode 100644 index 0000000..805d813 --- /dev/null +++ b/src/main/java/frc/robot/MechanismFlywheel2d.java @@ -0,0 +1,74 @@ +package frc.robot; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringTopic; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismObject2d; + +public class MechanismFlywheel2d extends MechanismObject2d { + + StringPublisher dummyTypePub; + // Only line seems to be supported by glass/advantageScope, so use that + private static String type = "line"; + + private double radius; + private double numSpokes; + + private double currentAngle = 0; + private long lastUpdate = System.currentTimeMillis(); + private double RPM = 0; + + private MechanismLigament2d[] spokes; + + public MechanismFlywheel2d(String name, double radius, int numSpokes) { + super(name); + + this.radius = radius; + this.numSpokes = numSpokes; + + spokes = new MechanismLigament2d[numSpokes]; + for(int i = 0; i < numSpokes; i++){ + MechanismLigament2d spoke = new MechanismLigament2d("Spoke " + i, radius, 0); + append(spoke); + spokes[i] = spoke; + } + update(); + } + + public synchronized void setRPM(double RPM) { + this.RPM = RPM; + } + + public void update(){ + long time = System.currentTimeMillis(); + currentAngle += RPM * 360 * (time - lastUpdate) / 60; + lastUpdate = time; + + + double dTheta = 360 / numSpokes; + for(int i = 0; i < numSpokes; i++) { + spokes[i].setAngle(i * dTheta + currentAngle); + } + } + + @Override + protected void updateEntries(NetworkTable table) { + if(dummyTypePub != null){ + dummyTypePub.close(); + } + dummyTypePub = table.getStringTopic(".type").publishEx(StringTopic.kTypeString, "{\"SmartDashboard\":\"" + type + "\"}"); + dummyTypePub.set(type); + } + + @Override + public void close(){ + super.close(); + if(dummyTypePub != null){ + dummyTypePub.close(); + } + for (MechanismLigament2d spoke : spokes) { + spoke.close(); + } + } +} From d868e8cb2242b8031f8c01770848c98f17fa8e5d Mon Sep 17 00:00:00 2001 From: Gregory Kelly Date: Thu, 10 Apr 2025 21:23:23 -0400 Subject: [PATCH 6/8] Add flywheel display --- src/main/java/frc/robot/subsystems/Crusher.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Crusher.java b/src/main/java/frc/robot/subsystems/Crusher.java index 57643a4..1d324fd 100644 --- a/src/main/java/frc/robot/subsystems/Crusher.java +++ b/src/main/java/frc/robot/subsystems/Crusher.java @@ -35,12 +35,21 @@ public class Crusher extends SubsystemBase { FlywheelSim flywheel = new FlywheelSim(sys, DCMotor.getCIM(1)); TalonSRXSimCollection simTalon; + Mechanism2d display = new Mechanism2d(3, 3); + MechanismFlywheel2d flywheelDisplay; + /** Creates a new Crusher. */ public Crusher(WPI_TalonSRX talon) { this.talon = talon; if (Robot.isSimulation()){ simTalon = talon.getSimCollection(); } + + MechanismRoot2d root = display.getRoot("Flywheel Root", 1.5, 1.5); + flywheelDisplay = new MechanismFlywheel2d("Flywheel", 0.15, 6); + root.append(flywheelDisplay); + + SmartDashboard.putData("Flywheel", display); } public void setOutput(double output){ @@ -68,5 +77,8 @@ public void simulationPeriodic() { flywheel.setInputVoltage(simTalon.getMotorOutputLeadVoltage()); flywheel.update(0.02); simTalon.setStatorCurrent(flywheel.getCurrentDrawAmps()); + + flywheelDisplay.update(); + flywheelDisplay.setRPM(flywheel.getAngularVelocityRPM()); } } From d025f436cbc9c35d6a0d0b8584589fb49c07c50a Mon Sep 17 00:00:00 2001 From: Gregory Kelly Date: Thu, 10 Apr 2025 22:32:20 -0400 Subject: [PATCH 7/8] Fix flywheel display RPM --- src/main/java/frc/robot/MechanismFlywheel2d.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/MechanismFlywheel2d.java b/src/main/java/frc/robot/MechanismFlywheel2d.java index 805d813..c8c66fd 100644 --- a/src/main/java/frc/robot/MechanismFlywheel2d.java +++ b/src/main/java/frc/robot/MechanismFlywheel2d.java @@ -40,9 +40,13 @@ public synchronized void setRPM(double RPM) { this.RPM = RPM; } + public synchronized double getAccumulatedAngle() { + return currentAngle; + } + public void update(){ long time = System.currentTimeMillis(); - currentAngle += RPM * 360 * (time - lastUpdate) / 60; + currentAngle += RPM * 360 * (time - lastUpdate) / 1000 / 60; lastUpdate = time; From b37bcba5ab7018667c22ed323e497026d5484e10 Mon Sep 17 00:00:00 2001 From: Gregory Kelly Date: Thu, 10 Apr 2025 22:32:52 -0400 Subject: [PATCH 8/8] Add graphical representation of crushing linkage --- .../java/frc/robot/subsystems/Crusher.java | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Crusher.java b/src/main/java/frc/robot/subsystems/Crusher.java index 1d324fd..0893e16 100644 --- a/src/main/java/frc/robot/subsystems/Crusher.java +++ b/src/main/java/frc/robot/subsystems/Crusher.java @@ -18,6 +18,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.MechanismFlywheel2d; import frc.robot.Robot; @Logged(strategy = Strategy.OPT_IN) @@ -37,6 +38,16 @@ public class Crusher extends SubsystemBase { Mechanism2d display = new Mechanism2d(3, 3); MechanismFlywheel2d flywheelDisplay; + MechanismLigament2d crankshaft; + MechanismLigament2d crankArm; + MechanismLigament2d crushArm; + + private double crankshaftRatio = 1 / 4d; + private double crankshaftLength = 0.15; + private double crankArmlength = 0.25; + private double crushArmLength = 0.25; + + public double crushPos; /** Creates a new Crusher. */ public Crusher(WPI_TalonSRX talon) { @@ -49,6 +60,14 @@ public Crusher(WPI_TalonSRX talon) { flywheelDisplay = new MechanismFlywheel2d("Flywheel", 0.15, 6); root.append(flywheelDisplay); + MechanismRoot2d crankshaftRoot = display.getRoot("Crankshaft Root", 1.5, 1); + crankshaft = new MechanismLigament2d("Crankshaft", crankshaftLength, 0); + crankArm = new MechanismLigament2d("Crank Arm", crankArmlength, 0); + crushArm = new MechanismLigament2d("Crush Arm", crushArmLength, 0); + crankshaftRoot.append(crankshaft); + crankshaft.append(crankArm); + crankArm.append(crushArm); + SmartDashboard.putData("Flywheel", display); } @@ -80,5 +99,16 @@ public void simulationPeriodic() { flywheelDisplay.update(); flywheelDisplay.setRPM(flywheel.getAngularVelocityRPM()); + + double crankshaftAngle = (flywheelDisplay.getAccumulatedAngle() * crankshaftRatio) % 360; + crankshaft.setAngle(crankshaftAngle); + double armAngle = Math.toDegrees(-Math.asin(crankshaftLength / crankArmlength * Math.sin(Math.toRadians(crankshaftAngle)))) - crankshaftAngle; + crankArm.setAngle(armAngle); + double crushAngle = -(crankshaftAngle + armAngle); + crushArm.setAngle(crushAngle); + + // Find the overall distance from rotation point to crusher end + crushPos = crankshaftLength * Math.cos(Math.toRadians(crankshaftAngle)) + crankArmlength * Math.cos(Math.toRadians(crankshaftAngle + armAngle)) + crushArmLength; + SmartDashboard.putNumber("Crush Pos", crushPos); } }