Add Amper commands + lift sim (#12)

* Amper commmands

Co-authored-by: Rahuldeb75 <Rahuldeb75@users.noreply.github.com>
Co-authored-by: k4limul <k4limul@users.noreply.github.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>

* preliminary mechanism2d visualization

Co-authored-by: Rahuldeb75 <Rahuldeb75@users.noreply.github.com>
Co-authored-by: k4limul <k4limul@users.noreply.github.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>

* fixed ElevatorSim, put MotionProfile with LiftController, cleaned Amper Commands

Co-authored-by: Zixi52 <Zixi52@users.noreply.github.com>
Co-authored-by: k4limul <k4limul@users.noreply.github.com>
Co-authored-by: Baiulus <Baiulus@users.noreply.github.com>

* added AmperWaitToHeight + cleaned command code

* Add AmperScoreForever

* Fix command requirements

* Reorganize Amper and its Settings

* Add gravity to elevator

---------

Co-authored-by: Rahuldeb75 <Rahuldeb75@users.noreply.github.com>
Co-authored-by: k4limul <k4limul@users.noreply.github.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>

src/main/java/com/stuypulse/robot/commands/amper/AmperIntake.java

@@ -0,0 +1,25 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AmperIntake extends Command {
+
+    private final Amper amper;
+
+    public AmperIntake() {
+        amper = Amper.getInstance();
+        addRequirements(amper);
+    } 
+
+    @Override
+    public void initialize() {
+        amper.intake();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        amper.stopRoller();
+    }
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperOuttake.java

@@ -0,0 +1,30 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AmperOuttake extends Command {
+
+    private final Amper amper;
+
+    public AmperOuttake() {
+        amper = Amper.getInstance();
+        addRequirements(amper);
+    } 
+
+    @Override
+    public void initialize() {
+        amper.score();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return !amper.hasNote();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        amper.stopRoller();
+    }
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreAmp.java

@@ -0,0 +1,17 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.constants.Settings.Amper.Score;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class AmperScoreAmp extends SequentialCommandGroup {
+
+    public AmperScoreAmp() {
+        addCommands(
+            new AmperToHeight(Score.AMP_SCORE_HEIGHT.get()),
+            new AmperWaitToHeight(Score.AMP_SCORE_HEIGHT.get()),
+            new AmperOuttake()
+        );
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreForever.java

@@ -0,0 +1,20 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class AmperScoreForever extends InstantCommand {
+
+    private final Amper amper;
+
+    public AmperScoreForever() {
+        amper = Amper.getInstance();
+        addRequirements(amper);
+    } 
+
+    @Override
+    public void initialize() {
+        amper.score();
+    }
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreTrap.java

@@ -0,0 +1,17 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.constants.Settings.Amper.Score;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class AmperScoreTrap extends SequentialCommandGroup {
+
+    public AmperScoreTrap() {
+        addCommands(
+            new AmperToHeight(Score.TRAP_SCORE_HEIGHT.get()),
+            new AmperWaitToHeight(Score.TRAP_SCORE_HEIGHT.get()),
+            new AmperOuttake()
+        ); 
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java

@@ -0,0 +1,22 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class AmperToHeight extends InstantCommand {
+    private final Amper amper;
+    private final double height;
+
+    public AmperToHeight(double height) {
+        amper = Amper.getInstance();
+        this.height = height;
+
+        addRequirements(amper);
+    }
+
+    @Override
+    public void initialize() {
+        amper.setTargetHeight(height);
+    }
+}

src/main/java/com/stuypulse/robot/commands/amper/AmperWaitToHeight.java

@@ -0,0 +1,24 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.constants.Settings.Amper.Lift;
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AmperWaitToHeight extends Command {
+
+    private final Amper amper;
+    private final double height;
+
+    public AmperWaitToHeight(double height) {
+        amper = Amper.getInstance();
+        this.height = height;
+
+        addRequirements(amper);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(amper.getLiftHeight() - height) < Lift.MAX_HEIGHT_ERROR;
+    }
+}

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -6,6 +6,7 @@
 package com.stuypulse.robot.constants;
 
 import edu.wpi.first.math.util.Units;
+
 import com.stuypulse.stuylib.math.Vector2D;
 import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
@@ -42,14 +43,21 @@ public interface Encoder {
 
     public interface Amper {
         public interface Score {
-            SmartNumber ROLLER_SPEED = new SmartNumber("Amper/Score/Roller Speed", 1.0); // change later
+            SmartNumber SCORE_SPEED = new SmartNumber("Amper/Score/Score Speed", 1.0);
+            SmartNumber INTAKE_SPEED = new SmartNumber("Amper/Score/Intake Speed", 1.0);
+
+            SmartNumber AMP_SCORE_HEIGHT = new SmartNumber("Amper/Score/Amp Score Height", 1.0); // TODO: determine
+            SmartNumber TRAP_SCORE_HEIGHT = new SmartNumber("Amper/Score/Trap Score Height", 1.0); // TODO: determine
         }
 
         public interface Lift {
             double CARRIAGE_MASS = 10; // kg
 
             double MIN_HEIGHT = 0;
-            double MAX_HEIGHT = 1.8475325; // meters 
+            double MAX_HEIGHT = 1.8475325; // meters
+
+            double VISUALIZATION_MIN_LENGTH = 0.5;
+            Rotation2d ANGLE_TO_GROUND = Rotation2d.fromDegrees(68.02);
 
             double MAX_HEIGHT_ERROR = 0.03;
 
@@ -65,10 +73,17 @@ public interface Encoder {
                 double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0;
             }
 
+            public interface Feedforward {
+                SmartNumber kS = new SmartNumber("Amper/Lift/kS", 0.0);
+                SmartNumber kV = new SmartNumber("Amper/Lift/kV", 0.0);
+                SmartNumber kA = new SmartNumber("Amper/Lift/kA", 0.0);
+                SmartNumber kG = new SmartNumber("Amper/Lift/kG", 0.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);
+                SmartNumber kP = new SmartNumber("Amper/Lift/kP", 3.0);
+                SmartNumber kI = new SmartNumber("Amper/Lift/kI", 0.0);
+                SmartNumber kD = new SmartNumber("Amper/Lift/kD", 0.0);
             }
         }
     }

src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java

@@ -1,9 +1,21 @@
 package com.stuypulse.robot.subsystems.amper;
 
+import com.stuypulse.robot.Robot;
 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.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.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 /*
@@ -23,20 +35,52 @@ public abstract class Amper extends SubsystemBase {
     private static Amper instance;
 
     static {
-        instance = new AmperImpl();
+        if (Robot.isReal()) {
+            instance = new AmperImpl();
+        }
+        else {
+            instance = new AmperSim();
+        }
     }
 
     public static Amper getInstance() {
         return instance;
     }
 
-    protected final SmartNumber targetHeight;
+    protected final Controller liftController;
+
+    private final SmartNumber targetHeight;
+
+    private final Mechanism2d mechanism2d;
+    private final MechanismLigament2d lift2d;
 
     public Amper() {
-        targetHeight = new SmartNumber("Amp/Target Height", 0); // TODO: determine the default value
+        liftController = new MotorFeedforward(Lift.Feedforward.kS, Lift.Feedforward.kV, Lift.Feedforward.kA).position()
+            .add(new ElevatorFeedforward(Lift.Feedforward.kG))
+            .setSetpointFilter(new MotionProfile(Lift.VEL_LIMIT, Lift.ACC_LIMIT))
+                .add(new PIDController(Lift.PID.kP, Lift.PID.kI, Lift.PID.kD));
+
+        targetHeight = new SmartNumber("Amper/Target Height", 0); // TODO: determine the default value
+
+        mechanism2d = new Mechanism2d(3, 3);
+        mechanism2d.getRoot("Base Origin", 1, 1).append(new MechanismLigament2d(
+            "Base",
+            1,
+            0,
+            10,
+            new Color8Bit(Color.kOrange)));
+
+        lift2d = mechanism2d.getRoot("Lift Origin", 1.5, 1).append(new MechanismLigament2d(
+            "Lift",
+            Settings.Amper.Lift.VISUALIZATION_MIN_LENGTH,
+            Settings.Amper.Lift.ANGLE_TO_GROUND.getDegrees(),
+            10,
+            new Color8Bit(Color.kAqua)));
+
+        SmartDashboard.putData("Lift Mechanism", mechanism2d);
     }
 
-    public void setTargetHeight(double height) {
+    public final void setTargetHeight(double height) {
         targetHeight.set(SLMath.clamp(height, Settings.Amper.Lift.MIN_HEIGHT, Settings.Amper.Lift.MAX_HEIGHT));
     }
 
@@ -52,4 +96,10 @@ public void setTargetHeight(double height) {
 
     public abstract boolean touchingAmp();
 
+    @Override
+    public void periodic() {
+        liftController.update(targetHeight.get(), getLiftHeight());
+
+        lift2d.setLength(Settings.Amper.Lift.VISUALIZATION_MIN_LENGTH + getLiftHeight());
+    }
 }

src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java

@@ -5,9 +5,6 @@
 import com.stuypulse.robot.constants.Motors;
 import com.stuypulse.robot.constants.Ports;
 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 edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -23,11 +20,7 @@ public class AmperImpl extends Amper {
     private final DigitalInput minSwitch;
     private final DigitalInput ampIRSensor;
 
-    public final Controller liftController;
-
     public AmperImpl() {
-        liftController = new PIDController(Lift.PID.kP, Lift.PID.kI, Lift.PID.kD);
-
         scoreMotor = new CANSparkMax(Ports.Amper.SCORE, MotorType.kBrushless);
         liftMotor = new CANSparkMax(Ports.Amper.LIFT, MotorType.kBrushless);
         liftEncoder = liftMotor.getEncoder();
@@ -43,6 +36,7 @@ public AmperImpl() {
         Motors.Amper.SCORE_MOTOR.configure(scoreMotor);
     }
 
+
     @Override
     public boolean hasNote() {
         return !ampIRSensor.get();
@@ -65,12 +59,12 @@ public boolean touchingAmp() {
 
     @Override
     public void score() {
-        scoreMotor.set(Settings.Amper.Score.ROLLER_SPEED.get());
+        scoreMotor.set(Settings.Amper.Score.SCORE_SPEED.get());
     }
 
     @Override
     public void intake() {
-        scoreMotor.set(-Settings.Amper.Score.ROLLER_SPEED.get());
+        scoreMotor.set(-Settings.Amper.Score.INTAKE_SPEED.get());
     }
 
     @Override
@@ -86,9 +80,7 @@ public void stopRoller() {
     @Override
     public void periodic() {
         super.periodic();
-
-        liftController.update(targetHeight.get(), getLiftHeight());
-
+
         if (liftAtBottom() && liftController.getOutput() < 0) {
             stopLift();
         } else {