adeck/rib-test-code
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Manipulator control core code

Browse Source
This commit is contained in:
evlryah 2023-09-28 20:42:12 -05:00
parent a326d961c3
commit 70bfc17aca
5 changed files with 276 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/globals.h
View File

@ -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

80
src/main.cpp
View File

@ -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 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);
}
}

123
src/manipulator.cpp
View File

@ -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;
}

56
src/manipulator.h
View File

@ -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

12
src/swerve.h
View File

@ -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