Arm subsystem

simgui.json

@@ -15,7 +15,9 @@
       "/SmartDashboard/Field": "Field2d",
       "/SmartDashboard/IntakeCommand": "Command",
       "/SmartDashboard/PIDController[2]": "PIDController",
-      "/SmartDashboard/Schedular": "Scheduler"
+      "/SmartDashboard/Schedular": "Scheduler",
+      "/SmartDashboard/VisionDebug": "Field2d",
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d"
     },
     "windows": {
       "/SmartDashboard/Auto Chooser": {

src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java

@@ -6,6 +6,8 @@
 
 import org.jmhsrobotics.frc2024.controlBoard.CompControl;
 import org.jmhsrobotics.frc2024.controlBoard.ControlBoard;
+import org.jmhsrobotics.frc2024.subsystems.arm.ArmCommand;
+import org.jmhsrobotics.frc2024.subsystems.arm.ArmSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.drive.DriveConstants;
 import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand;
@@ -32,6 +34,7 @@ public class RobotContainer {
 	private ControlBoard control = new CompControl();
 	// Subsystems
 	private final DriveSubsystem driveSubsystem = new DriveSubsystem();
+	private final ArmSubsystem armSubsystem = new ArmSubsystem();
 
 	private final VisionSubsystem vision = new VisionSubsystem(this.driveSubsystem);
 
@@ -43,6 +46,8 @@ public RobotContainer() {
 		SmartDashboard.putData("Schedular", CommandScheduler.getInstance());
 		configureBindings();
 
+		armSubsystem.setDefaultCommand(new ArmCommand(0, armSubsystem));
+
 		// Named commands must be added before building the chooser.
 		configurePathPlanner();
 		autoChooser = AutoBuilder.buildAutoChooser();

src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmCommand.java

@@ -0,0 +1,44 @@
+package org.jmhsrobotics.frc2024.subsystems.arm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+public class ArmCommand extends Command {
+
+	private ArmSubsystem armSubsystem;
+	private double angle;
+	ProfiledPIDController ArmPID;
+
+	public ArmCommand(double angle, ArmSubsystem armSubsystem) {
+		this.armSubsystem = armSubsystem;
+		this.angle = angle;
+		this.ArmPID = new ProfiledPIDController(.1, 0, 0, null);
+		addRequirements(armSubsystem);
+
+	}
+	@Override
+	public void initialize() {
+		// TODO Auto-generated method stub
+		super.initialize();
+		ArmPID.reset(new State(angle, 0));
+		ArmPID.setGoal(angle);
+		ArmPID.setTolerance(3, 3);
+	}
+
+	@Override
+	public void execute() {
+
+		double current = ArmPID.calculate(armSubsystem.getArmPitch());
+
+		armSubsystem.setArmPivot(current);
+
+	}
+
+	public boolean isFinished() {
+		return ArmPID.atGoal();
+	}
+
+	public void end() {
+		armSubsystem.setArmPivot(0);
+	}
+}

src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java

@@ -0,0 +1,50 @@
+package org.jmhsrobotics.frc2024.subsystems.arm;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+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 RelativeEncoder PitchEncoder = armPivot.getEncoder();
+	// private AbsoluteEncoder PitchEncoder=armPivot.getAbsoluteEncoder(kDutyCycle);
+	// private AbsoluteEncoder PitchEncoder=armPivot.getEncoder();
+
+	public ArmSubsystem() {
+
+		armPivot.setSmartCurrentLimit(40);
+		armPivot.setIdleMode(com.revrobotics.CANSparkBase.IdleMode.kBrake);
+
+	}
+
+	public void setArmPivot(double amount) {
+		armPivot.set(amount);
+	}
+
+	public double getArmPitch() {
+		return PitchEncoder.getPosition();
+	}
+
+	public void init2d() {
+		Mechanism2d mech = new Mechanism2d(3, 3);
+		MechanismRoot2d root = mech.getRoot("base", 1.5, 0);
+		var m_arm = root.append(new MechanismLigament2d("arm", 2, 0));
+		SmartDashboard.putData("Mech2d", mech);
+	}
+
+	@Override
+	public void periodic() {
+		// TODO Auto-generated method stub
+		super.periodic();
+		m_arm.setAngle(getArmPitch());
+	}
+
+}