diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..d705b1b --- /dev/null +++ b/simgui.json @@ -0,0 +1,29 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Autonomous": "String Chooser", + "/SmartDashboard/Visualizers/Elevator": "Mechanism2d" + }, + "windows": { + "/SmartDashboard/Visualizers/Elevator": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Elevator": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index e0f788d..76307d0 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -5,6 +5,8 @@ package com.stuypulse.robot; +import com.stuypulse.robot.commands.elevator.ElevatorToBottom; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -15,12 +17,15 @@ public class Robot extends TimedRobot { private Command auto; /*************************/ - /*** ROBOT SCHEDULEING ***/ + /*** ROBOT SCHEDULING ***/ /*************************/ @Override public void robotInit() { robot = new RobotContainer(); + + // Check with Philip to automatically reset elevator encoder + // new ElevatorToBottom().schedule(); } @Override diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5b55349..49fe7f1 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -9,6 +9,7 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; +import com.stuypulse.robot.subsystems.elevator.Elevator; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -21,6 +22,7 @@ public class RobotContainer { public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem + public final Elevator elevator = Elevator.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java new file mode 100644 index 0000000..bc6629b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Constants.Elevator; + +public class ElevatorToBottom extends ElevatorToHeight{ + public ElevatorToBottom(){ + super(Elevator.MIN_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java new file mode 100644 index 0000000..a15139d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToHandoff extends ElevatorToHeight{ + public ElevatorToHandoff() { + super(Elevator.HANDOFF_HEIGHT_METERS); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java new file mode 100644 index 0000000..95d40c8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java @@ -0,0 +1,29 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.subsystems.elevator.Elevator; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.Command; + +public class ElevatorToHeight extends InstantCommand { + + public static Command untilDone(double height) { + return new ElevatorToHeight(height) + .andThen(new WaitUntilCommand(() -> Elevator.getInstance().atTargetHeight())); + } + + private final Elevator elevator; + private final double targetHeight; + + public ElevatorToHeight(double targetHeight){ + elevator = Elevator.getInstance(); + this.targetHeight = targetHeight; + + addRequirements(elevator); + } + + public void initialize(){ + elevator.setTargetHeight(targetHeight); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Alt.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Alt.java new file mode 100644 index 0000000..7510446 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Alt.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl2Alt extends ElevatorToHeight{ + public ElevatorToLvl2Alt(){ + super(Elevator.ALT_L2_HEIGHT_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java new file mode 100644 index 0000000..6abad77 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl2Funnel extends ElevatorToHeight{ + public ElevatorToLvl2Funnel(){ + super(Elevator.FUNNEL_L2_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java new file mode 100644 index 0000000..59801d2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl3Alt extends ElevatorToHeight{ + public ElevatorToLvl3Alt () { + super(Elevator.ALT_L3_HEIGHT_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java new file mode 100644 index 0000000..7b72017 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl3Funnel extends ElevatorToHeight{ + public ElevatorToLvl3Funnel() { + super(Elevator.FUNNEL_L3_HEIGHT_METERS); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java new file mode 100644 index 0000000..a8a7f38 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl4Alt extends ElevatorToHeight{ + public ElevatorToLvl4Alt(){ + super(Elevator.ALT_L4_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java new file mode 100644 index 0000000..8e206f0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl4Funnel extends ElevatorToHeight{ + public ElevatorToLvl4Funnel() { + super(Elevator.FUNNEL_L4_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java new file mode 100644 index 0000000..c893688 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Constants; + +public class ElevatorToTop extends ElevatorToHeight{ + public ElevatorToTop() { + super(Constants.Elevator.MAX_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Constants.java b/src/main/java/com/stuypulse/robot/constants/Constants.java new file mode 100644 index 0000000..bcee24f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/constants/Constants.java @@ -0,0 +1,24 @@ +package com.stuypulse.robot.constants; + +import edu.wpi.first.math.util.Units; + +public class Constants { + public interface Elevator { + double MIN_HEIGHT_METERS = Units.inchesToMeters(9.09375); // FROM THE BOTTOM OF FIXED STAGE TO TOP OF CARRIAGE + double MAX_HEIGHT_METERS = Units.inchesToMeters(77); // FROM THE BOTTOM OF FIXED STAGE TO TOP ELEVATOR + + double MASS_KG = 10.0; + double DRUM_RADIUS_METERS = (MAX_HEIGHT_METERS / Encoders.NUM_ROTATIONS_TO_REACH_TOP * Encoders.GEARING) / 2 / Math.PI; + + public interface Encoders { + double GEARING = 4.0; + + double NUM_ROTATIONS_TO_REACH_TOP = (6 + 9.0 / 24) * GEARING; // Number of rotations that the motor has to spin, NOT the gear + + double POSITION_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP; + double VELOCITY_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP / 60; + + double DISTANCE_PER_ROTATION = 0.0; + } + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 1a2106d..9ef9a4e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -12,4 +12,10 @@ public interface Gamepad { int OPERATOR = 1; int DEBUGGER = 2; } + + public interface Elevator { + int MOTOR = 0; + int BOTTOM_SWITCH = 1; + int TOP_SWITCH = 2; + } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 570bbaa..1cc2a4a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,7 +5,6 @@ package com.stuypulse.robot.constants; -import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; /*- @@ -14,4 +13,54 @@ * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable * values that we can edit on Shuffleboard. */ -public interface Settings {} +public interface Settings { + + double DT = 0.020; // 20ms Differential Time + + public interface Elevator { + SmartNumber MAX_VELOCITY_METERS_PER_SECOND = new SmartNumber("Elevator/Max Velocity (m per s)", 1.0); + SmartNumber MAX_ACCEL_METERS_PER_SECOND_PER_SECOND = new SmartNumber("Elevator/Max Accel (m per s^2)", 2.0); + + // CHANGE + double HANDOFF_HEIGHT_METERS = 0.1; + // front and funnel + double ALT_L2_HEIGHT_METERS = 0.25; + double ALT_L3_HEIGHT_METERS = 0.5; + double ALT_L4_HEIGHT_METERS = 0.75; + double FUNNEL_L2_HEIGHT_METERS = 0.3; // funnel side; should be higher than L2 + double FUNNEL_L3_HEIGHT_METERS = 0.55; // funnel side; should be higher than L3 + double FUNNEL_L4_HEIGHT_METERS = 0.8; + + double RAMP_RATE_CURRENT = 1.0; + double RAMP_RATE_VOLTAGE = 0.1; + + // FIND OUT REAL GEAR RATIO + double GEAR_RATIO = 1.0/5.0; + double CURRENT_LIMIT = 5.0; + + // Magic motion, change RPS + double TARGET_CRUISE_VELOCITY = 0.0; //Rotations Per Second + double TARGET_ACCELERATION = 0.0; //Rotations Per Second^2 + double TARGET_JERK = 0.0; //Rotations Per Second^3 + + SmartNumber HEIGHT_TOLERANCE_METERS = new SmartNumber("Elevator/Height Tolerance (m)", 0.02); + + public interface PID { + //tune + SmartNumber kP = new SmartNumber("Elevator/Controller/kP",0.0); + SmartNumber kI = new SmartNumber("Elevator/Controller/kI",0.0); + SmartNumber kD = new SmartNumber("Elevator/Controller/kD",0.0); + } + + public interface FF { + SmartNumber kS = new SmartNumber("Elevator/Controller/kS",0.20506); + SmartNumber kV = new SmartNumber("Elevator/Controller/kV",3.7672); + SmartNumber kA = new SmartNumber("Elevator/Controller/kA", 0.27); + SmartNumber kG = new SmartNumber("Elevator/Controller/kG", 1.37); + } + + public interface Simulation { + double SCALE_FACTOR = 0.5 + 2.5/77; + } + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..d702aff --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,40 @@ +package com.stuypulse.robot.subsystems.elevator; + +import com.stuypulse.robot.Robot; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class Elevator extends SubsystemBase { + + private static final Elevator instance; + + static { + if (Robot.isReal()) { + instance = new ElevatorImpl(); + } else { + instance = new ElevatorSimu(); + } + } + + private final ElevatorVisualizer visualizer; + + public static Elevator getInstance() { + return instance; + } + + public Elevator() { + visualizer = ElevatorVisualizer.getInstance(); + } + + public abstract void setTargetHeight(double height); + public abstract double getTargetHeight(); + public abstract double getCurrentHeight(); + public abstract boolean atTargetHeight(); + public abstract boolean atBottom(); + + public void periodic() { + visualizer.update(); + } + +} + \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java new file mode 100644 index 0000000..703c5ba --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -0,0 +1,117 @@ +package com.stuypulse.robot.subsystems.elevator; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.stuypulse.robot.constants.Constants; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.network.SmartNumber; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ElevatorImpl extends Elevator { + private final TalonFX motor; + private final SmartNumber targetHeight; + private final DigitalInput bumpSwitchBottom; + + public ElevatorImpl() { + motor = new TalonFX(Ports.Elevator.MOTOR); + targetHeight = new SmartNumber("Elevator/Target Height", Constants.Elevator.MIN_HEIGHT_METERS); + bumpSwitchBottom = new DigitalInput(Ports.Elevator.BOTTOM_SWITCH); + + TalonFXConfiguration elevatorMotorConfig = new TalonFXConfiguration(); + + Slot0Configs slot0 = new Slot0Configs(); + + slot0.GravityType = GravityTypeValue.Elevator_Static; + + slot0.kS = Settings.Elevator.FF.kS.getAsDouble(); + slot0.kV = Settings.Elevator.FF.kV.getAsDouble(); + slot0.kA = Settings.Elevator.FF.kA.getAsDouble(); + slot0.kG = Settings.Elevator.FF.kG.getAsDouble(); + + slot0.kP = Settings.Elevator.PID.kP.getAsDouble(); + slot0.kI = Settings.Elevator.PID.kI.getAsDouble(); + slot0.kD = Settings.Elevator.PID.kD.getAsDouble(); + + elevatorMotorConfig.Slot0 = slot0; + + // Base motor configs + elevatorMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + elevatorMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + elevatorMotorConfig.Feedback.SensorToMechanismRatio = Settings.Elevator.GEAR_RATIO; + + // Current limiting + elevatorMotorConfig.CurrentLimits.StatorCurrentLimit = Settings.Elevator.CURRENT_LIMIT; + elevatorMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + + // Ramp motor voltage + elevatorMotorConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Elevator.RAMP_RATE_VOLTAGE; + elevatorMotorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Elevator.RAMP_RATE_VOLTAGE; + + elevatorMotorConfig.OpenLoopRamps.TorqueOpenLoopRampPeriod = Settings.Elevator.RAMP_RATE_CURRENT; + elevatorMotorConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = Settings.Elevator.RAMP_RATE_CURRENT; + + var elevatorMotionMagicConfigs = elevatorMotorConfig.MotionMagic; + elevatorMotionMagicConfigs.MotionMagicCruiseVelocity = Settings.Elevator.TARGET_CRUISE_VELOCITY; + elevatorMotionMagicConfigs.MotionMagicAcceleration = Settings.Elevator.TARGET_ACCELERATION; + elevatorMotionMagicConfigs.MotionMagicJerk = Settings.Elevator.TARGET_JERK; // Set to 0, not being used + + + motor.getConfigurator().apply(elevatorMotorConfig); + } + + @Override + public void setTargetHeight(double height) { + targetHeight.set( + SLMath.clamp( + height, + Constants.Elevator.MIN_HEIGHT_METERS, + Constants.Elevator.MAX_HEIGHT_METERS + ) + ); + } + + @Override + public double getTargetHeight() { + return targetHeight.getAsDouble(); + } + + @Override + public double getCurrentHeight() { + return motor.getPosition().getValueAsDouble() * Constants.Elevator.Encoders.DISTANCE_PER_ROTATION; + } + + @Override + public boolean atTargetHeight() { + return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); + } + + @Override + public boolean atBottom() { + return !bumpSwitchBottom.get(); + } + + @Override + public void periodic() { + super.periodic(); + + final MotionMagicVoltage controlRequest = new MotionMagicVoltage(getTargetHeight()/Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR); + motor.setControl(controlRequest); + + if (atBottom()) { + motor.setPosition(0); + } + + SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); + SmartDashboard.putNumber("Elevator/Motor Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Elevator/Motor Current", motor.getStatorCurrent().getValueAsDouble()); + + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java new file mode 100644 index 0000000..58eb6a4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java @@ -0,0 +1,96 @@ +package com.stuypulse.robot.subsystems.elevator; + +import com.stuypulse.robot.constants.Constants; +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.ElevatorFeedforward; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.network.SmartNumber; +import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; + +import edu.wpi.first.math.system.plant.DCMotor; +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.SmartDashboard; + +public class ElevatorSimu extends Elevator { + + private final ElevatorSim sim; + private final double minHeight, maxHeight; + + private final SmartNumber targetHeight; + + private final Controller controller; + + ElevatorSimu() { + + sim = new ElevatorSim( + DCMotor.getNEO(2), + Constants.Elevator.Encoders.GEARING, + Constants.Elevator.MASS_KG, + Constants.Elevator.DRUM_RADIUS_METERS, + Constants.Elevator.MIN_HEIGHT_METERS, + Constants.Elevator.MAX_HEIGHT_METERS, + true, + Constants.Elevator.MIN_HEIGHT_METERS + ); + + minHeight = Constants.Elevator.MIN_HEIGHT_METERS; + maxHeight = Constants.Elevator.MAX_HEIGHT_METERS; + + targetHeight = new SmartNumber("Elevator/Target Height (m)", Constants.Elevator.MIN_HEIGHT_METERS); + + MotionProfile motionProfile = new MotionProfile(Settings.Elevator.MAX_VELOCITY_METERS_PER_SECOND, Settings.Elevator.MAX_ACCEL_METERS_PER_SECOND_PER_SECOND); + + controller = new MotorFeedforward(Settings.Elevator.FF.kS, Settings.Elevator.FF.kV, Settings.Elevator.FF.kA).position() + .add(new ElevatorFeedforward(Settings.Elevator.FF.kG)) + .add(new PIDController(Settings.Elevator.PID.kP, Settings.Elevator.PID.kI, Settings.Elevator.PID.kD)) + .setSetpointFilter(motionProfile); + } + + public ElevatorSim getSim() { + return sim; + } + + @Override + public void setTargetHeight(double height) { + targetHeight.set(SLMath.clamp(height, minHeight, maxHeight)); + } + + @Override + public double getTargetHeight() { + return targetHeight.get(); + } + + @Override + public double getCurrentHeight() { + return sim.getPositionMeters(); + } + + @Override + public boolean atTargetHeight() { + return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); + } + + @Override + public boolean atBottom() { + return false; + } + + @Override + public void periodic() { + super.periodic(); + + controller.update(getTargetHeight(), getCurrentHeight()); + sim.setInputVoltage(controller.getOutput()); + sim.update(Settings.DT); + RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); + + ElevatorVisualizer.getInstance().update(); // delete this line later + + SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java new file mode 100644 index 0000000..692f750 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java @@ -0,0 +1,136 @@ +package com.stuypulse.robot.subsystems.elevator; + +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +import com.stuypulse.robot.constants.Constants; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.math.util.Units; +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; + +public class ElevatorVisualizer { + + private static final ElevatorVisualizer instance; + + static { + instance = new ElevatorVisualizer(); + } + + private final Mechanism2d elevator2d; + + private final MechanismRoot2d elevatorBL; + private final MechanismRoot2d elevatorTR; + + private final MechanismRoot2d stageTwoBL; + private final MechanismRoot2d stageTwoTR; + + public static ElevatorVisualizer getInstance(){ + return instance; + } + + public ElevatorVisualizer() { + + // Mechanism2d + elevator2d = new Mechanism2d(Units.inchesToMeters(14), Units.inchesToMeters(150)); + + // Stage One + // Bottom Left Node + elevatorBL = elevator2d.getRoot("Elevator BL", Units.inchesToMeters(2), Constants.Elevator.MIN_HEIGHT_METERS); + + elevatorBL.append(new MechanismLigament2d( + "Left Tower", + Units.inchesToMeters(39), + 90, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + elevatorBL.append(new MechanismLigament2d( + "Bottom Tower", + Units.inchesToMeters(10), //Change + 0, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + // Top Right Node + elevatorTR = elevator2d.getRoot("Elevator TR", Units.inchesToMeters(12), Units.inchesToMeters(39) + Constants.Elevator.MIN_HEIGHT_METERS); + + elevatorTR.append(new MechanismLigament2d( + "Right Tower", + Units.inchesToMeters(39), + -90, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + elevatorTR.append(new MechanismLigament2d( + "Top Side", + Units.inchesToMeters(10), //Change + 180, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + // Stage Two + // Bottom Left Node + stageTwoBL = elevator2d.getRoot("Outer BL", Units.inchesToMeters(3), Constants.Elevator.MIN_HEIGHT_METERS); + + stageTwoBL.append(new MechanismLigament2d( + "Left Side", + Units.inchesToMeters(35), + 90, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + stageTwoBL.append(new MechanismLigament2d( + "Bottom Side", + Units.inchesToMeters(6), + 0, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + // Top Right Node + stageTwoTR = elevator2d.getRoot("Outer TR", Units.inchesToMeters(12), Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); + + stageTwoTR.append(new MechanismLigament2d( + "Right Side", + Units.inchesToMeters(35), + -90, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + stageTwoTR.append(new MechanismLigament2d( + "Top Side", + Units.inchesToMeters(6), + 180, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + SmartDashboard.putData("Visualizers/Elevator", elevator2d); + } + + public void update() { + // Top of Stage Two is target height + Elevator elevator = Elevator.getInstance(); + stageTwoBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(4)); + stageTwoTR.setPosition(Units.inchesToMeters(10), elevator.getCurrentHeight() + Units.inchesToMeters(39)); + } + +} \ No newline at end of file