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 <Zixi52@users.noreply.github.com>
Co-authored-by: Baiulus <Baiulus@users.noreply.github.com>
Co-authored-by: Naowal Rahman <naowal.ar@gmail.com>
Co-authored-by: Rahel Arka <RahelArka@users.noreply.github.com>

* clean up amper code

* Added singleton in RobotContainer.java

* Co-authored-by: Zixi52 <Zixi52@users.noreply.github.com>
Co-authored-by: Rahuldeb75 <Rahuldeb75@users.noreply.github.com>
Co-authored-by: Rahel Arka <RahelArka@users.noreply.github.com>
Co-authored-by: Baiulus <Baiulus@users.noreply.github.com>
Co-authored-by: Naowal Rahman <naowal.ar@gmail.com>
Co-authored-by: Ian Shi <IanShiii@users.noreply.github.com

* Made some fixes to Amper, AmperImpl

* Organize settings + velocity conversion factor

* Fix AmperSim

---------

Co-authored-by: Kalimul Kaif <kalimul.kaif@stuypulse.com>
Co-authored-by: Zixi52 <Zixi52@users.noreply.github.com>
Co-authored-by: Baiulus <Baiulus@users.noreply.github.com>
Co-authored-by: Rahel Arka <RahelArka@users.noreply.github.com>
Co-authored-by: BenG49 <ben@goldfisher.com>
Co-authored-by: BenG49 <64862590+BenG49@users.noreply.github.com>

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();

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) {
          }
 
      }
+
 }

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;

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

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()));
+    }
+
+}

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());
+    }
+
+}

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());
+    }
+
+}
+
+