diff --git a/src/globals.h b/src/globals.h index ef9d197..4d63d41 100644 --- a/src/globals.h +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index 5c8c5e6..d0dfc11 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -14,6 +14,7 @@ #include "SerialUART.h" #include #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 loop_counter = 0; // Counter for loop() , incremented at the end of each 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_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 - // TODO as of 20230927: get encoder values for the correct motors here - int front_left_encoder; - int front_right_encoder; - int back_left_encoder; - int back_right_encoder; + // Initialize swerve_drive struct, load with initial encoder values + int front_left_encoder = enc1.getCount(), front_right_encoder = enc2.getCount(), back_left_encoder = enc3.getCount(), back_right_encoder = enc4.getCount(); swrv = initializeSwerveDrive(front_left_encoder, front_right_encoder, back_left_encoder, back_right_encoder); - swrv.target_drive_power = 0; - swrv.current_drive_power = 0; // don't want the robot to move right away here + // Initialize manipulator + 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(); @@ -955,6 +961,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 @@ -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"); @@ -1205,4 +1245,4 @@ void loop1() { digitalWrite(3, HIGH);*/ delay(25); -} \ No newline at end of file +} diff --git a/src/manipulator.cpp b/src/manipulator.cpp index 5d40368..fb0b2dc 100644 --- a/src/manipulator.cpp +++ b/src/manipulator.cpp @@ -1,3 +1,126 @@ #include #include "globals.h" #include "manipulator.h" +//#include + +// 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; +} + diff --git a/src/manipulator.h b/src/manipulator.h index 8b128b5..05314f9 100644 --- a/src/manipulator.h +++ b/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 + + diff --git a/src/swerve.h b/src/swerve.h index 32b3d39..4a608e3 100644 --- a/src/swerve.h +++ b/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 current_drive_power = 127.0; // -127.0 to 127.0 : CURRENT power being given to drive motors (before coefficient is applied) + 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 = 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_right_power = 127.0; // -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_right_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 = 0.0f; // -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 = 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