From b4b07f286de4cf78ad4bcc9f0264131085cab3f2 Mon Sep 17 00:00:00 2001 From: Edgar Ganahl Date: Fri, 1 Dec 2023 16:53:34 -0500 Subject: [PATCH 1/6] Start shooter and random formatting --- simgui.json | 4 +- .../com/stuypulse/robot/RobotContainer.java | 4 + .../stuypulse/robot/commands/TurretPoint.java | 4 +- .../com/stuypulse/robot/constants/Ports.java | 2 + .../stuypulse/robot/constants/Settings.java | 33 +++++++ .../robot/subsystems/shooter/Shooter.java | 91 +++++++++++++++++++ .../robot/subsystems/shooter/ShooterImpl.java | 47 ++++++++++ .../robot/subsystems/shooter/ShooterSim.java | 30 ++++++ .../robot/subsystems/swerve/SwerveDrive.java | 2 - .../robot/subsystems/turret/Turret.java | 3 +- 10 files changed, 213 insertions(+), 7 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java diff --git a/simgui.json b/simgui.json index 56d17ce..d2b6273 100644 --- a/simgui.json +++ b/simgui.json @@ -2,7 +2,9 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/LiveWindow/SimModule": "Subsystem", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro", "/SmartDashboard/Autonomous": "String Chooser", "/SmartDashboard/Field": "Field2d" }, @@ -26,7 +28,7 @@ "NetworkTables": { "transitory": { "SmartDashboard": { - "Odometry": { + "Shooter": { "open": true }, "open": true diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bb2f9bc..7f798e5 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -10,6 +10,8 @@ import com.stuypulse.robot.commands.odometry.OdometryRealign; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Motors.Swerve; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.swerve.evenMoreSwerve.*; import com.stuypulse.robot.subsystems.turret.Turret; import com.stuypulse.stuylib.input.Gamepad; @@ -35,6 +37,8 @@ public class RobotContainer { // Subsystem public final Turret turret = Turret.getInstance(); + public final SwerveDrive swerve = SwerveDrive.getInstance(); + public final Shooter shooter = Shooter.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); diff --git a/src/main/java/com/stuypulse/robot/commands/TurretPoint.java b/src/main/java/com/stuypulse/robot/commands/TurretPoint.java index 6d79d86..c818cc8 100644 --- a/src/main/java/com/stuypulse/robot/commands/TurretPoint.java +++ b/src/main/java/com/stuypulse/robot/commands/TurretPoint.java @@ -1,6 +1,5 @@ package com.stuypulse.robot.commands; -import com.stuypulse.robot.Robot; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.turret.Turret; @@ -32,7 +31,8 @@ public final void execute() { turret.setTargetAngle(0, 360, -360); } - else {turret.setTargetAngle( + else { + turret.setTargetAngle( (180 + Math.toDegrees( Math.atan((robotPose.getY() - target.getY()) / (robotPose.getX() - target.getX())) )), 180, -180 diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 5beba8f..32952c2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -7,6 +7,7 @@ /** This file contains the different ports of motors, solenoids and sensors */ public interface Ports { + public interface Gamepad { int DRIVER = 0; int OPERATOR = 1; @@ -34,4 +35,5 @@ public interface BackRight { int TURN = 17; } } + } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 5334881..7de75fc 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -6,10 +6,13 @@ package com.stuypulse.robot.constants; import com.pathplanner.lib.auto.PIDConstants; +import com.stuypulse.stuylib.control.feedback.PIDController; +// import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.math.Vector2D; import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; @@ -23,12 +26,41 @@ public interface Settings { double DT = 0.02; + public interface Turret { int port = 0; SmartNumber kV = new SmartNumber("Turret kV", 0.1); SmartNumber kA = new SmartNumber("Turret kA", 0.01); } + public interface Shooter { + //Notice how the constants are grouped together in a single class, all CAPS and static + //think about how you would use these constants in your methods + static double MAX_RPM = 5700.0; + static double MIN_RPM = 100.0; + static double MAX_RPM_CHANGE = 2000.0; + static double MAX_RPM_ERROR = 100.0; + static double RING_RPM = 4000.0; + public interface ShooterPID { + double kP = 0.005; + double kI = 0.0; + double kD = 0.00033; + + static PIDController getController() { + return new PIDController(ShooterPID.kP, ShooterPID.kI, ShooterPID.kD); + } + } + public interface ShooterFF { + double kS = 0.17118; + double kV = 0.0020763; + double kA = 0.00011861; + + static SimpleMotorFeedforward getController() { + return new SimpleMotorFeedforward(ShooterFF.kS, ShooterFF.kV, ShooterFF.kA); + } + } + } + public interface Swerve { double WIDTH = Units.inchesToMeters(26.504); double LENGTH = Units.inchesToMeters(20.508); @@ -154,4 +186,5 @@ public interface GyroFeedback { public static Vector2D vpow(Vector2D vec, double power) { return vec.mul(Math.pow(vec.magnitude(), power - 1)); } + } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..554aed9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,91 @@ +package com.stuypulse.robot.subsystems.shooter; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * Shooter class contains the hardware logic for the Shooter class. + * + *

Contains a simple feedforward model of the shooter based on the voltage-balance equation and a + * PID controller to correct for any error. + * + * @author Richie Xue + */ +public abstract class Shooter extends SubsystemBase{ + // Singleton (makes it so that there is only one instance of the Shooter class) + private static final Shooter instance; + + static { + if (RobotBase.isReal()) { + instance = new ShooterImpl(); + } + else { + // it literally does nothing lol + instance = new ShooterSim(); + } + } + + public static Shooter getInstance() { + return instance; + } + // Don't worry about anything above + + private Controller controller; + private double targetRPM; + private final SimpleMotorFeedforward feedforward; + private final Controller feedback; + + + public Shooter() { + + controller = new PIDController(1, 0, 0); + + this.targetRPM = 0.0; + + this.feedforward = Settings.Shooter.ShooterFF.getController(); + this.feedback = Settings.Shooter.ShooterPID.getController(); + } + + public void setTargetRPM(double targetRPM) { + if (targetRPM < Settings.Shooter.MIN_RPM) { + this.targetRPM = 0.0; + } + else if (targetRPM > Settings.Shooter.MAX_RPM) { + this.targetRPM = Settings.Shooter.MAX_RPM; + } + else { + this.targetRPM = targetRPM; + } + } + + public double getTargetRPM() { + return this.targetRPM; + } + + public abstract double getVelocity(); + public abstract void setVoltage(double voltage); + public abstract double getVoltage(); + + @Override + public void periodic() { + if (getTargetRPM() < Settings.Shooter.MIN_RPM) { + this.setVoltage(0.0); + } + else { + // Calculate feedforward and feedback (inputting a desired RPM and outputting needed voltage) + double ff = feedforward.calculate(getTargetRPM()); + double fb = feedback.update(getTargetRPM(), getVelocity()); + this.setVoltage(ff + fb); + } + SmartDashboard.putNumber("Shooter/Target RPM", getTargetRPM()); + periodicallyCalled(); + } + + public abstract void periodicallyCalled(); +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java new file mode 100644 index 0000000..476f83e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -0,0 +1,47 @@ +package com.stuypulse.robot.subsystems.shooter; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; + +/** + * ShooterImpl class contains the hardware logic for the Shooter class. + * + *

Contains a simple feedforward model of the shooter based on the voltage-balance equation and a + * PID controller to correct for any error. + * + * @author Richie Xue + */ +public class ShooterImpl extends Shooter { + private final CANSparkMax motor; + private final RelativeEncoder encoder; + + public ShooterImpl() { + super(); + + this.motor = new CANSparkMax(1, CANSparkMax.MotorType.kBrushless); + this.encoder = motor.getEncoder(); + } + + public double getVelocity() { + return encoder.getVelocity(); + } + + @Override + public void setVoltage(double voltage) { + motor.setVoltage(voltage); + } + + @Override + public double getVoltage() { + return motor.getAppliedOutput(); + } + + @Override + public void periodicallyCalled() { + SmartDashboard.putNumber("Shooter/Velocity", getVelocity()); + SmartDashboard.putNumber("Shooter/Error", getTargetRPM() - getVelocity()); + SmartDashboard.putNumber("Shooter/Voltage", getVoltage()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java new file mode 100644 index 0000000..b5cbac7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java @@ -0,0 +1,30 @@ +package com.stuypulse.robot.subsystems.shooter; + +public class ShooterSim extends Shooter { + + private double velocity; + + public ShooterSim() { + super(); + } + + @Override + public double getVelocity() { + return velocity; + } + + @Override + public void periodicallyCalled() { + velocity = getTargetRPM(); + } + + @Override + public void setVoltage(double voltage) { + return; + } + + @Override + public double getVoltage() { + return 0.0; + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 70e8606..ba1c9be 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -124,8 +124,6 @@ public ChassisSpeeds getChassisSpeeds() { return getKinematics().toChassisSpeeds(getModuleStates()); } - - /** MODULE STATES API **/ public void drive(Vector2D velocity, double omega) { ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java index a707159..cd509e9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java @@ -65,8 +65,7 @@ public final void periodic() { double output = controller.getOutput(); setTurretVoltage(output); - - //SmartDashboard.putNumber("Calculated Voltage", output); + SmartDashboard.putNumber("Calculated Voltage", 0); SmartDashboard.putNumber("Turret Angle", 0); SmartDashboard.putNumber("Target Angle", targetAngle.get()); From e2425822c402efbe3db8fed2fc89a79b79a4302e Mon Sep 17 00:00:00 2001 From: Edgar Ganahl Date: Fri, 1 Dec 2023 16:54:16 -0500 Subject: [PATCH 2/6] Reformat settings, add shooter linear system sim --- .../com/stuypulse/robot/constants/Ports.java | 4 ++++ .../stuypulse/robot/constants/Settings.java | 24 +++++++------------ .../robot/subsystems/shooter/ShooterSim.java | 11 ++++++++- .../robot/subsystems/turret/Turret.java | 3 ++- .../robot/subsystems/turret/TurretSim.java | 2 +- 5 files changed, 25 insertions(+), 19 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 32952c2..2c76186 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -14,6 +14,10 @@ public interface Gamepad { int DEBUGGER = 2; } + public interface Turret { + int TURN = -1; + } + public interface Swerve { public interface FrontRight { int DRIVE = 10; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 7de75fc..28b7dbc 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -28,36 +28,28 @@ public interface Settings { double DT = 0.02; public interface Turret { - int port = 0; - SmartNumber kV = new SmartNumber("Turret kV", 0.1); - SmartNumber kA = new SmartNumber("Turret kA", 0.01); + public interface Feedforward { + SmartNumber kV = new SmartNumber("Turret kV", 0.1); + SmartNumber kA = new SmartNumber("Turret kA", 0.01); + } } public interface Shooter { - //Notice how the constants are grouped together in a single class, all CAPS and static - //think about how you would use these constants in your methods static double MAX_RPM = 5700.0; static double MIN_RPM = 100.0; static double MAX_RPM_CHANGE = 2000.0; static double MAX_RPM_ERROR = 100.0; - static double RING_RPM = 4000.0; - public interface ShooterPID { + + public interface Feedback { double kP = 0.005; double kI = 0.0; double kD = 0.00033; - - static PIDController getController() { - return new PIDController(ShooterPID.kP, ShooterPID.kI, ShooterPID.kD); - } } - public interface ShooterFF { + + public interface Feedforward { double kS = 0.17118; double kV = 0.0020763; double kA = 0.00011861; - - static SimpleMotorFeedforward getController() { - return new SimpleMotorFeedforward(ShooterFF.kS, ShooterFF.kV, ShooterFF.kA); - } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java index b5cbac7..f0d6743 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java @@ -1,11 +1,20 @@ package com.stuypulse.robot.subsystems.shooter; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; + public class ShooterSim extends Shooter { private double velocity; + private LinearSystemSim shooterSim; public ShooterSim() { - super(); + shooterSim = new LinearSystemSim( + LinearSystemId.identifyVelocitySystem(Settings.Shooter.Feedforward.kV, Settings.Shooter.Feedforward.kA) + ); } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java index cd509e9..8626f81 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.subsystems.turret; import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.control.feedback.PIDController; @@ -15,7 +16,7 @@ public abstract class Turret extends SubsystemBase { static { if(Robot.isReal()) { - instance = new TurretImpl(Settings.Turret.port); + instance = new TurretImpl(Ports.Turret.TURN); } else { instance = new TurretSim(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java index 2f2c863..1738887 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java @@ -20,7 +20,7 @@ public class TurretSim extends Turret { public TurretSim() { turretSim = new LinearSystemSim( - LinearSystemId.identifyPositionSystem(Settings.Turret.kV.get(), Settings.Turret.kA.get()) + LinearSystemId.identifyPositionSystem(Settings.Turret.Feedforward.kV.get(), Settings.Turret.Feedforward.kA.get()) ); turretPose2d = Odometry.getInstance().getField().getObject("Turret Pose 2d"); From d90b955e328e05eaf75a312dc117583bdf575224 Mon Sep 17 00:00:00 2001 From: Edgar Ganahl Date: Fri, 1 Dec 2023 17:12:37 -0500 Subject: [PATCH 3/6] better --- .../robot/subsystems/shooter/Shooter.java | 25 ++++++------------- .../robot/subsystems/shooter/ShooterImpl.java | 6 ----- .../robot/subsystems/shooter/ShooterSim.java | 17 +++++++------ 3 files changed, 17 insertions(+), 31 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index 554aed9..1eb302e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -3,6 +3,7 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.RobotBase; @@ -18,7 +19,7 @@ * @author Richie Xue */ public abstract class Shooter extends SubsystemBase{ - // Singleton (makes it so that there is only one instance of the Shooter class) + private static final Shooter instance; static { @@ -26,7 +27,6 @@ public abstract class Shooter extends SubsystemBase{ instance = new ShooterImpl(); } else { - // it literally does nothing lol instance = new ShooterSim(); } } @@ -34,22 +34,16 @@ public abstract class Shooter extends SubsystemBase{ public static Shooter getInstance() { return instance; } - // Don't worry about anything above - private Controller controller; - private double targetRPM; - private final SimpleMotorFeedforward feedforward; - private final Controller feedback; + private final Controller controller; + private double targetRPM; public Shooter() { - - controller = new PIDController(1, 0, 0); - this.targetRPM = 0.0; - this.feedforward = Settings.Shooter.ShooterFF.getController(); - this.feedback = Settings.Shooter.ShooterPID.getController(); + controller = new PIDController(Settings.Shooter.Feedback.kP, Settings.Shooter.Feedback.kI, Settings.Shooter.Feedback.kD) + .add(new MotorFeedforward(Settings.Shooter.Feedforward.kS, Settings.Shooter.Feedforward.kV, Settings.Shooter.Feedforward.kA).velocity()); } public void setTargetRPM(double targetRPM) { @@ -70,7 +64,6 @@ public double getTargetRPM() { public abstract double getVelocity(); public abstract void setVoltage(double voltage); - public abstract double getVoltage(); @Override public void periodic() { @@ -78,10 +71,8 @@ public void periodic() { this.setVoltage(0.0); } else { - // Calculate feedforward and feedback (inputting a desired RPM and outputting needed voltage) - double ff = feedforward.calculate(getTargetRPM()); - double fb = feedback.update(getTargetRPM(), getVelocity()); - this.setVoltage(ff + fb); + controller.update(targetRPM, getVelocity()); + this.setVoltage(controller.getOutput()); } SmartDashboard.putNumber("Shooter/Target RPM", getTargetRPM()); periodicallyCalled(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 476f83e..eb6b2a0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -33,15 +33,9 @@ public void setVoltage(double voltage) { motor.setVoltage(voltage); } - @Override - public double getVoltage() { - return motor.getAppliedOutput(); - } - @Override public void periodicallyCalled() { SmartDashboard.putNumber("Shooter/Velocity", getVelocity()); SmartDashboard.putNumber("Shooter/Error", getTargetRPM() - getVelocity()); - SmartDashboard.putNumber("Shooter/Voltage", getVoltage()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java index f0d6743..c5aaeaa 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java @@ -4,11 +4,12 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ShooterSim extends Shooter { - private double velocity; private LinearSystemSim shooterSim; public ShooterSim() { @@ -19,21 +20,21 @@ public ShooterSim() { @Override public double getVelocity() { - return velocity; + return Units.radiansPerSecondToRotationsPerMinute(shooterSim.getOutput(0)); } @Override - public void periodicallyCalled() { - velocity = getTargetRPM(); + public void setVoltage(double voltage) { + shooterSim.setInput(voltage); } @Override - public void setVoltage(double voltage) { - return; + public void simulationPeriodic() { + shooterSim.update(Settings.DT); } @Override - public double getVoltage() { - return 0.0; + public void periodicallyCalled() { + SmartDashboard.putNumber("Shooter/RPM", getVelocity()); } } \ No newline at end of file From 2a48ab9efd88b328d6635a2682e932253366a277 Mon Sep 17 00:00:00 2001 From: Edgar Ganahl Date: Wed, 6 Dec 2023 16:20:44 -0500 Subject: [PATCH 4/6] made it absolute lol --- .../com/stuypulse/robot/subsystems/shooter/Shooter.java | 2 +- .../com/stuypulse/robot/subsystems/turret/TurretImpl.java | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index 1eb302e..3829be4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -5,7 +5,7 @@ import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; +// import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java index e5ceb69..20afafe 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -2,16 +2,17 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxAbsoluteEncoder.Type; +import com.revrobotics.SparkMaxAbsoluteEncoder; public class TurretImpl extends Turret { private CANSparkMax motor; - private RelativeEncoder encoder; + private SparkMaxAbsoluteEncoder encoder; public TurretImpl(int port) { motor = new CANSparkMax(port, MotorType.kBrushless); - encoder = motor.getEncoder(); + encoder = motor.getAbsoluteEncoder(Type.kDutyCycle); } @Override From dc78093b9b436283d7873ec6524f7e2d5081dd6b Mon Sep 17 00:00:00 2001 From: Edgar Ganahl Date: Wed, 6 Dec 2023 17:14:01 -0500 Subject: [PATCH 5/6] lol --- .../com/stuypulse/robot/subsystems/turret/TurretSim.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java index 1738887..407a8cc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java @@ -25,7 +25,13 @@ public TurretSim() { turretPose2d = Odometry.getInstance().getField().getObject("Turret Pose 2d"); - turretPose2d.setPose(new Pose2d(4, 4, Rotation2d.fromDegrees(0))); + // turretPose2d.setPose(new Pose2d(4, 4, Rotation2d.fromDegrees(0))); + // turretPose2d.setPose(Odometry.getInstance().getPose()); + turretPose2d.setPose(new Pose2d( + Odometry.getInstance().getTranslation(), + Rotation2d.fromDegrees(getTurretAngle()) + )); + SmartDashboard.putData(Odometry.getInstance().getField()); } From 46f02d61df0f1503e74685d4c9f930bd8164ab7f Mon Sep 17 00:00:00 2001 From: Edgar Ganahl Date: Wed, 6 Dec 2023 17:21:25 -0500 Subject: [PATCH 6/6] lol2 --- src/main/java/com/stuypulse/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 7f798e5..b721cde 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -8,6 +8,7 @@ import com.stuypulse.robot.commands.TurretPoint; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.odometry.OdometryRealign; +import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Motors.Swerve; import com.stuypulse.robot.subsystems.shooter.Shooter; @@ -57,6 +58,7 @@ public RobotContainer() { private void configureDefaultCommands() { turret.setDefaultCommand(new TurretPoint (new Translation2d())); + swerve.setDefaultCommand(new SwerveDriveDrive(driver)); } /***************/