From c509c15c089b5afdfb43dd965f0b1a5a270aefc5 Mon Sep 17 00:00:00 2001 From: Luhan Wang <117863446+Lu-han-wang@users.noreply.github.com> Date: Thu, 1 Feb 2024 18:05:33 -0500 Subject: [PATCH] Arm subsystem (#52) * WORK IN PROGRESS. basic arm setpoint control. TODO: switch to abs encoder, make open loop control. * changed rel encoder to absolute, and finished sim. TODO: tune PID values and add setpoints * added basic setpoints, tried to get sim to work. TODO: fix sim * Fix git ignore. * Fix compile Error broken import * Very hacky simulation support. * Fix arm simulation * Add pose3d for advantagescope * Renamed Closed loop command to open loop. Added bad static example for armcommand * Cleanup * updated commands' location, fixed formatting * removed unused import in robot container * Clamp Sim voltage * Simulation Fixes. * Update sim to use constant --------- Co-authored-by: m10653 --- .../jmhsrobotics/frc2024/RobotContainer.java | 12 ++- .../frc2024/controlBoard/CompControl.java | 12 +++ .../frc2024/controlBoard/ControlBoard.java | 19 ++-- .../frc2024/subsystems/arm/ArmSubsystem.java | 87 +++++++++++++++++++ .../arm/SimableAbsoluteEncoder.java | 86 ++++++++++++++++++ .../subsystems/arm/commands/ArmCommand.java | 60 +++++++++++++ .../commands/ArmOpenLoopControlCommand.java | 29 +++++++ 7 files changed, 297 insertions(+), 8 deletions(-) create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmCommand.java create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmOpenLoopControlCommand.java diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index 8ae12b09..fc788a40 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -8,6 +8,9 @@ import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; import org.jmhsrobotics.frc2024.subsystems.LED.LEDSubsystem; import org.jmhsrobotics.frc2024.subsystems.LED.commands.RainbowLEDCommand; +import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; +import org.jmhsrobotics.frc2024.subsystems.arm.commands.ArmCommand; +import org.jmhsrobotics.frc2024.subsystems.arm.commands.ArmOpenLoopControlCommand; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; import org.jmhsrobotics.frc2024.subsystems.drive.commands.auto.DriveTimeCommand; @@ -41,16 +44,20 @@ public class RobotContainer implements Logged { private final LEDSubsystem ledSubsystem = new LEDSubsystem(); private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + private final ArmSubsystem armSubsystem = new ArmSubsystem(); + private final SendableChooser autoChooser; public RobotContainer() { this.driveSubsystem.setDefaultCommand(new DriveCommand(this.driveSubsystem, this.control)); this.ledSubsystem.setDefaultCommand(new RainbowLEDCommand(this.ledSubsystem)); + SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); SmartDashboard.putData("LockAprilTagCommand", new LockAprilTag(4, this.driveSubsystem, this.visionSubsystem)); + SmartDashboard.putData("ArmCommand", new ArmCommand(90, this.armSubsystem)); configureBindings(); - + armSubsystem.setDefaultCommand(new ArmOpenLoopControlCommand(armSubsystem, control)); // Named commands must be added before building the chooser. configurePathPlanner(); autoChooser = AutoBuilder.buildAutoChooser(); @@ -76,6 +83,9 @@ private boolean getAllianceFlipState() { } private void configureBindings() { + this.control.presetHigh().onTrue(new ArmCommand(100, this.armSubsystem)); + this.control.presetLow().onTrue(new ArmCommand(0, this.armSubsystem)); + } public Command getAutonomousCommand() { diff --git a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java index e72dc875..a996b6b9 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java +++ b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java @@ -26,6 +26,18 @@ public double rotationalInput() { return this.driver.getRightX(); } + public double pitchInput() { + return this.driver.getRightY(); + } + + public Trigger presetHigh() { + return this.driver.y(); + } + + public Trigger presetLow() { + return this.driver.a(); + } + @Override public Trigger brake() { return this.driver.leftBumper(); diff --git a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java index 2958f026..6ee743cc 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java +++ b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java @@ -4,17 +4,22 @@ public interface ControlBoard { - // =============Operator Controls============= + // =============Operator Controls============= - // =============Driver Controls============= - public Trigger brake(); + public Trigger presetHigh(); + public Trigger presetLow(); - public Trigger setZeroHeading(); + // =============Driver Controls============= + public Trigger brake(); - public double xInput(); + public Trigger setZeroHeading(); - public double yInput(); + public double xInput(); - public double rotationalInput(); + public double yInput(); + + public double rotationalInput(); + + public double pitchInput(); } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java new file mode 100644 index 00000000..31596ac1 --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java @@ -0,0 +1,87 @@ +package org.jmhsrobotics.frc2024.subsystems.arm; + +import org.jmhsrobotics.frc2024.Constants; +import org.jmhsrobotics.warcore.nt.NT4Util; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder.Type; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ArmSubsystem extends SubsystemBase { + + private MechanismLigament2d m_arm; + private CANSparkMax armPivot = new CANSparkMax(9, MotorType.kBrushless); + private SimableAbsoluteEncoder pitchEncoder; + private Mechanism2d mech; + + public ArmSubsystem() { + pitchEncoder = new SimableAbsoluteEncoder(armPivot.getAbsoluteEncoder(Type.kDutyCycle)); + armPivot.setSmartCurrentLimit(40); + armPivot.setIdleMode(IdleMode.kBrake); + init2d(); + if (RobotBase.isSimulation()) { + initSim(); + } + } + + public void setArmPivot(double amount) { + armPivot.set(amount); + // SmartDashboard.putNumber("ArmSubsystem/data/ArmPivotSpeed", amount); + } + + public double getArmPitch() { + return this.pitchEncoder.getPosition(); + } + + public void init2d() { + // TODO: finish sim + mech = new Mechanism2d(3, 3); + MechanismRoot2d root = mech.getRoot("base", 1.5, 1.5); + m_arm = root.append(new MechanismLigament2d("arm", 2, 0)); + // pitchencsim = new SparkAnalogSensor(pitchEncoder); + + } + + @Override + public void periodic() { + // TODO Auto-generated method stub + super.periodic(); + m_arm.setAngle(getArmPitch()); + SmartDashboard.putData("ArmSubsystem/armSIM", mech); + SmartDashboard.putNumber("ArmSubsystem/velocity", this.pitchEncoder.getVelocity()); + SmartDashboard.putNumber("ArmSubsystem/encoder", pitchEncoder.getPosition()); + NT4Util.putPose3d("ArmSubsystem/armpose3d", + new Pose3d(0.2, 0, 0.283, new Rotation3d(0, Units.degreesToRadians(getArmPitch()), 0))); + + } + SingleJointedArmSim armSim; + public void initSim() { + double armGearRatio = 183.33; + double moi = 0.399649199419221; + armSim = new SingleJointedArmSim(DCMotor.getNEO(2), armGearRatio, moi, Units.inchesToMeters(23), 0, + Units.degreesToRadians(180), true, 0); + // simEncoder = new RevEncoderSimWrapper(null, null); + } + @Override + public void simulationPeriodic() { + double armVolts = MathUtil.clamp(armPivot.get() * 12, -12, 12); + armSim.setInputVoltage(armVolts); + armSim.update(Constants.ksimDtSec); + pitchEncoder.setPosition(Units.radiansToDegrees(armSim.getAngleRads())); + } + +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java new file mode 100644 index 00000000..789a65bc --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java @@ -0,0 +1,86 @@ +package org.jmhsrobotics.frc2024.subsystems.arm; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.REVLibError; + +import edu.wpi.first.wpilibj.RobotBase; + +public class SimableAbsoluteEncoder implements AbsoluteEncoder { + + private AbsoluteEncoder enc; + private double position; + + SimableAbsoluteEncoder(AbsoluteEncoder enc) { + this.enc = enc; + } + + @Override + public double getPosition() { + if (RobotBase.isSimulation()) { + return position; + } else { + return enc.getPosition(); + } + + } + + @Override + public double getVelocity() { + return enc.getVelocity(); + } + + @Override + public REVLibError setPositionConversionFactor(double factor) { + return enc.setPositionConversionFactor(factor); + } + + @Override + public double getPositionConversionFactor() { + return enc.getPositionConversionFactor(); + } + + @Override + public REVLibError setVelocityConversionFactor(double factor) { + return enc.setVelocityConversionFactor(factor); + } + + @Override + public double getVelocityConversionFactor() { + return enc.getVelocityConversionFactor(); + } + + @Override + public REVLibError setInverted(boolean inverted) { + return enc.setInverted(inverted); + } + + @Override + public boolean getInverted() { + return enc.getInverted(); + } + + @Override + public REVLibError setAverageDepth(int depth) { + return enc.setAverageDepth(depth); + } + + @Override + public int getAverageDepth() { + return enc.getAverageDepth(); + } + + @Override + public REVLibError setZeroOffset(double offset) { + return enc.setZeroOffset(offset); + } + + @Override + public double getZeroOffset() { + return enc.getZeroOffset(); + } + + public void setPosition(double position) { + this.position = position; + } + +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmCommand.java new file mode 100644 index 00000000..65f52a3c --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmCommand.java @@ -0,0 +1,60 @@ +package org.jmhsrobotics.frc2024.subsystems.arm.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ArmCommand extends Command { + + private ArmSubsystem armSubsystem; + private double angle; + private static ProfiledPIDController armPID; + private static int num = 0; + public ArmCommand(double angle, ArmSubsystem armSubsystem) { + this.armSubsystem = armSubsystem; + this.angle = angle; + // TODO: tune Values; + armPID = new ProfiledPIDController(0.01, 0, 0, new Constraints(180, 180)); + SmartDashboard.putData("ArmPID2", this.armPID); + addRequirements(this.armSubsystem); + + } + + @Override + public void initialize() { + // TODO Auto-generated method stub + + armPID.reset(new State(0, 0)); + armPID.setGoal(this.angle); + armPID.setTolerance(1, 3); + } + + @Override + public void execute() { + + double PIDOut = this.armPID.calculate(this.armSubsystem.getArmPitch()); + PIDOut = MathUtil.clamp(PIDOut, -.5, .5); + armSubsystem.setArmPivot(PIDOut); + SmartDashboard.putNumber("ArmCommand/data/goal", this.angle); + SmartDashboard.putNumber("ArmCommand/data/setPoint", this.armPID.getSetpoint().position); + SmartDashboard.putNumber("ArmCommand/data/PIDOut", PIDOut); + SmartDashboard.putNumber("ArmCommand/data/Current", this.armSubsystem.getArmPitch()); + } + + @Override + public boolean isFinished() { + return false; + // return armPID.atGoal(); + } + + @Override + public void end(boolean interrupted) { + armSubsystem.setArmPivot(0); + } +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmOpenLoopControlCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmOpenLoopControlCommand.java new file mode 100644 index 00000000..f924303b --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmOpenLoopControlCommand.java @@ -0,0 +1,29 @@ +package org.jmhsrobotics.frc2024.subsystems.arm.commands; + +import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; +import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ArmOpenLoopControlCommand extends Command { + + ArmSubsystem armSubsystem; + ControlBoard control; + + public ArmOpenLoopControlCommand(ArmSubsystem armSubsystem, ControlBoard control) { + this.armSubsystem = armSubsystem; + this.control = control; + addRequirements(armSubsystem); + } + + @Override + public void initialize() { + this.armSubsystem.setArmPivot(0); + + } + @Override + public void execute() { + armSubsystem.setArmPivot(control.pitchInput()); + } + +}