From edf89f0d27c5eb5977cd3f31f9b258a3f639a412 Mon Sep 17 00:00:00 2001 From: reyamiller <72947255+reyamiller@users.noreply.github.com> Date: Fri, 26 Jan 2024 14:45:59 -0500 Subject: [PATCH 1/3] set up climber subsystem (#1) * set up climber subsystem * Add setVoltage * Add control * Add motor constants * Add limit constants * rename files * Add fixes Co-authored-by: reyamiller * Remove static imports + add position/velocity conversion Co-authored-by: reyamiller * Remove tabs * Remove BangBangController * fix targetHeight * Fix setVoltage * Add climber settings --------- Co-authored-by: colyic Co-authored-by: reyamiller 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 | 5 + .../com/stuypulse/robot/constants/Ports.java | 10 ++ .../stuypulse/robot/constants/Settings.java | 19 +++- .../robot/subsystems/climber/Climber.java | 39 +++++++ .../robot/subsystems/climber/ClimberImpl.java | 105 ++++++++++++++++++ 6 files changed, 180 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index b5c842c4..c88ca1ab 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -19,6 +19,8 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; +import com.stuypulse.robot.subsystems.climber.*; + import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -30,6 +32,7 @@ public class RobotContainer { public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem + public final Climber climber = Climber.getInstance(); public final Amper amper = Amper.getInstance(); public final SwerveDrive swerve = SwerveDrive.getInstance(); public final Odometry odometry = Odometry.getInstance(); diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 62982d2b..d6a79bef 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -48,6 +48,11 @@ public interface Conveyor { CANSparkMaxConfig SHOOTER_FEEDER_MOTOR = new CANSparkMaxConfig(false, IdleMode.kCoast); } + public interface Climber { + CANSparkMaxConfig LEFT_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake, 80); + CANSparkMaxConfig RIGHT_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake, 80); + } + public static class TalonSRXConfig { public final boolean INVERTED; public final NeutralMode NEUTRAL_MODE; diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 46fdb439..a0a1a801 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -13,6 +13,16 @@ public interface Gamepad { int DEBUGGER = 2; } + public interface Climber { + int LEFT_MOTOR = 60; + int RIGHT_MOTOR = 61; + + int TOP_RIGHT_LIMIT = 8; + int TOP_LEFT_LIMIT = 7; + int BOTTOM_RIGHT_LIMIT = 6; + int BOTTOM_LEFT_LIMIT = 5; + } + public interface Amper { int SCORE = 31; int LIFT = 30; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 1f942a5e..a0d6f787 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,6 +5,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; @@ -22,7 +23,23 @@ public interface Settings { double DT = 1.0 / 50.0; - + + public interface Climber { + double MIN_HEIGHT = 0.0; + double MAX_HEIGHT = 0.0; + + double AT_HEIGHT_THRESHOLD = Units.inchesToMeters(1); + + double BANGBANG_VOLTAGE = 8; + + public interface Encoder { + double GEAR_RATIO = 0.0; + + double POSITION_CONVERSION = 0.0; + double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0; + } + } + public interface Amper { public interface Score { SmartNumber ROLLER_SPEED = new SmartNumber("Amper/Score/Roller Speed", 1.0); // change later diff --git a/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java b/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java new file mode 100644 index 00000000..116d014b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java @@ -0,0 +1,39 @@ +package com.stuypulse.robot.subsystems.climber; + +import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class Climber extends SubsystemBase { + private static final Climber instance; + + private final SmartNumber targetHeight; + + static { + instance = new ClimberImpl(); + } + + public static Climber getInstance() { + return instance; + } + + public Climber() { + targetHeight = new SmartNumber("Climber/Target Height", 0.0); + } + + public void setTargetHeight(double height) { + targetHeight.set(height); + } + + public double getTargetHeight() { + return targetHeight.get(); + } + + public abstract double getHeight(); + public abstract double getVelocity(); + + public abstract void setVoltage(double voltage); + + public abstract boolean atTop(); + public abstract boolean atBottom(); +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java new file mode 100644 index 00000000..1b1fa556 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java @@ -0,0 +1,105 @@ +package com.stuypulse.robot.subsystems.climber; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; +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.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ClimberImpl extends Climber { + private final CANSparkMax rightMotor; + private final CANSparkMax leftMotor; + + private final RelativeEncoder rightEncoder; + private final RelativeEncoder leftEncoder; + + private final DigitalInput topRightLimit; + private final DigitalInput topLeftLimit; + private final DigitalInput bottomRightLimit; + private final DigitalInput bottomLeftLimit; + + protected ClimberImpl() { + rightMotor = new CANSparkMax(Ports.Climber.RIGHT_MOTOR, MotorType.kBrushless); + leftMotor = new CANSparkMax(Ports.Climber.LEFT_MOTOR, MotorType.kBrushless); + + rightEncoder = rightMotor.getEncoder(); + leftEncoder = leftMotor.getEncoder(); + + rightEncoder.setPositionConversionFactor(Settings.Climber.Encoder.POSITION_CONVERSION); + leftEncoder.setPositionConversionFactor(Settings.Climber.Encoder.POSITION_CONVERSION); + + rightEncoder.setVelocityConversionFactor(Settings.Climber.Encoder.VELOCITY_CONVERSION); + leftEncoder.setVelocityConversionFactor(Settings.Climber.Encoder.VELOCITY_CONVERSION); + + topRightLimit = new DigitalInput(Ports.Climber.TOP_RIGHT_LIMIT); + topLeftLimit = new DigitalInput(Ports.Climber.TOP_LEFT_LIMIT); + bottomRightLimit = new DigitalInput(Ports.Climber.BOTTOM_RIGHT_LIMIT); + bottomLeftLimit = new DigitalInput(Ports.Climber.BOTTOM_LEFT_LIMIT); + + Motors.Climber.LEFT_MOTOR.configure(leftMotor); + Motors.Climber.RIGHT_MOTOR.configure(rightMotor); + } + + @Override + public double getHeight() { + return (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2; + } + + @Override + public double getVelocity() { + return (leftEncoder.getVelocity() + rightEncoder.getVelocity()) / 2; + } + + @Override + public boolean atTop() { + return !topRightLimit.get() || !topLeftLimit.get(); + } + + @Override + public boolean atBottom() { + return !bottomRightLimit.get() || !bottomLeftLimit.get(); + } + + @Override + public void setVoltage(double voltage) { + if (atTop() && voltage > 0) { + DriverStation.reportWarning("Climber Top Limit Reached", false); + voltage = 0.0; + } else if (atBottom() && voltage < 0) { + DriverStation.reportWarning("Climber Bottom Limit Reached", false); + voltage = 0.0; + } + + rightMotor.setVoltage(voltage); + leftMotor.setVoltage(voltage); + } + + @Override + public void periodic() { + super.periodic(); + + if (Math.abs(getHeight() - getTargetHeight()) < Settings.Climber.AT_HEIGHT_THRESHOLD) { + setVoltage(0.0); + } else if (getHeight() > getTargetHeight()) { + setVoltage(-Settings.Climber.BANGBANG_VOLTAGE); + } else { + setVoltage(+Settings.Climber.BANGBANG_VOLTAGE); + } + + SmartDashboard.putNumber("Climber/Target Height", getTargetHeight()); + SmartDashboard.putNumber("Climber/Height", getHeight()); + + SmartDashboard.putNumber("Climber/Velocity", getVelocity()); + + if (atBottom()) { + leftEncoder.setPosition(Settings.Climber.MIN_HEIGHT); + rightEncoder.setPosition(Settings.Climber.MIN_HEIGHT); + } + } +} \ No newline at end of file From 25357a5fece245173d5580806f13c0860305ee57 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 26 Jan 2024 14:53:40 -0500 Subject: [PATCH 2/3] Shorten swerve calls --- .../com/stuypulse/robot/subsystems/swerve/SwerveDrive.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 9c6e958b..27ea95d0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -146,7 +146,7 @@ public Translation2d[] getModuleOffsets() { } public ChassisSpeeds getChassisSpeeds() { - return getKinematics().toChassisSpeeds(getModuleStates()); + return kinematics.toChassisSpeeds(getModuleStates()); } /** Setters **/ @@ -232,7 +232,6 @@ public void periodic() { public void simulationPeriodic() { //show gyro angle in simulation - ChassisSpeeds speeds = kinematics.toChassisSpeeds(getModuleStates()); - gyro.setAngleAdjustment(gyro.getAngle() - Math.toDegrees(speeds.omegaRadiansPerSecond * Settings.DT)); + gyro.setAngleAdjustment(gyro.getAngle() - Math.toDegrees(getChassisSpeeds().omegaRadiansPerSecond * Settings.DT)); } } \ No newline at end of file From 3eb81d35d1b89453464048a4a49650f2355f8321 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Sat, 27 Jan 2024 21:39:47 -0500 Subject: [PATCH 3/3] 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