Manipulator control core code

src/globals.h

@@ -16,6 +16,7 @@ extern long last_p;
 
 // Loop timing
 #define LOOP_DELAY_MS 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
+#define LOOP_DELAY_SECONDS ((float)LOOP_DELAY_MS / 1000.0f) // Previous number expressed in seconds
 
 // Math things
 #define DEGREES_PER_RADIAN (180.0 / M_PI)
@@ -57,6 +58,37 @@ extern long last_p;
 // Start decelerating the steering motors linearly when they're within this many degrees of their target angle
 #define STEERING_SLOW_DELTA 30.0
 
+// Claw status
+#define CLAW_UNKNOWN        1   // Position unknown
+#define CLAW_STOPPED        2   // Stopped in current position (somewhere between open and closed)
+#define CLAW_CLOSED         3   // Claw is currently closed
+#define CLAW_OPEN           4   // Claw is currently open
+#define CLAW_MOVING         5   // The claw is currently moving
+
+// Claw commands
+#define CLAW_COMMAND_UNSET 0    // No command has been set yet, not doing anything
+// #define CLAW_COMMAND_STOP   1   // Stop immediately, no matter the location
+#define CLAW_COMMAND_CLOSE  2   // Close the claw
+#define CLAW_COMMAND_OPEN   3   // Open the claw
+
+// Claw things
+#define CLAW_OPEN_ANGLE 90.0f       // Open position of the claw
+#define CLAW_CLOSED_ANGLE -90.0f    // Closed position of the claw
+#define CLAW_DEFAULT_ANGLE 0.0f     // Default starting claw position
+
+// Tilt servo control parameters
+#define TILT_ANGLE_MIN_UPDATE_INTERVAL 0.2f // Update the tilt servo's target angle only after this many seconds have elapsed since the previous angle update
+#define TILT_ANGLE_MIN_UPDATE_LOOPS (TILT_ANGLE_MIN_UPDATE_INTERVAL / LOOP_DELAY_SECONDS) // Previous value expressed as a number of control loops to wait between updates
+#define TILT_ANGLE_UPDATE_DISTANCE 10.0f // Distance in degrees to shift the servo angle by each update
+#define TILT_MAX_ANGLE 90.0f // Maximum angle allowed for the tilt servo
+#define TILT_MIN_ANGLE -90.0f // Minimum angle allowed for the tilt servo
+#define TILT_FLAT_ANGLE 0.0f // Default/flat/starting angle for the tilt servo
+
+// Tilt servo commands
+#define TILT_COMMAND_UNSET 0
+#define TILT_COMMAND_UP 1
+#define TILT_COMMAND_DOWN 2
+
 #define SerComm Serial1   //Serial port connected to Xbee
 #define DIAMOND_LEFT 0
 #define DIAMOND_DOWN 1

src/main.cpp

@@ -14,6 +14,7 @@
 #include "SerialUART.h"
 #include <Sabertooth.h>
 #include "swerve.h"
+#include "manipulator.h"
 
 const char* ssid = "TEST";
 const char* password = "pink4bubble";
@@ -22,10 +23,14 @@ const char* password = "pink4bubble";
 //SevenSegmentRowDDLayer *sevenSeg; 
 //JoystickDDLayer *appjoystick;
 
+// Swerve Drive control
 swerve_drive swrv;
 // bool drive_type = DRIVE_TRANSLATION;
 int drive_mode = DRIVE_TRANSLATION; 
 
+// Manipulator control (arm and claw)
+manipulator_arm clawarm;
+
 packet_t pA, pB, safe;
 packet_t *astate, *incoming;
 comm_state cs;
@@ -66,8 +71,8 @@ int stepperX_pos = 0;
 int stepperY_pos = 0;
 int stepperZ_pos = 0;
 
