From 3eb81d35d1b89453464048a4a49650f2355f8321 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Sat, 27 Jan 2024 21:39:47 -0500 Subject: [PATCH] Add Amper commands + lift sim (#12) * Amper commmands Co-authored-by: Rahuldeb75 Co-authored-by: k4limul Co-authored-by: Zixi52 Co-authored-by: Baiulus Co-authored-by: Rahel Arka * preliminary mechanism2d visualization Co-authored-by: Rahuldeb75 Co-authored-by: k4limul Co-authored-by: Zixi52 Co-authored-by: Baiulus Co-authored-by: Rahel Arka * fixed ElevatorSim, put MotionProfile with LiftController, cleaned Amper Commands Co-authored-by: Zixi52 Co-authored-by: k4limul Co-authored-by: Baiulus * added AmperWaitToHeight + cleaned command code * Add AmperScoreForever * Fix command requirements * Reorganize Amper and its Settings * Add gravity to elevator --------- Co-authored-by: Rahuldeb75 Co-authored-by: k4limul Co-authored-by: Zixi52 Co-authored-by: Baiulus Co-authored-by: Rahel Arka Co-authored-by: BenG49 --- .../robot/commands/amper/AmperIntake.java | 25 ++++++++ .../robot/commands/amper/AmperOuttake.java | 30 ++++++++++ .../robot/commands/amper/AmperScoreAmp.java | 17 ++++++ .../commands/amper/AmperScoreForever.java | 20 +++++++ .../robot/commands/amper/AmperScoreTrap.java | 17 ++++++ .../robot/commands/amper/AmperToHeight.java | 22 +++++++ .../commands/amper/AmperWaitToHeight.java | 24 ++++++++ .../stuypulse/robot/constants/Settings.java | 25 ++++++-- .../robot/subsystems/amper/Amper.java | 58 +++++++++++++++++-- .../robot/subsystems/amper/AmperImpl.java | 16 ++--- .../robot/subsystems/amper/AmperSim.java | 30 +++++----- 11 files changed, 248 insertions(+), 36 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/amper/AmperIntake.java create mode 100644 src/main/java/com/stuypulse/robot/commands/amper/AmperOuttake.java create mode 100644 src/main/java/com/stuypulse/robot/commands/amper/AmperScoreAmp.java create mode 100644 src/main/java/com/stuypulse/robot/commands/amper/AmperScoreForever.java create mode 100644 src/main/java/com/stuypulse/robot/commands/amper/AmperScoreTrap.java create mode 100644 src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java create mode 100644 src/main/java/com/stuypulse/robot/commands/amper/AmperWaitToHeight.java diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperIntake.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperIntake.java new file mode 100644 index 00000000..1ae73d54 --- /dev/null +++ b/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(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperOuttake.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperOuttake.java new file mode 100644 index 00000000..745dd149 --- /dev/null +++ b/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(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreAmp.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreAmp.java new file mode 100644 index 00000000..b6ebdb86 --- /dev/null +++ b/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() + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreForever.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreForever.java new file mode 100644 index 00000000..a35c6db4 --- /dev/null +++ b/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(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreTrap.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreTrap.java new file mode 100644 index 00000000..a564430c --- /dev/null +++ b/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() + ); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java new file mode 100644 index 00000000..029483d4 --- /dev/null +++ b/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); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperWaitToHeight.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperWaitToHeight.java new file mode 100644 index 00000000..bafc83da --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index a0d6f787..2d63cf09 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/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); } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java b/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java index 6c5f46b5..a749f26f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java +++ b/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()); + } } \ 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 index 8adaa7da..a323dd57 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java +++ b/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 { diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java index 368b37c0..48d1c9b4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java @@ -1,26 +1,23 @@ package com.stuypulse.robot.subsystems.amper; -import static com.stuypulse.robot.constants.Settings.Amper.Lift.*; +import static com.stuypulse.robot.constants.Settings.Amper.Lift.CARRIAGE_MASS; +import static com.stuypulse.robot.constants.Settings.Amper.Lift.MAX_HEIGHT; +import static com.stuypulse.robot.constants.Settings.Amper.Lift.MIN_HEIGHT; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Amper.Lift; import com.stuypulse.robot.constants.Settings.Amper.Lift.Encoder; -import com.stuypulse.stuylib.control.Controller; -import com.stuypulse.stuylib.control.feedback.PIDController; 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 AmperSim extends Amper { private final ElevatorSim sim; - public final Controller liftController; - public AmperSim() { - liftController = new PIDController(Lift.PID.kP, Lift.PID.kI, Lift.PID.kD); - sim = new ElevatorSim( DCMotor.getNEO(1), Encoder.GEARING, @@ -68,8 +65,8 @@ public double getLiftHeight() { public void stopRoller() {} @Override - public void simulationPeriodic() { - liftController.update(targetHeight.get(), getLiftHeight()); + public void periodic() { + super.periodic(); if (liftAtBottom() && liftController.getOutput() < 0) { stopLift(); @@ -77,11 +74,14 @@ public void simulationPeriodic() { sim.setInputVoltage(liftController.getOutput()); } - sim.update(Settings.DT); - SmartDashboard.putNumber("Amper/Lift Height", getLiftHeight()); } - -} - \ No newline at end of file + @Override + public void simulationPeriodic() { + RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); + + sim.update(Settings.DT); + } + +} \ No newline at end of file