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 <[email protected]>

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<Command> 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() {

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

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

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

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

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

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