-uint64_t previous_loop_start_time = 0; // Track the previous loop start time, used to calculate how long to sleep for until the start of the next loop
+uint64_t previous_loop_start_time_core_0 = 0, previous_loop_start_time_core_1 = 0; // Track the previous loop start time, used to calculate how long to sleep for until the start of the next loop
-uint64_t loop_counter = 0; // Counter for loop() , incremented at the end of each loop
+uint64_t loop_counter_core_0 = 0, loop_counter_core_1 = 0; // Counter for loop() and loop1() respectively, incremented at the end of each loop
 
 #define defMaxSpeed     8000
 #define defAcceleration 8000
@@ -685,7 +690,7 @@ void setup() {
 
 
   
-
+  // Initialize encoders
   Serial.print("Initializing encoders..");
   set_hex(0x5);
   //enc1.begin();
@@ -695,6 +700,7 @@ void setup() {
   Serial.println(" done");
   delay(20);
 
+  // Radio initialization
   Serial.print("Initializing xBee radio..");
   set_hex(0x6);
   SerComm.setRX(17);
@@ -830,16 +836,16 @@ void setup() {
   //dumbdisplay.configAutoPin(DD_AP_VERT); // auto vertical pin layout
 
 
-  // Initialize the swerve_drive struct
+  // Initialize swerve_drive struct, load with initial encoder values
-  // TODO as of 20230927: get encoder values for the correct motors here
+  int front_left_encoder = enc1.getCount(), front_right_encoder = enc2.getCount(), back_left_encoder = enc3.getCount(), back_right_encoder = enc4.getCount();
-  int front_left_encoder;
-  int front_right_encoder;
-  int back_left_encoder;
-  int back_right_encoder;
   swrv = initializeSwerveDrive(front_left_encoder, front_right_encoder, back_left_encoder, back_right_encoder);
 
-  swrv.target_drive_power = 0;
+  // Initialize manipulator
-  swrv.current_drive_power = 0; // don't want the robot to move right away here
+  clawarm = initializeManipulatorArm(clawarm);
+  clawarm = setArmSpeedLimit(clawarm, 1.0f);
+  clawarm = setArmSpeedCoefficient(clawarm, 1.0f);
+
+  // Setup 
   setup_complete = true;
 
   
@@ -881,7 +887,7 @@ void telemetry(float zeroed_lx_float, float zeroed_ly_float, float zeroed_rx_flo
 
   // Encoder data and loop number
   sprintf(buffer, "Loop = %llu  E1 = %i  E2 = %i  E3 = %i  E4 = %i  z_lx = %f  z_ly = %f  z_rx = %f  z_ry = %f  mode = %i  mag-L = %f  ang-L = %f mag-R = %f  ang-R = %f", \
-  loop_counter, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount(), \
+  loop_counter_core_0, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount(), \
   zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, drive_mode, \
   left_joystick_magnitude, left_joystick_angle, right_joystick_magnitude, right_joystick_angle);
   Serial.println(buffer);
@@ -889,7 +895,7 @@ void telemetry(float zeroed_lx_float, float zeroed_ly_float, float zeroed_rx_flo
 }
 
 void loop() {
-  previous_loop_start_time = millis(); // Track the start time of this loop to determine how long to sleep before the next loop
+  previous_loop_start_time_core_0 = millis(); // Track the start time of this loop to determine how long to sleep before the next loop
   
   rp2040.wdt_reset();
   
@@ -956,6 +962,24 @@ void loop() {
   swrv = updateEncoderData(swrv, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct
   swrv = updateSwerveCommand(swrv); // Calculate power for each drive and steering motor
   
+  // Arm motor control (stepper motors)
+  float arm_speed = 0.0f; // TODO as of 20230928: PLACEHOLDER for input for arm speed 
+  clawarm = setArmSpeed(clawarm, arm_speed);
+
+  // Claw servo control
+  int new_claw_command = CLAW_COMMAND_UNSET;
+  /*
+    // TODO select action for claw
+  */
+  clawarm = updateClawCommand(clawarm, new_claw_command);
+
+  // Tilt servo control
+  int new_tilt_command = TILT_COMMAND_UNSET;
+  /*
+    // TODO select action for the tilt servo
+  */
+  clawarm = updateTiltCommand(clawarm, new_tilt_command);
+
   telemetry(zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, loop_drive_mode, left_joystick_angle, left_joystick_magnitude, right_joystick_angle, right_joystick_magnitude); // DEBUG ONLY, print steering motor encoder positions
 
   // update motors after calculation
@@ -968,6 +992,21 @@ void loop() {
   set_motor(FRDRIVE, swrv.front_right_power);
   set_motor(BLDRIVE, swrv.back_left_power);
 
+  // update stepper motors
+  set_motor(LIFTALL, clawarm.arm_set_motor_int);
+
+  // update servos
+  /*
+  // TODO: Figure out servo mapping
+  set_motor(SERVOTILT, clawarm.tilt_set_motor_int);
+  set_motor(SERVOCLAW, clawarm.claw_set_motor_int);
+
+  */
+
+
+  /////////////////////////////////////////////////////////////
+  // END OF MODULUS PLUS CODE UNTIL THE END OF THIS FUNCTION //
+  /////////////////////////////////////////////////////////////
 
   // Goliath / 2 side arcade tank drive code below
   /*int zeroed_power =    -1 * ((int)(astate->stickX) - 127);
@@ -1114,27 +1153,28 @@ void loop() {
   }*/
   //delay(200);
 
-  int64_t delay_time_ms = LOOP_DELAY_MS - (int64_t) (millis() - previous_loop_start_time); // Dynamically calculate delay time
+  int64_t delay_time_ms = LOOP_DELAY_MS - (int64_t) (millis() - previous_loop_start_time_core_0); // Dynamically calculate delay time
   if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS
     delay(delay_time_ms);
   }
-  loop_counter++;
+  loop_counter_core_0++;
   //DDYield();
 
 }
-int loopcount = 0;
+
 void loop1() {
+  previous_loop_start_time_core_1 = millis();
   rp2040.wdt_reset();
   //drive_left(left_enabled, 255);
   //digitalWrite(LED_BUILTIN, HIGH);  
-  if(loopcount == 20) {
+  if(loop_counter_core_1 == 20) {
     //print_status();
-    loopcount = 0;
+    loop_counter_core_1 = 0;
     delay(25);
   }
   else {
     delay(25);
-    loopcount++;
+    loop_counter_core_1++;
   }
 
   //SerComm.println("update");

src/manipulator.cpp

@@ -1,3 +1,126 @@
 #include <Arduino.h>
 #include "globals.h"
 #include "manipulator.h"
+//#include <math.h>
+
+// Utilities
+
+manipulator_arm initializeManipulatorArm(manipulator_arm input) // Initialize an existing manipulator_arm struct
+{
+    manipulator_arm out = input;
+
+    // Do initialization of values here
+    
+    return out;
+}
+
+int servoDegreesToInt(float servo_degrees) // Convert a servo position in degrees to an integer between -127 and 127 to be used by the set_motor() function
+{
+    return (int)(servo_degrees * (90.0 / 127.0));
+}
+
+float clampServoAngle(float servo_angle) // Clamp an angle for a servo between -90.0 and 90.0
+{
+    return max(-90.0f, min(90.0f, servo_angle));
+}
+
+
+// Tilt servo functions
+
+manipulator_arm setTiltAngle(manipulator_arm input, float new_tilt_angle) // Internal function which sets the target angle of the tilt servo
+{
+    manipulator_arm out = input;
+    new_tilt_angle = clampServoAngle(new_tilt_angle); // Clamp the new tilt angle
+    out.tilt_target_angle = new_tilt_angle;
+    out.tilt_set_motor_int = max(TILT_MIN_ANGLE, min(TILT_MAX_ANGLE, new_tilt_angle));
+    return out;
+}
+
+manipulator_arm updateTiltCommand(manipulator_arm input, int new_tilt_command) // Update the command for the tilt motor
+{
+    manipulator_arm out = input;
+    if(new_tilt_command != TILT_COMMAND_UNSET && out.tilt_angle_loops_since_update >= TILT_ANGLE_MIN_UPDATE_LOOPS) { // Ignore value of 0, make sure that its been long enough since the last update
+        float tilt_angle_offset_direction = 0.0f;
+        switch(new_tilt_command) {
+            case TILT_COMMAND_UP:
+                tilt_angle_offset_direction = 1.0f;
+                break;
+            case TILT_COMMAND_DOWN:
+                tilt_angle_offset_direction = -1.0f;
+                break;
+        }
+        float angle_offset = TILT_ANGLE_UPDATE_DISTANCE * tilt_angle_offset_direction; // Degrees that the target angle is being changed by
+        out = setTiltAngle(out, out.tilt_target_angle + angle_offset);
+    } else { // Increment the number of loops since the previous update, since an update was not performed during this loop
+        out.tilt_angle_loops_since_update++;
+    }
+    
+    return out;
+}
+
+// Claw servo functions
+
+manipulator_arm setClawTargetAngle(manipulator_arm input, float new_claw_angle) // Internal function which sets the target angle of the claw servo (NOT THE ACTUAL COMMAND ANGLE)
+{
+    manipulator_arm out = input;
+    out.claw_target_angle = clampServoAngle(new_claw_angle);
+    return out;
+}
+
+manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command) // Update the command and position for the claw servo
+{
+    manipulator_arm out = input;
+    float new_claw_angle;
+    switch(new_claw_command) {
+        case CLAW_COMMAND_UNSET:
+            new_claw_angle = CLAW_DEFAULT_ANGLE;
+            break;
+        case CLAW_COMMAND_OPEN:
+            new_claw_angle = CLAW_OPEN_ANGLE;
+            break;
+        case CLAW_COMMAND_CLOSE:
+            new_claw_angle = CLAW_CLOSED_ANGLE;
+            break;
+        default:
+            new_claw_angle = CLAW_DEFAULT_ANGLE;
+    }
+    out.claw_position = new_claw_angle;
+    out.claw_set_motor_int = servoDegreesToInt(new_claw_angle);
+    return out;
+}
+
+
+// Arm functions (stepper motors)
+
+manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed) // Set the arm's speed
+{
+    manipulator_arm out = input;
+    arm_speed = out.arm_speed_coefficient * min(out.arm_speed_limit, max(-out.arm_speed_limit, arm_speed));
+    out.arm_speed = arm_speed;
+    out.arm_set_motor_int = min(127, max(-127, (int) (127.0f * arm_speed)));
+    return out;
+}
+manipulator_arm setArmSpeedLimit(manipulator_arm input, float arm_speed_limit) // Set the arm's speed limit (applied before coefficient), limit must be between 0.0 and 1.0
+{
+    manipulator_arm out = input;
+    out.arm_speed_limit = min(1.0f, max(0.0f, arm_speed_limit));
+    return out;
+}
+manipulator_arm setArmSpeedCoefficient(manipulator_arm input, float arm_speed_coefficient) // Set the arm's speed coefficient, coefficient must be between 0.0 and 1.0
+{
+    manipulator_arm out = input;
+    out.arm_speed_coefficient = min(1.0f, max(0.0f, arm_speed_coefficient));;
+    return out;
+}
+
+// Template function
+
+manipulator_arm manipulatorTemplate(manipulator_arm input) // manipulator_arm template function
+{
+    manipulator_arm out = input;
+    
+    // Do stuff
+    
+    return out;
+}
+

src/manipulator.h

@@ -1,6 +1,60 @@
 #include "globals.h"
 
-typedef struct {
+// Manipulator arm
 
+// Stepper motors for lift
+// Servo to open and close claw
+// Servo to tilt claw
+
+typedef struct {
+    // Claw servo variables
+    int claw_command = CLAW_COMMAND_UNSET;          // COMMAND for claw servo
+    int claw_set_motor_int = 0;                     // COMMAND for claw servo, formatted to integer used in set_motor()
+    int claw_status = CLAW_UNKNOWN;                 // CURRENT claw position, initially unknown
+    float claw_position = CLAW_DEFAULT_ANGLE;       // CURRENT angle of claw servo
+    float claw_target_angle = CLAW_DEFAULT_ANGLE;   // TARGET position of claw servo, -90.0 to 90.0 degrees
+    float claw_power_limit = 0.0f;                  // LIMIT power for the claw servo
+
+    // Tilt servo variables
+    int tilt_command = TILT_COMMAND_UNSET;          // COMMAND for the tilt servo
+    float tilt_target_angle = TILT_FLAT_ANGLE;      // CURRENT angle in degrees for the claw tilt servo, can be between -90.0 and 90.0 degrees
+    int tilt_set_motor_int = 0;                     // Tilt motor target angle value as an integer used for set_motor()
+    int tilt_angle_loops_since_update = 0;          // Number of control loops since the tilt angle was modified, don't modify the tilt angle if this is less than TILT_ANGLE_MIN_UPDATE_LOOPS
+
+    // Arm motor variables (stepper motors)
+    float arm_speed = 0.0f;                         // Arm command, initialize as stopped, between -1.0 and 1.0
+    float arm_speed_limit = 1.0f;                   // Limit applied to the speed of the arm (before the coefficient), must be between 0.0 and 1.0
+    float arm_speed_coefficient = 1.0f;             // Coefficient applied to the arm speed, has no constraints, default to 1.0 (unmodified)
+    int arm_set_motor_int = 0;                      // Arm speed used for the set_motor() function
 
 } manipulator_arm;
+
+
+// Utilities
+
+manipulator_arm initializeManipulatorArm(manipulator_arm input); // Initialize a manipulator_arm struct
+
+int servoDegreesToInt(float servo_degrees); // Convert a servo position in degrees to an integer between -127 and 127 to be used by the set_motor() function
+
+float clampServoAngle(float servo_angle); // Clamp an angle for a servo between -90.0 and 90.0
+
+// Tilt servo functions
+manipulator_arm setTiltAngle(manipulator_arm input, float new_claw_angle); // Internal function which sets the target angle of the tilt servo
+
+manipulator_arm updateTiltCommand(manipulator_arm input, int tilt_command); // Update the command for the tilt motor
+
+// Claw servo functions
+
+manipulator_arm setClawTargetAngle(manipulator_arm input, float new_claw_angle); // Internal function which sets the target angle of the claw servo (NOT THE ACTUAL COMMAND ANGLE)
+
+manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command); // Update the command and position for the claw servo
+
+// Arm functions (stepper motors)
+
+manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed); // Set the arm's speed
+
+manipulator_arm setArmSpeedLimit(manipulator_arm input, float arm_speed_limit); // Set the arm's speed limit (applied before coefficient), limit must be between 0.0 and 1.0
+
+manipulator_arm setArmSpeedCoefficient(manipulator_arm input, float arm_speed_coefficient); // Set the arm's speed coefficient, coefficient must be between 0.0 and 1.0
+
+

src/swerve.h

@@ -9,13 +9,13 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
     
     int drive_mode = DRIVE_BASIC;
     
-    float target_drive_power = 127.0; // -127.0 to 127.0 : TARGET power that the robot is trying to get to
+    float target_drive_power = 0.0f; // -127.0 to 127.0 : TARGET power that the robot is trying to get to
-    float current_drive_power = 127.0; // -127.0 to 127.0 : CURRENT power being given to drive motors (before coefficient is applied)
+    float current_drive_power = 0.0f; // -127.0 to 127.0 : CURRENT power being given to drive motors (before coefficient is applied)
     
-    float front_left_power = 127.0; // -127.0 to 127.0 : CURRENT power for this specific drive motor
+    float front_left_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor
-    float front_right_power = 127.0; // -127.0 to 127.0 : CURRENT power for this specific drive motor
+    float front_right_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor
-    float back_left_power = 127.0; // -127.0 to 127.0 : CURRENT power for this specific drive motor
+    float back_left_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor
-    float back_right_power = 127.0; // -127.0 to 127.0 : CURRENT power for this specific drive motor
+    float back_right_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor
 
     float spin_location = 0; // 0 - 360, CURRENT tracked body orientation relative to initial (or reset) orientation
     float spin_offset = 0; // 0 - 360, offset applied to target spin when setting, this allows modification of the robot's forward orientation relative to its body