driving works

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.4.2"
+    id "edu.wpi.first.GradleRIO" version "2023.4.3"
     // START: Setup spotless
     id 'com.diffplug.spotless' version '6.18.0'
     // END: Setup spotless
@@ -47,7 +47,7 @@ wpi.sim.addGui().defaultEnabled = true
 wpi.sim.addDriverstation()
 
 //Sets the websocket client remote host.
-wpi.sim.envVar("HALSIMWS_HOST", "10.0.0.2")
+wpi.sim.envVar("HALSIMWS_HOST", "192.168.50.221")
 wpi.sim.addWebsocketsServer().defaultEnabled = true
 wpi.sim.addWebsocketsClient().defaultEnabled = true
 

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,98 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,31 @@
+{
+  "HALProvider": {
+    "Other Devices": {
+      "RomiGyro": {
+        "header": {
+          "open": true
+        }
+      }
+    }
+  },
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Field": "Field2d",
+      "/SmartDashboard/SendableChooser[0]": "String Chooser"
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "Shuffleboard": {
+        "open": true
+      },
+      "SmartDashboard": {
+        "SendableChooser[0]": {
+          "open": true
+        },
+        "open": true
+      }
+    }
+  }
+}

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

@@ -21,8 +21,10 @@
 import frc.robot.commands.AutonomousDistance;
 import frc.robot.commands.AutonomousTime;
 import frc.robot.commands.DriveVision;
+import frc.robot.commands.GripperCommand;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Gripper;
 import frc.robot.subsystems.OnBoardIO;
 import frc.robot.subsystems.OnBoardIO.ChannelMode;
 import frc.robot.subsystems.drive.DriveIORomi;
@@ -41,6 +43,7 @@ public class RobotContainer {
   // Create an arm sub-system
   private final Arm m_arm = new Arm();
   // END: Setup arm
+  private final Gripper m_gripper = new Gripper();
 
   // Assumes a XBox controller plugged into channnel 0
   private final XboxController m_controller = new XboxController(0);
@@ -108,8 +111,9 @@ private void configureButtonBindings() {
     // In this case, we want "forward" = "arm up" = positive value, but forward is reported as a
     // negative value from
     // the controller's stick, so we negate the returned value.
-    m_arm.setDefaultCommand(new ArmCommand(m_arm, () -> -m_controller.getRightY()));
+    m_arm.setDefaultCommand(new ArmCommand(m_arm, () -> m_controller.getRightY()));
     // END: Setup arm
+    m_gripper.setDefaultCommand(new GripperCommand(m_gripper, () -> m_controller.getRightX()));
   }
 
   /**

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

@@ -1,9 +1,11 @@
 package frc.robot.commands;
 
+import java.util.function.DoubleSupplier;
+
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Arm;
-import java.util.function.DoubleSupplier;
 
 public class ArmCommand extends CommandBase {
   // the arm subsystem used by this comment
@@ -13,18 +15,18 @@ public class ArmCommand extends CommandBase {
   private final DoubleSupplier m_baseSpeed;
 
   // default start position for the arm when the command is first scheduled
-  private final double m_defaultPosition = 0.5;
+  private final double m_defaultPosition = 0.2;
 
   private final double m_deadband =
       0.05; // to prevent stick drift, this value sets the min absolute value the speed needs to be
 
   // to prevent stick drift, this value sets the min absolute value the speed needs to be
   // before we assume it is not zero
   private final double m_speedScale =
-      16; // to reduce stick sensitivity, this value indicates how much to scale the returned speed
+      64; // to reduce stick sensitivity, this value indicates how much to scale the returned speed
   // by
-  private final double m_minBaseRange = 0; // min range for the arm's base
-  private final double m_maxBaseRange = 1; // max range for the arm's base
+  private final double m_minBaseRange = 0.2; // min range for the arm's base
+  private final double m_maxBaseRange = 0.525; // max range for the arm's base
 
   public ArmCommand(Arm arm, DoubleSupplier baseSpeed) {
     m_arm = arm;
@@ -58,5 +60,8 @@ public void execute() {
 
     // Finally, set the arm base positon to the new calculated arm position.
     m_arm.setArmBasePosition(newArmPosition);
+
+    SmartDashboard.putNumber("Arm Position", m_arm.getArmBasePosition());
+    SmartDashboard.putNumber("New Arm Position", newArmPosition);
   }
 }

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

@@ -16,9 +16,10 @@ public class AutonomousDistance extends SequentialCommandGroup {
    */
   public AutonomousDistance(Drivetrain drivetrain) {
     addCommands(
-        new DriveDistance(-0.5, 10, drivetrain),
-        new TurnDegrees(-0.5, 180, drivetrain),
-        new DriveDistance(-0.5, 10, drivetrain),
-        new TurnDegrees(0.5, 180, drivetrain));
+        new DriveDistance(0.65, 11, drivetrain)
+        //new TurnDegrees(0.3, 80, drivetrain),
+        //new DriveDistance(0.5, 5, drivetrain),
+        //new TurnDegrees(0.35, 90, drivetrain)
+    );
   }
 }

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

