Initial Arm implementation

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

@@ -26,4 +26,38 @@ public static class OperatorConstants {
 public static final int LEFT_INTAKE = 0;
 public static final int XBOX_CONTROLLER_PORT = 0;
 public static final int LINE_BREAKER_PORT = 1;
+
+// arm constants
+//TODO all the constants are subject for further tuning and assignment
+public static final int frameMotorPort      = 5;
+public static final int rodMotorPort        = 6;
+public static final int throwerMotorPort    = 7;
+
+public static final int[] kEncoderPorts     =
+                new int[] { 4,5,
+                            6,7,
+                            8,9};
+public static final int kEncoderPPR         = 256;
+public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR;
+
+public static final double kArmOffsetRads   = 0.5;
+public static final double kP               = 1;
+
+public static final double kMaxAccelerationRadPerSecSquared = 10;
+public static final double kMaxVelocityRadPerSecond         = 3;
+
+public static final double kSVolts                      = 1;
+public static final double kGVolts                      = 1;
+public static final double kVVoltSecondPerRad           = 0.5;
+public static final double kAVoltSecondSquaredPerRad    = 0.1;
+
+public enum ArmCommands {
+    CALIBRATE,
+    REST,
+    THROW_UP,
+    THROW_MID,
+    INTAKE
+}
+
 }
+

src/main/java/frc/robot/RobotContainer.java

@@ -12,13 +12,16 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.POVButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.DriveArcade;
 import frc.robot.commands.DriveArcadeCustomized;
 import frc.robot.commands.DriveCurvature;
 import frc.robot.commands.DriveTank;
 import frc.robot.commands.GrabberOne;
+import frc.robot.commands.ArmCommand;
 import frc.robot.subsystems.BoxGrabber;
+import frc.robot.subsystems.Arm;
 // import frc.robot.commands.GrabberIntake;
 // import frc.robot.commands.GrabberOne;
 // import frc.robot.subsystems.BoxGrabber;
@@ -32,18 +35,22 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
+  Arm arm = new Arm();
   BoxGrabber boxGrabber = new BoxGrabber();
   Drivetrain driveTrain = new Drivetrain();
   DigitalInput digitalInput = new DigitalInput(Constants.LINE_BREAKER_PORT); // add port number in constants file
 
-
   XboxController xboxController = new XboxController(Constants.XBOX_CONTROLLER_PORT);
-
+  POVButton[] povButtons;
   // Replace with CommandPS4Controller or CommandJoystick if needed
 
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+    for (int i=0; i<4; ++i) {
+      povButtons[i] = new POVButton(xboxController, i * 90);
+    }
+
     // Configure the trigger bindings
     configureBindings();
     // driveTrain.setDefaultCommand(new DriveArcade(driveTrain, xboxController::getRightX, xboxController::getLeftY));
@@ -66,10 +73,20 @@ public RobotContainer() {
    * joysticks}.
    */
   private void configureBindings() {
+
     // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
       new JoystickButton(xboxController, Button.kX.value).whileTrue(new GrabberOne(boxGrabber, 0.75, digitalInput));
       new JoystickButton(xboxController, Button.kY.value).whileTrue(new GrabberOne(boxGrabber, 0.75, digitalInput));
 
+      // Arm controls
+      //TODO set appropriate button # for each command - 0,1,2,3 - may be inconvenient
+      //OR even map the actions to a different buttons like A,B,...
+      new JoystickButton(xboxController, Button.kA.value).whileTrue(new ArmCommand(arm,Constants.ArmCommands.CALIBRATE));
+      povButtons[0].onTrue(new ArmCommand(arm, Constants.ArmCommands.INTAKE));
+      povButtons[1].onTrue(new ArmCommand(arm, Constants.ArmCommands.THROW_UP));
+      povButtons[2].onTrue(new ArmCommand(arm, Constants.ArmCommands.THROW_MID));
+      povButtons[3].onTrue(new ArmCommand(arm, Constants.ArmCommands.REST));
+
     // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
     // cancelling on release.
 
@@ -81,4 +98,4 @@ private void configureBindings() {
    * @return the command to run in autonomous
    */
 
-}
+}

src/main/java/frc/robot/commands/ArmCommand.java

@@ -0,0 +1,60 @@
+
+// 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;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Arm;
+import frc.robot.Constants;
+
+public class ArmCommand extends CommandBase {
+
+
+    Arm arm;
+    Constants.ArmCommands command;
+
+    public ArmCommand(Arm arm, Constants.ArmCommands comm) {
+        this.arm = arm;
+        this.command = comm;
+
+        addRequirements(arm);
+    }
+
+
+  // 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() {
+      switch (command) {
+          case CALIBRATE:
+              arm.calibrate();
+              break;
+          case INTAKE:
+              arm.intake();
+              break;
+          case THROW_UP:
+              arm.throwUp();
+              break;
+          case THROW_MID:
+              arm.throwMid();
+              break;
+          default:
+              arm.rest();
+      }
+  }
+
+  // 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 false;
+  }
+}

