From 36fe001957384ab09c3c8722dcebc10e1e9e1f16 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Fri, 26 Jan 2024 09:42:00 -0500 Subject: [PATCH] Add amp/trap + lift subsystem (#3) * started subsystem * some fixes to Amper and AmperImpl * Implemented Amper methods in AmperImpl.java, added ElevatorSim in AmperSim.java, added logging for lift and intake, added configure for lift and score motor Co-authored-by: Zixi52 Co-authored-by: Baiulus Co-authored-by: Naowal Rahman Co-authored-by: Rahel Arka * clean up amper code * Added singleton in RobotContainer.java * Co-authored-by: Zixi52 Co-authored-by: Rahuldeb75 Co-authored-by: Rahel Arka Co-authored-by: Baiulus Co-authored-by: Naowal Rahman Co-authored-by: Ian Shi Co-authored-by: Zixi52 Co-authored-by: Baiulus Co-authored-by: Rahel Arka Co-authored-by: BenG49 Co-authored-by: BenG49 <64862590+BenG49@users.noreply.github.com> --- .../com/stuypulse/robot/RobotContainer.java | 3 +- .../com/stuypulse/robot/constants/Motors.java | 11 ++- .../com/stuypulse/robot/constants/Ports.java | 9 ++ .../stuypulse/robot/constants/Settings.java | 36 +++++++ .../robot/subsystems/amper/Amper.java | 66 +++++++++++++ .../robot/subsystems/amper/AmperImpl.java | 99 +++++++++++++++++++ .../robot/subsystems/amper/AmperSim.java | 75 ++++++++++++++ 7 files changed, 296 insertions(+), 3 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bc893dd1..40e8aa2e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.subsystems.amper.Amper; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.vision.AprilTagVision; @@ -28,7 +29,7 @@ public class RobotContainer { public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem - + public final Amper amper = Amper.getInstance(); public final SwerveDrive swerve = SwerveDrive.getInstance(); public final Odometry odometry = Odometry.getInstance(); public final AprilTagVision vision = AprilTagVision.getInstance(); diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index de2be5c2..5957a3d5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -8,9 +8,8 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; - +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkMax; -import static com.revrobotics.CANSparkMax.IdleMode; /*- * File containing all of the configurations that different motors require. @@ -22,7 +21,14 @@ * - The Open Loop Ramp Rate */ public interface Motors { + /** Classes to store all of the values a motor needs */ + + public interface Amper { + CANSparkMaxConfig LIFT_MOTOR = new CANSparkMaxConfig(false, IdleMode.kCoast, 80, 0.1); + CANSparkMaxConfig SCORE_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake, 80, 0.1); + } + public interface Swerve { public CANSparkMaxConfig DRIVE_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake); public CANSparkMaxConfig TURN_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake); @@ -137,4 +143,5 @@ public void configure(CANSparkMax motor) { } } + } diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 310d955f..36ec505c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -13,6 +13,15 @@ public interface Gamepad { int DEBUGGER = 2; } + public interface Amper { + int SCORE = 31; + int LIFT = 30; + + int ALIGNED_SWITCH_CHANNEL = 3 ; + int MIN_LIFT_CHANNEL = 4 ; + int AMP_IR_CHANNEL = 2; + } + 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 2dd793d1..f99047c6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -20,7 +20,43 @@ * values that we can edit on Shuffleboard. */ public interface Settings { + double DT = 0.02; + + public interface Amper { + + public interface Score { + SmartNumber ROLLER_SPEED = new SmartNumber("Amper/Score/Roller Speed", 1.0); // change later + } + + public interface Lift { + + double CARRIAGE_MASS = 10; // kg + + double MIN_HEIGHT = 0; + double MAX_HEIGHT = 1.8475325; // meters + + double MAX_HEIGHT_ERROR = 0.03; + + SmartNumber VEL_LIMIT = new SmartNumber("Velocity Limit", 3); + SmartNumber ACC_LIMIT = new SmartNumber("Acceleration Limit", 2); + + public interface Encoder { + double GEARING = 9; // ~9:1 + double DRUM_RADIUS = 0.025; // meters + double DRUM_CIRCUMFERENCE = DRUM_RADIUS * Math.PI * 2; + + double POSITION_CONVERSION = GEARING * DRUM_CIRCUMFERENCE; + double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0; + } + + public interface PID { + SmartNumber kP = new SmartNumber("Amper/Lift/kP", 1); + SmartNumber kI = new SmartNumber("Amper/Lift/kI", 0); + SmartNumber kD = new SmartNumber("Amper/Lift/kD", 0); + } + } + } public interface Swerve { // between wheel centers diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java b/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java new file mode 100644 index 00000000..17717190 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java @@ -0,0 +1,66 @@ +package com.stuypulse.robot.subsystems.amper; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Amper.Lift; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/* +AMP: +1 motor +1 limit switch +IR Sensor + +LIFT: +1 motor +1 encoder +Bottom (shooter) limit switch +*/ + +public abstract class Amper extends SubsystemBase { + + private static Amper instance; + + static { + instance = new AmperImpl(); + } + + public static Amper getInstance() { + return instance; + } + + public final SmartNumber targetHeight; + public final Controller liftController; + + public Amper() { + liftController = new PIDController(Lift.PID.kP, Lift.PID.kI, Lift.PID.kD); + targetHeight = new SmartNumber("Amp/Target Height", 0); // TODO: determine the default value + } + + public void setTargetHeight(double height) { + targetHeight.set(SLMath.clamp(height, Settings.Amper.Lift.MIN_HEIGHT, Settings.Amper.Lift.MAX_HEIGHT)); + } + + public abstract boolean hasNote(); + + public abstract void score(); + public abstract void intake(); + public abstract void stopRoller(); + + public abstract boolean liftAtBottom(); + public abstract double getLiftHeight(); + public abstract void stopLift(); + public abstract void setLiftVoltageImpl(double voltage); + + public abstract boolean touchingAmp(); + + @Override + public void periodic() { + setLiftVoltageImpl(liftController.update(targetHeight.get(), getLiftHeight())); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java new file mode 100644 index 00000000..86ca9f78 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java @@ -0,0 +1,99 @@ +package com.stuypulse.robot.subsystems.amper; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +public class AmperImpl extends Amper { + + private final CANSparkMax scoreMotor; + private final CANSparkMax liftMotor; + private final RelativeEncoder liftEncoder; + + private final DigitalInput alignedSwitch; + private final DigitalInput minSwitch; + private final DigitalInput ampIRSensor; + + public AmperImpl() { + scoreMotor = new CANSparkMax(Ports.Amper.SCORE, MotorType.kBrushless); + liftMotor = new CANSparkMax(Ports.Amper.LIFT, MotorType.kBrushless); + liftEncoder = liftMotor.getEncoder(); + + liftEncoder.setPositionConversionFactor(Settings.Amper.Lift.Encoder.POSITION_CONVERSION); + liftEncoder.setVelocityConversionFactor(Settings.Amper.Lift.Encoder.VELOCITY_CONVERSION); + + alignedSwitch = new DigitalInput(Ports.Amper.ALIGNED_SWITCH_CHANNEL); + minSwitch = new DigitalInput(Ports.Amper.MIN_LIFT_CHANNEL); + ampIRSensor = new DigitalInput(Ports.Amper.AMP_IR_CHANNEL); + + Motors.Amper.LIFT_MOTOR.configure(liftMotor); + Motors.Amper.SCORE_MOTOR.configure(scoreMotor); + } + + @Override + public boolean hasNote() { + return !ampIRSensor.get(); + } + + @Override + public boolean liftAtBottom() { + return !minSwitch.get(); + } + + @Override + public double getLiftHeight() { + return liftEncoder.getPosition(); + } + + @Override + public boolean touchingAmp() { + return !alignedSwitch.get(); + } + + @Override + public void score() { + scoreMotor.set(Settings.Amper.Score.ROLLER_SPEED.get()); + } + + @Override + public void intake() { + scoreMotor.set(-Settings.Amper.Score.ROLLER_SPEED.get()); + } + + @Override + public void stopLift() { + liftMotor.stopMotor(); + } + + @Override + public void stopRoller() { + scoreMotor.stopMotor(); + } + + @Override + public void setLiftVoltageImpl(double voltage) { + if (liftAtBottom() && voltage < 0) { + stopLift(); + } + else { + liftMotor.setVoltage(voltage); + } + } + + public void periodic() { + super.periodic(); + + SmartDashboard.putNumber("Amper/Intake Speed", scoreMotor.get()); + SmartDashboard.putNumber("Amper/Lift Speed", liftMotor.get()); + SmartDashboard.putNumber("Amper/Intake Current", scoreMotor.getOutputCurrent()); + SmartDashboard.putNumber("Amper/Lift Current", liftMotor.getOutputCurrent()); + SmartDashboard.putNumber("Amper/Lift Height", getLiftHeight()); + } + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java new file mode 100644 index 00000000..10fcb23a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java @@ -0,0 +1,75 @@ +package com.stuypulse.robot.subsystems.amper; + +import static com.stuypulse.robot.constants.Settings.Amper.Lift.*; + +import com.stuypulse.robot.constants.Settings; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class AmperSim extends Amper { + + private final ElevatorSim sim; + + public AmperSim() { + sim = new ElevatorSim( + DCMotor.getNEO(1), + Encoder.GEARING, + CARRIAGE_MASS, + Encoder.DRUM_RADIUS, + MIN_HEIGHT, + MAX_HEIGHT, + true, + 0 + ); + } + + @Override + public boolean hasNote() { + return false; + } + + @Override + public void intake() {} + + @Override + public void score() {} + + @Override + public boolean liftAtBottom() { + return sim.hasHitLowerLimit(); + } + + @Override + public boolean touchingAmp() { + return false; + } + + @Override + public void stopLift() { + sim.setInputVoltage(0.0); + } + + @Override + public double getLiftHeight() { + return sim.getPositionMeters(); + } + + @Override + public void setLiftVoltageImpl(double voltage) { + sim.setInputVoltage(voltage); + } + + @Override + public void stopRoller() {} + + @Override + public void simulationPeriodic() { + sim.update(Settings.DT); + + SmartDashboard.putNumber("Amper/Lift Height", getLiftHeight()); + } + +} + + \ No newline at end of file