@@ -0,0 +1,68 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Gripper;
+import java.util.function.DoubleSupplier;
+
+public class GripperCommand extends CommandBase {
+  // the gripper subsystem used by this comment
+  private final Gripper m_gripper;
+
+  // the supplier of the desired speed
+  private final DoubleSupplier m_baseSpeed;
+
+  // default start position for the gripper when the command is first scheduled
+  private final double m_defaultPosition = 0.5;
+
+  private final double m_deadband =
+      0.05; // to prevent stick drift, this value sets the min absolute value the speed needs to be
+
+  // to prevent stick drift, this value sets the min absolute value the speed needs to be
+  // before we assume it is not zero
+  private final double m_speedScale =
+      16; // to reduce stick sensitivity, this value indicates how much to scale the returned speed
+  // by
+  private final double m_minBaseRange = -1.50; // min range for the gripper's base
+  private final double m_maxBaseRange = 4.00; // max range for the gripper's base
+
+  public GripperCommand(Gripper gripper, DoubleSupplier baseSpeed) {
+    m_gripper = gripper;
+    m_baseSpeed = baseSpeed;
+
+    // To prevent scheduling conflicts, all commands need to indicate which sub-system(s) it uses
+    addRequirements(gripper);
+  }
+
+  @Override
+  public void initialize() {
+    // We set the gripper's base position to the default position when this command is first
+    // scheduled
+    m_gripper.setClawPosition(m_defaultPosition);
+  }
+
+  @Override
+  public void execute() {
+    // Variable that will store the new calculated gripper position
+    double newClawPosition;
+
+    // Determine the requested speed, but ignoring inputs near-zero (i.e. +/= m_deadband)
+    double newBasePositionDelta = MathUtil.applyDeadband(m_baseSpeed.getAsDouble(), m_deadband);
+    // Scale the speed as desired to reduce sensitivity
+    newBasePositionDelta /= m_speedScale;
+
+    // Calculate the gripper position by adding the computer speed to the gripper base's current
+    // position
+    newClawPosition = m_gripper.getClawPosition() + newBasePositionDelta;
+
+    // Clamp the gripper's upper/lower position to the min/max range allowed
+    newClawPosition = MathUtil.clamp(newClawPosition, m_minBaseRange, m_maxBaseRange);
+
+    // Finally, set the gripper base positon to the new calculated gripper position.
+    m_gripper.setClawPosition(newClawPosition);
+
+    SmartDashboard.putNumber("Gripper Position", m_gripper.getClawPosition());
+    SmartDashboard.putNumber("New Gripper Position", newClawPosition);
+  }
+}

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

@@ -7,7 +7,7 @@ public class Arm extends SubsystemBase {
   // This code assumes we are using the ROMI's external IO configuration and the arm base servo is
   // connected to Romi Pin EXT4 which is configured as PWM and mapped to port 3
   // https://docs.wpilib.org/en/stable/docs/romi-robot/web-ui.html#external-io-configuration
-  private final Servo m_armBaseServo = new Servo(3);
+  private final Servo m_armBaseServo = new Servo(2);
 
   // Note: If you have a multi-axis arm, you could create another Servo object using a differemt PWM
   // channel and add the corresponding methods for controlling the position.

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

@@ -0,0 +1,40 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Gripper extends SubsystemBase {
+  // This code assumes we are using the ROMI's external IO configuration and the arm claw servo is
+  // connected to Romi Pin EXT4 which is configured as PWM and mapped to port 2
+  // https://docs.wpilib.org/en/stable/docs/romi-robot/web-ui.html#external-io-configuration
+  private final Servo m_armClawServo = new Servo(3);
+
+  // Note: If you have a multi-axis arm, you could create another Servo object using a differemt PWM
+  // channel and add the corresponding methods for controlling the position.
+  // private final Servo m_armElbowServo = new Servo(2);
+
+  /**
+   * Moves the arm claw to the desired position. -1.0 is max down, and 1.0 is max up
+   *
+   * <p>Note: The concept of "up and down" depends on how the servo is mounted to the arm. E.g. if
+   * the servo is flipped, the directions would be reversed. This module abstracts the hardware
+   * implementation details and follows the convention of 1.0 is up.
+   *
+   * @param position desired target position for arm claw [0.0 to 1.0]
+   */
+  public void setClawPosition(double position) {
+    // Note: This code doesn't validate the requested position.  If we don't want the arm to move
+    // within the entire range (e.g. due to physical constraints), we could clamp the position to
+    // the min/max values before setting the servo's position
+    m_armClawServo.set(position);
+  }
+
+  /**
+   * @return the current desired target position for arm claw. Note: since the basic servo doesn't
+   *     have any feedback, this is just the _requested_ position. The actual servo position is not
+   *     known.
+   */
+  public double getClawPosition() {
+    return m_armClawServo.get();
+  }
+}