src/main/java/frc/robot/subsystems/Arm.java

@@ -0,0 +1,138 @@
+// 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.subsystems;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+// sim dependencies
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+
+public class Arm extends SubsystemBase {
+
+    private class Frame extends ProfiledPIDSubsystem {
+        private final PWMSparkMax m_frameMotor =
+            new PWMSparkMax(Constants.frameMotorPort);
+        private final Encoder m_frameEncoder =
+            new Encoder(Constants.kEncoderPorts[0], Constants.kEncoderPorts[1]);
+        private final ArmFeedforward m_frameFeedforward =
+            new ArmFeedforward(Constants.kSVolts, Constants.kGVolts,
+                               Constants.kVVoltSecondPerRad,
+                               Constants.kAVoltSecondSquaredPerRad);
+
+        public Frame() {
+            super(
+                new ProfiledPIDController(
+                    Constants.kP,
+                    0,
+                    0,
+                    new TrapezoidProfile.Constraints(
+                        Constants.kMaxVelocityRadPerSecond,
+                        Constants.kMaxAccelerationRadPerSecSquared)),
+                0);
+            m_frameEncoder.setDistancePerPulse(Constants.kEncoderDistancePerPulse);
+        }
+
+        @Override
+        public void useOutput(double output, TrapezoidProfile.State setpoint) {
+            // Calculate the feedforward from the sepoint
+            double feedforward = m_frameFeedforward.calculate(setpoint.position,
+                                                              setpoint.velocity);
+            // Add the feedforward to the PID output to get the motor output
+            m_frameMotor.setVoltage(output + feedforward);
+        }
+
+        @Override
+        public double getMeasurement() {
+            return m_frameEncoder.getDistance() + Constants.kArmOffsetRads;
+        }
+        // Start arm at rest in neutral position
+        //setGoal(Constants.kArmOffsetRads);
+    }
+
+    private class Rod extends ProfiledPIDSubsystem {
+        private final PWMSparkMax m_rodMotor =
+            new PWMSparkMax(Constants.rodMotorPort);
+        private final Encoder m_rodEncoder =
+            new Encoder(Constants.kEncoderPorts[2], Constants.kEncoderPorts[3]);
+        private final ArmFeedforward m_rodFeedforward =
+            new ArmFeedforward(Constants.kSVolts, Constants.kGVolts,
+                               Constants.kVVoltSecondPerRad,
+                               Constants.kAVoltSecondSquaredPerRad);
+
+        public Rod() {
+            super(
+                new ProfiledPIDController(
+                    Constants.kP,
+                    0,
+                    0,
+                    new TrapezoidProfile.Constraints(
+                        Constants.kMaxVelocityRadPerSecond,
+                        Constants.kMaxAccelerationRadPerSecSquared)),
+                0);
+            m_rodEncoder.setDistancePerPulse(Constants.kEncoderDistancePerPulse);
+        }
+
+        @Override
+        public void useOutput(double output, TrapezoidProfile.State setpoint) {
+            // Calculate the feedforward from the sepoint
+            double feedforward = m_rodFeedforward.calculate(setpoint.position,
+                                                            setpoint.velocity);
+            // Add the feedforward to the PID output to get the motor output
+            m_rodMotor.setVoltage(output + feedforward);
+        }
+
+        @Override
+        public double getMeasurement() {
+            return m_rodEncoder.getDistance() + Constants.kArmOffsetRads;
+        }
+
+    }
+
+    public Arm() {
+        Frame frame = new Frame();
+        frame.setGoal(0);
+
+        Rod rod = new Rod();
+        rod.setGoal(0);
+    }
+
+    public void intake(){
+        // move frame and rod to position for pickup,
+        //  frame.setGoal(X);
+        //  rod.setGoal(X);
+        // take cubicBall with claw
+    }
+
+    public void throwUp(){
+        // move frame and rod to position to throw up,
+        // throw cubicBall with claw
+    }
+
+    public void throwMid(){
+        // move frame and rod to position to throw mid,
+        // throw cubicBall with claw
+    }
+
+    public void rest(){
+        // move frame and rod to rest position
+    }
+
+    public void calibrate() {
+    }
+
+    public void stop(){
+        // stop all motors (frame, rod, claw)
+    }
+
+}
+