doing another refactor bruh

Author: aaronrhim

src/arm_control/include/controller_config.h

@@ -15,6 +15,13 @@
  */
 #pragma once
 
+// ============ Controller Selection ============
+// Set ONE of these as the active controller:
+#define CONTROLLER_PRO_CONTROLLER   1
+#define CONTROLLER_CYBORG_STICK     2
+
+#define ACTIVE_CONTROLLER CONTROLLER_PRO_CONTROLLER
+
 #include <sensor_msgs/msg/joy.hpp>
 #include <arm_hardware_interface/ArmSerialProtocol.h>
 inline static constexpr int MAX_BUTTONS = 20; // can be decreased
@@ -216,9 +223,21 @@ namespace ArmControllerConfig { // Can make into a class later?
         case GameController::SWITCH_PRO_CONTROLLER:
             {
                 using namespace switch_index;
-                // TODO
+                arm_control_msg.fk_axes[AXIS_2_INDEX] = joy_msg->axes[axes::LEFT_JOYSTICK_Y];
+                arm_control_msg.fk_axes[AXIS_3_INDEX] = joy_msg->axes[axes::RIGHT_JOYSTICK_Y];
+                arm_control_msg.fk_axes[AXIS_4_INDEX] = joy_msg->axes[axes::RIGHT_JOYSTICK_X];
+                // dpad is buttons on Switch Pro and controls z axis
+                arm_control_msg.fk_axes[AXIS_5_INDEX] = static_cast<float>(joy_msg->buttons[buttons::DPAD_UP] - joy_msg->buttons[buttons::DPAD_DOWN]);
+                arm_control_msg.fk_axes[AXIS_6_INDEX] = static_cast<float>(joy_msg->buttons[buttons::DPAD_RIGHT] - joy_msg->buttons[buttons::DPAD_LEFT]);
 
+                arm_control_msg.ik_axes[IK_LIN_X_INDEX] = joy_msg->axes[axes::LEFT_JOYSTICK_Y];
+                arm_control_msg.ik_axes[IK_LIN_Z_INDEX] = joy_msg->axes[axes::RIGHT_JOYSTICK_Y];
+                arm_control_msg.ik_axes[IK_LIN_Y_INDEX] = joy_msg->axes[axes::LEFT_JOYSTICK_X];
+                arm_control_msg.ik_axes[IK_ANG_X_INDEX] = static_cast<float>(joy_msg->buttons[buttons::DPAD_RIGHT] - joy_msg->buttons[buttons::DPAD_LEFT]);
+                arm_control_msg.ik_axes[IK_ANG_Y_INDEX] = static_cast<float>(joy_msg->buttons[buttons::DPAD_UP] - joy_msg->buttons[buttons::DPAD_DOWN]);
 
+                arm_control_msg.end_effector = joy_msg->buttons[buttons::L1] - joy_msg->buttons[buttons::R1];
+                arm_control_msg.kinematics_mode_switch = joy_msg->buttons[buttons::B];
             }
             break;
         case GameController::CYBORG_JOYSTICK:
@@ -284,10 +303,12 @@ namespace ArmControllerConfig { // Can make into a class later?
 namespace ControllerConfig {
 
     // --- Face Buttons ---
-    constexpr int BTN_B      = 0;   // East
-    constexpr int BTN_A      = 1;   // South
-    constexpr int BTN_Y      = 2;   // North
-    constexpr int BTN_X      = 3;   // West
+    constexpr int BTN_B = 0;   // East (A)
+    constexpr int BTN_A = 1;   // South (B)
+    constexpr int BTN_Y = 2;   // North (X)
+    constexpr int BTN_X = 3;   // West (Y)
+
+    constexpr int BTN_HOME = 5; // Home
 
     constexpr int BTN_UP     = 12;   // Left shoulder
     constexpr int BTN_DOWN   = 13;   // Right shoulder
@@ -321,9 +342,6 @@ namespace ControllerConfig {
     constexpr double CART_BUTTON_SPEED = 0.5;  // unitless, range [0.0, 1.0]
     constexpr double ROT_STICK_SPEED   = 0.6;  // unitless, range [0.0, 1.0] for angular
 
-    // --- Deadzone for analog axes (used later for joystick support) ---
-    constexpr double AXIS_DEADZONE = 0.15;
-
     // --- Frame for Cartesian twist commands ---
     // "base_link" = world-fixed directions (forward is always forward)
     // "ee_base_link" = relative to end-effector orientation
@@ -343,8 +361,16 @@ namespace ControllerConfig {
 
     // --- Gripper ---
     constexpr int BTN_GRIPPER_TOGGLE = 7;  // ZR button — edge-triggered toggle
+    constexpr int AXIS_GRIPPER_TOGGLE = 5; // ZR analog axis (rests at 1.0, pressed = -1.0)
+    constexpr double AXIS_GRIPPER_PRESSED_THRESHOLD = 0.0; // trigger considered pressed below this
     constexpr double GRIPPER_OPEN_VALUE  = 1.0;
     constexpr double GRIPPER_CLOSE_VALUE = 0.0;
+
+    // --- Gripper Sim Positions ---
+    constexpr double GRIPPER_SIM_LEFT_OPEN_POS   =  0.04;
+    constexpr double GRIPPER_SIM_RIGHT_OPEN_POS  = -0.04;
+    constexpr double GRIPPER_SIM_LEFT_CLOSE_POS  =  0.0;
+    constexpr double GRIPPER_SIM_RIGHT_CLOSE_POS =  0.0;
 }
 
 #elif ACTIVE_CONTROLLER == CONTROLLER_CYBORG_STICK
@@ -392,8 +418,28 @@ namespace ControllerConfig {
 
     // --- Gripper ---
     constexpr int BTN_GRIPPER_TOGGLE = BTN_TRIGGER;  // trigger = gripper toggle
+    constexpr int AXIS_GRIPPER_TOGGLE = -1;           // no analog trigger axis
+    constexpr double AXIS_GRIPPER_PRESSED_THRESHOLD = 0.0;
     constexpr double GRIPPER_OPEN_VALUE  = 1.0;
     constexpr double GRIPPER_CLOSE_VALUE = 0.0;
+
+    // --- Gripper Sim Positions ---
+    constexpr double GRIPPER_SIM_LEFT_OPEN_POS   =  0.04;
+    constexpr double GRIPPER_SIM_RIGHT_OPEN_POS  = -0.04;
+    constexpr double GRIPPER_SIM_LEFT_CLOSE_POS  =  0.0;
+    constexpr double GRIPPER_SIM_RIGHT_CLOSE_POS =  0.0;
+
+    // --- Orientation ---
+    constexpr double ROT_STICK_SPEED = 0.4;
+    constexpr int AXIS_ROLL  = AXIS_STICK_X;  // stick X → roll
+    constexpr int AXIS_PITCH = AXIS_STICK_Y;  // stick Y → pitch
+    constexpr int AXIS_YAW   = AXIS_TWIST;    // twist   → yaw
+    constexpr bool INVERT_ROLL  = false;
+    constexpr bool INVERT_PITCH = false;
+    constexpr bool INVERT_YAW   = false;
+
+    // --- Misc ---
+    constexpr int BTN_HOME = -1;  // no home button on Cyborg stick
 }
 
 #else

src/arm_hardware_interface/CMakeLists.txt

@@ -26,8 +26,12 @@ add_executable(
   serial_arm_driver src/ArmSerialInterface.cpp include/ArmSerialInterface.h
                     include/arm_hardware_interface/ArmSerialProtocol.h)
 add_executable(
-  moteus_arm_driver src/MoteusInterface.cpp include/ArmMoteusInterface.h
-                    include/arm_hardware_interface/ArmSerialProtocol.h include/axis_5_6_differential.h)
+  moteus_arm_driver
+    src/moteus_CAN_interface/MoteusMain.cpp
+    src/moteus_CAN_interface/MoteusNode.cpp
+    src/moteus_CAN_interface/CommandCallback.cpp
+    src/moteus_CAN_interface/MoteusInterface.cpp
+    src/moteus_CAN_interface/MoteusIO.cpp)
 
 target_compile_definitions(moteus_arm_driver PRIVATE BUILDING_NEW_DRIVER=1) #??
 

src/arm_hardware_interface/include/ArmMoteusInterface.h

@@ -62,13 +62,13 @@ struct MotorConfig {
 
   // Maximum velocity limit (revolutions/s)
   // Moteus Config: servo.velocity_limit (soft) or servo.max_velocity (hard limit)
-  float max_velocity = 0.05f;
+  float max_velocity = 0.3f;
 
   // Position Limits (revolutions)
   // Moteus Config: servopos.position_min / servopos.position_max
   float position_min = -1.0f;
   float position_max = 1.0f;
-  float position_warn_rev_padding = 0.01; // If position gets to within this amount to min or max, raise rover alert
+  float position_warn_rev_padding = 0.02f;
 
   // ---------------------------------------------------------
   // 2. Torque / Current Limits

src/arm_hardware_interface/include/arm_hardware_interface/ArmSerialProtocol.h

@@ -1,31 +1,31 @@
-/*
-    Converts differential drive requests to motor outputs.
-    Directions given from bird eye view and top down view of each motor.
-
-    ex. A5 = -5, A6 = -3 means spin A5 -5 R to L and A6 down by 3.
-    This requires M5 to move 3 from L to R (positive direction) and M6 to move 3 from R to L (negative direction)
-    from a top down view of each motor. The A5 spin also requires M5 to move 5 more in the positive direction. 
-    (We choose to move the motor to execute the spin as the one that can continue in the current direction of motion)
-
-    takes:
-        axis5_input: Unitless input for axis 5 
-        axis6_input: Unitless input for axis 6
-    returns: 
-        (motor5_output, motor6_output) Unitless values for motors 5 and 6
-*/
-void differential_drive(float axis5_input, float axis6_input, float &motor5_output, float &motor6_output) {
-
-    // If the inputs have the same sign, to continue in the same direction of motion, use axis 5 for rotation
-    if((axis5_input > 0 && axis6_input > 0) || (axis5_input < 0 && axis6_input < 0)) {
-        motor5_output = - axis5_input - axis6_input;
-        motor6_output = motor5_output;
-    }
-
-    // Else, use axis 6 for rotation
-    else {
-        motor5_output = - axis6_input;
-        motor6_output = axis6_input;
-    }  
-
-    return;
+/*
+    Converts differential drive requests to motor outputs.
+    Directions given from bird eye view and top down view of each motor.
+
+    ex. A5 = -5, A6 = -3 means spin A5 -5 R to L and A6 down by 3.
+    This requires M5 to move 3 from L to R (positive direction) and M6 to move 3 from R to L (negative direction)
+    from a top down view of each motor. The A5 spin also requires M5 to move 5 more in the positive direction. 
+    (We choose to move the motor to execute the spin as the one that can continue in the current direction of motion)
+
+    takes:
+        axis5_input: Unitless input for axis 5 
+        axis6_input: Unitless input for axis 6
+    returns: 
+        (motor5_output, motor6_output) Unitless values for motors 5 and 6
+*/
+inline void differential_drive(float axis5_input, float axis6_input, float &motor5_output, float &motor6_output) {
+
+    // If the inputs have the same sign, to continue in the same direction of motion, use axis 5 for rotation
+    if((axis5_input > 0 && axis6_input > 0) || (axis5_input < 0 && axis6_input < 0)) {
+        motor5_output = - axis5_input - axis6_input;
+        motor6_output = motor5_output;
+    }
+
+    // Else, use axis 6 for rotation
+    else {
+        motor5_output = - axis6_input;
+        motor6_output = axis6_input;
+    }  
+
+    return;
 }