Merge pull request #16 from gwhs/ARMS2

Arms2

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
 }
 
 java {
@@ -98,4 +98,4 @@ wpi.java.configureTestTasks(test)
 // Configure string concat to always inline compile
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
-}
+}

src/main/java/frc/robot/Constants.java

@@ -66,6 +66,17 @@ public static final class LimeLightConstants {
     public static final double MID_DISTANCE_SHOOT = 76;
     public static final double TOP_DISTANCE_SHOOT = 42;
   }
+    public static final class Arm {
+    public static final int kSlotIdx = 0;
+    public static final int kPIDLoopIdx = 0;
+    public static final int kTimeoutMs = 30;
+    public static final int ARM_MAX_ANGLE = 270;
+    public static final int ARM_MIN_ANGLE = 0;
+    public static final int ROTATION_TO_DEGREES = 360;
+    public static final int GEAR_RATIO = 16;
+    public static final double ENCODER_RAW_TO_ROTATION = 8132.;
+  }
+
 
   public static final class IntakeConstants {
     public static final double INTAKE_MOTOR_VELOCITY = 2; //units?

src/main/java/frc/robot/Robot.java

@@ -31,7 +31,7 @@ public class Robot extends TimedRobot {
   public static final String LED = "LED";
 
   // change this to match the subsystem container you want to use, or GAME for complete robot
-  public static final String container = DRIVE;
+  public static final String container = ARM;
 
   private Command m_autonomousCommand;
 
@@ -45,7 +45,7 @@ public class Robot extends TimedRobot {
   public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and use the subsystems needed
     // for the specific robot
-
+    
     switch (container){
       case GAME:
         m_baseContainer = new GameRobotContainer();

src/main/java/frc/robot/commands/Arm/SpinNoteContainerMotor.java

@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.Arm;
+import frc.robot.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SpinNoteContainerMotor extends Command {
+
+  private double velocity;
+  private double acceleration;
+
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final ArmSubsystem armSubsystem;
+
+  public SpinNoteContainerMotor(ArmSubsystem armSubsystem, double velocity, double acceleration) {
+    this.armSubsystem = armSubsystem;
+    this.velocity = velocity;
+    this.acceleration = acceleration;
+    addRequirements(armSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    armSubsystem.spinPizzaBoxMotor(velocity, acceleration);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/commands/Arm/StopNoteContainerMotor.java

@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.Arm;
+import frc.robot.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class StopNoteContainerMotor extends Command {
+
+
+
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final ArmSubsystem armSubsystem;
+
+  public StopNoteContainerMotor(ArmSubsystem armSubsystem) {
+    this.armSubsystem = armSubsystem;
+    addRequirements(armSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    armSubsystem.stopPizzaBoxMotor();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/commands/Arm/SwingBack.java

@@ -0,0 +1,49 @@
+package frc.robot.commands.Arm;
+import frc.robot.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+
+public class SwingBack extends Command{
+
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final ArmSubsystem armSubsystem;
+  private double velocity;
+  private double acceleration;
+  private double motorAng;
+  private double tolerance;
+
+
+    // Called when the command is initially scheduled.
+
+  public SwingBack( ArmSubsystem armSubsystem, double velocity, 
+  double acceleration, double tolerance) {
+    this.armSubsystem = armSubsystem;
+    this.velocity = velocity;
+    this.acceleration = acceleration;
+    this.tolerance = tolerance;
+    addRequirements(armSubsystem);
+  }
+  public void initialize() {
+    armSubsystem.setAngle(0, velocity, acceleration);
+
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    armSubsystem.stopArmMotor();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    motorAng = armSubsystem.encoderGetAngle();
+    return Math.abs(motorAng) < tolerance;
+  }
+}

src/main/java/frc/robot/commands/Arm/SwingForward.java

@@ -0,0 +1,51 @@
+package frc.robot.commands.Arm;
+import frc.robot.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+
+public class SwingForward extends Command{
+
+  private double motorAng;
+  private double angle;
+  private double velocity;
+  private double acceleration;
+  private double tolerance;
+
+
+
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final ArmSubsystem armSubsystem;
+    // Called when the command is initially scheduled.
+
+
+  public SwingForward(ArmSubsystem armSubsystem, double angle, double velocity, double acceleration, double tolerance)
+  {
+    this.armSubsystem = armSubsystem;
+    this.angle = angle;
+    this.velocity = velocity;
+    this.acceleration = acceleration;
+    this.tolerance = tolerance;
+    addRequirements(armSubsystem);
+  }
+  public void initialize() {
+    armSubsystem.setAngle(angle, velocity, acceleration);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    armSubsystem.stopArmMotor();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    motorAng = armSubsystem.encoderGetAngle();
+    return Math.abs(motorAng - angle) < tolerance;
+  }
+}

src/main/java/frc/robot/commands/Arm/SwingServo.java

@@ -0,0 +1,47 @@
+package frc.robot.commands.Arm;
+import frc.robot.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+
+public class SwingServo extends Command{
+
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final ArmSubsystem armSubsystem;
+  private double targetAng; 
+    // Called when the command is initially scheduled.
+
+
+  public SwingServo(ArmSubsystem armSubsystem)
+  {
+    this.armSubsystem = armSubsystem;
+    addRequirements(armSubsystem);
+  }
+  public void initialize() {
+    if(Math.abs(armSubsystem.getServoAngle() - 180) < .25) {
+      targetAng = 0;
+      armSubsystem.setServoAngle(0);
+    } else {
+      targetAng = 180;
+      armSubsystem.setServoAngle(180);
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    double motorAng = armSubsystem.getServoAngle();
+
+    return Math.abs(motorAng - targetAng) < .25;
+  }
+}