Merge branch 'modulus-plus-dev'
This commit is contained in:
evlryah
commit
d1e03616c5
@ -25,7 +25,8 @@ extern long last_p;
|
||||
#define max(x,y) ( (x) > (y) ? (x) : (y) )
|
||||
#define min(x,y) ( (x) < (y) ? (x) : (y) )
|
||||
|
||||
#define MOTOR_MAX_POWER 127.0 // Highest value accepted by motor control functions
|
||||
#define MOTOR_MAX_POWER 127.0 // Highest value accepted by motor control functions
|
||||
#define DRIVE_MOTOR_MAX_POWER 64.0 // Maximum power for drive motors
|
||||
|
||||
// Drive modes
|
||||
#define DRIVE_STOP 0
|
||||
@ -55,8 +56,9 @@ extern long last_p;
|
||||
// Length of the buffer to monitor recent steering encoder positions to calculate speed
|
||||
// The buffer will track the last N states of the encoders, and the times at which they were recorded, to determine the steering motors' current speeds
|
||||
// This value must always be at least 2, otherwise the code will break due to there being an array with a zero or negative length or a division by zero
|
||||
#define ENCODER_BUFFER_ENTRY_COUNT 5
|
||||
#define ENCODER_BUFFER_ENTRY_COUNT 3
|
||||
|
||||
<<<<<<< HEAD
|
||||
|
||||
// Number of encoder ticks per full rotation of each swerve drive steering motor
|
||||
#define STEERING_ENCODER_TICKS_PER_ROTATION (1024.0 * 8.0)
|
||||
@ -68,6 +70,20 @@ 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
|
||||
=======
|
||||
// Steering parameters
|
||||
#define STEERING_ENCODER_TICKS_PER_ROTATION (1024.0 * 8.0) // Number of encoder ticks per full rotation of each swerve drive steering motor
|
||||
#define STEERING_ENCODER_TICKS_PER_DEGREE (STEERING_ENCODER_TICKS_PER_ROTATION / 360.0) // Number of encoder ticks per degree of rotation for the swerve drive steering motors
|
||||
#define STEERING_MOTOR_SPEED_LIMIT 80.0 // Maximum speed allowed for the steering motors (out of 127.0)
|
||||
// Steering PID parameters
|
||||
#define STEERING_SLOW_DELTA 35.0 // Start decelerating the steering motors linearly when they're within this many degrees of their target angle
|
||||
#define STEERING_ACCEL_SLOW_DELAY 0.20 // Estimated acceleration delay of steering motors at low speeds (seconds)
|
||||
#define STEERING_TOLERANCE 1.0 // Steering tolerance in degrees
|
||||
#define STEERING_STALL_DETECT_ANGULAR_SPEED 5.0 // Detect steering motor stall if measured angular speed is below this
|
||||
#define STEERING_SLOW_APPROACH_SPEED (0.16 * (MOTOR_MAX_POWER / STEERING_MOTOR_SPEED_LIMIT)) // Slow approach speed for steering motors
|
||||
#define STEERING_TOLERANCE_DISABLE_DRIVE 30.0 // Disable the drive motors if any steering motor is off-target by more than this many degrees
|
||||
#define STEERING_HOVER_RANGE 10.0 // Angular range where steering motors tend to hover around their targets
|
||||
>>>>>>> modulus-plus-dev
|
||||
|
||||
// Claw status
|
||||
#define CLAW_UNKNOWN 1 // Position unknown
|
||||
|
||||
97
src/main.cpp
97
src/main.cpp
@ -44,8 +44,13 @@ PCF8574 ioex2(0x21, 20, 21);
|
||||
|
||||
// JANK: soldered to pico or headers
|
||||
PioEncoder enc1(18); // Front Left
|
||||
<<<<<<< HEAD
|
||||
PioEncoder enc2(0); // Front Right // TODO WIRING AND CONFIRMATION 20230929
|
||||
PioEncoder enc3(27); // Back Left // TODO WIRING AND CONFIRMATION 20230929
|
||||
=======
|
||||
PioEncoder enc2(0); // Front Right
|
||||
PioEncoder enc3(2); // Back Left
|
||||
>>>>>>> modulus-plus-dev
|
||||
PioEncoder enc4(14); // Back Right
|
||||
|
||||
|
||||
@ -103,8 +108,8 @@ Sabertooth actuators(130, Serial2);
|
||||
#define TALON_PIN_3 7
|
||||
#define TALON_PIN_4 9
|
||||
// pins for arm servos
|
||||
#define ARM_SERVO_PIN_1 2
|
||||
#define ARM_SERVO_PIN_2 3
|
||||
#define ARM_SERVO_PIN_1 26
|
||||
#define ARM_SERVO_PIN_2 27
|
||||
#define ARM_SERVO_PIN_3 8
|
||||
|
||||
static _107_::Servo talon1, talon2, talon3, talon4, arm1, arm2, arm3;
|
||||
@ -407,10 +412,13 @@ void set_motor(byte motor, int speed) {
|
||||
// 14 : drive 11-13 with identical position & speed
|
||||
// 15 - 17 : arm servos
|
||||
// speed is -127 to 127
|
||||
|
||||
Serial.print("Driving motor ");
|
||||
Serial.print(motor);
|
||||
Serial.print(" with speed ");
|
||||
Serial.println(speed);
|
||||
|
||||
|
||||
if (motor <= 4) {
|
||||
// swerve controls
|
||||
speed *= (((motor % 2) * 2) - 1); // Flip motors 2 and 4
|
||||
@ -434,7 +442,8 @@ void set_motor(byte motor, int speed) {
|
||||
//stepperX.setSpeed((float)speed);
|
||||
if (abs(speed) > 0)
|
||||
ioex1.digitalWrite(2, LOW); // enable
|
||||
|
||||
else
|
||||
stepperX_pos = stepperX.currentPosition();
|
||||
|
||||
stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
|
||||
stepperX_pos = speed * 96 + stepperX.currentPosition();
|
||||
@ -446,7 +455,8 @@ void set_motor(byte motor, int speed) {
|
||||
//stepperY.setSpeed((float)speed);
|
||||
if (abs(speed) > 0)
|
||||
ioex1.digitalWrite(2, LOW); // enable
|
||||
|
||||
else
|
||||
stepperY_pos = stepperY.currentPosition();
|
||||
|
||||
stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
|
||||
stepperY_pos = speed * 96 + stepperY.currentPosition();
|
||||
@ -458,6 +468,8 @@ void set_motor(byte motor, int speed) {
|
||||
//stepperY.setSpeed((float)speed);
|
||||
if (abs(speed) > 0)
|
||||
ioex1.digitalWrite(2, LOW); // enable
|
||||
else
|
||||
stepperZ_pos = stepperZ.currentPosition();
|
||||
|
||||
stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
|
||||
stepperZ_pos = speed * 96 + stepperZ.currentPosition();
|
||||
@ -472,21 +484,34 @@ void set_motor(byte motor, int speed) {
|
||||
|
||||
stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
|
||||
stepperX.moveTo(stepperX_pos);
|
||||
stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
|
||||
stepperY.moveTo(stepperX_pos);
|
||||
//stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
|
||||
//stepperY.moveTo(stepperX_pos);
|
||||
stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
|
||||
stepperZ.moveTo(stepperX_pos);
|
||||
|
||||
stepperX.runState();
|
||||
stepperY.runState();
|
||||
//stepperY.runState();
|
||||
stepperZ.runState();
|
||||
} else {
|
||||
ioex1.digitalWrite(2, HIGH); // disable
|
||||
|
||||
// stepperX_pos = stepperX.currentPosition();
|
||||
|
||||
stepperX.setCurrentPosition(stepperX_pos);
|
||||
//stepperY.setCurrentPosition(stepperX_pos);
|
||||
stepperZ.setCurrentPosition(stepperX_pos);
|
||||
|
||||
//stepperX.stop();
|
||||
//stepperY.stop();
|
||||
//stepperZ.stop();
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
else if (motor == 15)
|
||||
arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
|
||||
else if (motor == 16)
|
||||
arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
|
||||
arm2.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
|
||||
else if (motor == 17)
|
||||
arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
|
||||
}
|
||||
@ -589,19 +614,11 @@ void setup() {
|
||||
ioex1.digitalWrite(3, HIGH);
|
||||
delay(2000);
|
||||
|
||||
digitalWrite(ALI1, LOW);
|
||||
digitalWrite(BLI1, LOW);
|
||||
digitalWrite(AHI1, LOW);
|
||||
digitalWrite(BHI1, LOW);
|
||||
digitalWrite(ALI2, LOW);
|
||||
digitalWrite(BLI2, LOW);
|
||||
digitalWrite(AHI2, LOW);
|
||||
digitalWrite(BHI2, LOW);
|
||||
|
||||
pinMode(ALI1, OUTPUT);
|
||||
pinMode(AHI1, OUTPUT);
|
||||
pinMode(BLI1, OUTPUT);
|
||||
pinMode(BHI1, OUTPUT);
|
||||
pinMode(ALI2, OUTPUT);
|
||||
pinMode(AHI2, OUTPUT);
|
||||
pinMode(BLI2, OUTPUT);
|
||||
@ -694,7 +711,9 @@ void setup() {
|
||||
enc1.begin();
|
||||
//enc1.flip();
|
||||
enc2.begin();
|
||||
//enc2.flip();
|
||||
enc3.begin();
|
||||
//enc3.flip();
|
||||
enc4.begin();
|
||||
Serial.println(" done");
|
||||
delay(20);
|
||||
@ -971,13 +990,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
|
||||
|
||||
|
||||
//DEBUG TESTING code:
|
||||
Serial.printf("FL spin target %f \t\t at %f\r\n", swrv.front_left_target_spin, normalizeAngle(swrv.front_right_spin_angle));
|
||||
Serial.printf("FR spin target %f \t\t at %f\r\n", swrv.front_right_target_spin, normalizeAngle(swrv.front_right_spin_angle));
|
||||
Serial.printf("BL spin target %f \t\t at %f\r\n", swrv.back_left_target_spin, normalizeAngle(swrv.back_left_spin_angle));
|
||||
Serial.printf("BR spin target %f \t\t at %f\r\n", swrv.back_right_target_spin, normalizeAngle(swrv.back_right_spin_angle));
|
||||
|
||||
// Arm motor control (stepper motors), DPAD_UP to move arm up, DPAD_DOWN to move arm down, both or neither being pressed stops the arm
|
||||
float arm_speed = (float) (((int) getButton(DPAD_UP)) - ((int) getButton(DPAD_DOWN))); // TODO 20230929 confirm speed and polarity
|
||||
clawarm = setArmSpeed(clawarm, arm_speed);
|
||||
|
||||
// Claw servo control
|
||||
int new_claw_command = CLAW_COMMAND_UNSET;
|
||||
<<<<<<< HEAD
|
||||
int claw_direction = getButton(CLAW_OPEN_BUTTON) - getButton(CLAW_CLOSE_BUTTON);
|
||||
=======
|
||||
int claw_direction = ((int) getButton(CLAW_OPEN_BUTTON)) - ((int) getButton(CLAW_CLOSE_BUTTON));
|
||||
>>>>>>> modulus-plus-dev
|
||||
switch(claw_direction) {
|
||||
case 0:
|
||||
new_claw_command = CLAW_COMMAND_STAY;
|
||||
@ -995,7 +1025,13 @@ void loop() {
|
||||
int new_tilt_command = TILT_COMMAND_UNSET;
|
||||
clawarm = updateTiltCommand(clawarm, new_tilt_command);
|
||||
|
||||
<<<<<<< HEAD
|
||||
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, previous_loop_processing_duration_core_0); // DEBUG ONLY, telemetry
|
||||
=======
|
||||
|
||||
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, previous_loop_processing_duration_core_0); // DEBUG ONLY, telemetry
|
||||
|
||||
>>>>>>> modulus-plus-dev
|
||||
|
||||
// update motors after calculation
|
||||
set_motor(FLDRIVE, swrv.front_left_power);
|
||||
@ -1003,6 +1039,16 @@ void loop() {
|
||||
set_motor(FRDRIVE, swrv.front_right_power);
|
||||
set_motor(BLDRIVE, swrv.back_left_power);
|
||||
|
||||
|
||||
// Lock the spinlock and transfer the steering motor data to core 1, which will send the data to the sabertooth motor controllers
|
||||
spinlock_lock_core_0(&drive_power_command_spinlock_flag);
|
||||
|
||||
power_data_transfer_fl = swrv.front_left_spin_power;
|
||||
power_data_transfer_fr = swrv.front_right_spin_power;
|
||||
power_data_transfer_bl = swrv.back_left_spin_power;
|
||||
power_data_transfer_br = swrv.back_right_spin_power;
|
||||
spinlock_release(&drive_power_command_spinlock_flag);
|
||||
|
||||
// Lock the spinlock and transfer the steering motor data to core 1, which will send the data to the sabertooth motor controllers
|
||||
spinlock_lock_core_0(&drive_power_command_spinlock_flag);
|
||||
power_data_transfer_fl = swrv.front_left_power;
|
||||
@ -1012,9 +1058,13 @@ void loop() {
|
||||
spinlock_release(&drive_power_command_spinlock_flag);
|
||||
|
||||
// update stepper motors
|
||||
// TESTING: comment out this code to check performance impact
|
||||
set_motor(LIFTALL, clawarm.arm_set_motor_int);
|
||||
|
||||
// update servos
|
||||
Serial.printf("claw set motor int %i\r\n", clawarm.claw_set_motor_int);
|
||||
set_motor(ARMSERVO1, clawarm.claw_set_motor_int);
|
||||
set_motor(ARMSERVO2, - clawarm.claw_set_motor_int);
|
||||
/*
|
||||
// TODO: Figure out servo mapping
|
||||
set_motor(SERVOTILT, clawarm.tilt_set_motor_int);
|
||||
@ -1043,6 +1093,7 @@ void drive_control_core_1() { // Control drive motors from core 1 from loop1() f
|
||||
spinlock_release(&drive_power_command_spinlock_flag); // Release the spinlock
|
||||
|
||||
// Set motors if the requested power is different than the previously requested power
|
||||
<<<<<<< HEAD
|
||||
if(local_fl != power_data_transfer_prev_fl) {
|
||||
set_motor(FLSTEER, local_fl);
|
||||
}
|
||||
@ -1055,6 +1106,20 @@ void drive_control_core_1() { // Control drive motors from core 1 from loop1() f
|
||||
if(local_br != power_data_transfer_prev_br) {
|
||||
set_motor(BRSTEER, local_br);
|
||||
}
|
||||
=======
|
||||
//if(local_fl != power_data_transfer_prev_fl) {
|
||||
set_motor(FLSTEER, local_fl);
|
||||
//}
|
||||
//if(local_fr != power_data_transfer_prev_fr) {
|
||||
set_motor(FRSTEER, local_fr);
|
||||
//}
|
||||
//if(local_bl != power_data_transfer_prev_bl) {
|
||||
set_motor(BLSTEER, local_bl);
|
||||
//}
|
||||
//if(local_br != power_data_transfer_prev_br) {
|
||||
set_motor(BRSTEER, local_br);
|
||||
//}
|
||||
>>>>>>> modulus-plus-dev
|
||||
|
||||
// Set the previously requested power data to the current power data, will be read in the next loop
|
||||
power_data_transfer_prev_fl = local_fl;
|
||||
|
||||
@ -106,7 +106,7 @@ manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed) // Set the a
|
||||
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)));
|
||||
out.arm_set_motor_int = min(127, max(-127, (int) (127.0f * arm_speed))) * 2;
|
||||
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
|
||||
|
||||
@ -54,18 +54,33 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
|
||||
swerve_drive updateSwerveCommand(swerve_drive input)
|
||||
{
|
||||
swerve_drive out = input;
|
||||
|
||||
float new_drive_coefficient = out.target_drive_power;
|
||||
// Set the new speed of the steering motors
|
||||
if((out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) && out.enable_steering) { // Only set the steering power if the robot is trying to move, and if steering is enabled
|
||||
if(/*(out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) &&*/ out.enable_steering) { // Only set the steering power if the robot is trying to move, and if steering is enabled
|
||||
// Calculate the distance and direction each motor needs to steer from where it is now
|
||||
float front_left_delta = closestAngle(out.front_left_spin_angle, out.front_left_target_spin);
|
||||
float front_right_delta = closestAngle(out.front_right_spin_angle, out.front_right_target_spin);
|
||||
float back_left_delta = closestAngle(out.back_left_spin_angle, out.back_left_target_spin);
|
||||
float back_right_delta = closestAngle(out.back_right_spin_angle, out.back_right_target_spin);
|
||||
out.front_left_spin_power = calculateSteeringMotorSpeed(front_left_delta);
|
||||
out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta);
|
||||
out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta);
|
||||
out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta);
|
||||
// Use the delta and speed of each steering motor to calculate the necessary speed
|
||||
out.front_left_spin_power = calculateSteeringMotorSpeed(front_left_delta, out.front_left_measured_spin_speed);
|
||||
out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta, out.front_right_measured_spin_speed);
|
||||
out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta, out.back_left_measured_spin_speed);
|
||||
out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta, out.back_right_measured_spin_speed);
|
||||
|
||||
float max_abs_steering_delta = max(max(fabs(front_left_delta), fabs(front_right_delta)), max(fabs(back_left_delta), fabs(back_right_delta)));
|
||||
if (max_abs_steering_delta > STEERING_TOLERANCE_DISABLE_DRIVE) {
|
||||
new_drive_coefficient = 0;
|
||||
}
|
||||
Serial.printf("max_abs_steering_delta = %f\t\tndc = %f\r\n", max_abs_steering_delta, new_drive_coefficient);
|
||||
|
||||
// TESTING DEBUG print 20230929
|
||||
Serial.printf("FL delta = %f\t\tFL steer = %f\r\n", front_left_delta, out.front_left_spin_power);
|
||||
Serial.printf("FR delta = %f\t\tFR steer = %f\r\n", front_right_delta, out.front_right_spin_power);
|
||||
Serial.printf("BL delta = %f\t\tBL steer = %f\r\n", back_left_delta, out.back_left_spin_power);
|
||||
Serial.printf("BR delta = %f\t\tBR steer = %f\r\n", back_right_delta, out.back_right_spin_power);
|
||||
|
||||
|
||||
} else { // Stop the steering motors if the robot is stopped and not trying to move, or if steering is disabled
|
||||
out.front_left_spin_power = 0.0f;
|
||||
out.front_right_spin_power = 0.0f;
|
||||
@ -74,24 +89,43 @@ swerve_drive updateSwerveCommand(swerve_drive input)
|
||||
}
|
||||
|
||||
// Set the current drive power to the target drive power, TODO: this is TEMPORARY, add in something to slow the current (set) speed until the wheels are in the correct direction
|
||||
out.current_drive_power = out.target_drive_power;
|
||||
|
||||
//out.current_drive_power = out.target_drive_power;
|
||||
// Set the new drive motor power, apply coefficients, set between -127.0 and 127.0
|
||||
out.front_left_power = out.current_drive_power * out.front_left_coefficient * MOTOR_MAX_POWER;
|
||||
out.front_right_power = out.current_drive_power * out.front_right_coefficient * MOTOR_MAX_POWER;
|
||||
out.back_left_power = out.current_drive_power * out.back_left_coefficient * MOTOR_MAX_POWER;
|
||||
out.back_right_power = out.current_drive_power * out.back_right_coefficient * MOTOR_MAX_POWER;
|
||||
out.current_drive_power = new_drive_coefficient;
|
||||
out.front_left_power = new_drive_coefficient * out.front_left_coefficient * DRIVE_MOTOR_MAX_POWER;
|
||||
out.front_right_power = new_drive_coefficient * out.front_right_coefficient * DRIVE_MOTOR_MAX_POWER;
|
||||
out.back_left_power = new_drive_coefficient * out.back_left_coefficient * DRIVE_MOTOR_MAX_POWER;
|
||||
out.back_right_power = new_drive_coefficient * out.back_right_coefficient * DRIVE_MOTOR_MAX_POWER;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
float calculateSteeringMotorSpeed(float steering_delta) // Calculate the speed of a steering motor based on its distance from its target angle
|
||||
float calculateSteeringMotorSpeed(float steering_delta, float current_angular_speed) // Calculate the speed of a steering motor based on its distance from its target angle and its current angular speed
|
||||
{
|
||||
float abs_steering_delta = fabs(steering_delta);
|
||||
if(abs_steering_delta > STEERING_SLOW_DELTA) { // In full speed range, still far enough away from the target angle
|
||||
return STEERING_MOTOR_SPEED_LIMIT;
|
||||
if(abs_steering_delta > STEERING_SLOW_DELTA && abs_steering_delta > STEERING_TOLERANCE) { // In full speed range, still far enough away from the target angle
|
||||
return STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f);
|
||||
} else { // Slow down the speed of the steering motor since it's close to its target angle
|
||||
return STEERING_MOTOR_SPEED_LIMIT * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
|
||||
|
||||
float calc_steering_delta = steering_delta + (STEERING_ACCEL_SLOW_DELAY * current_angular_speed); // Modify the steering delta to the estimated delta in STEERING_ACCEL_SLOW_DELAY seconds to account for motor acceleration
|
||||
float calc_steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (calc_steering_delta < 0.0f ? -1.0f : 1.0f); // Update the sign to account for the future location estimation above
|
||||
float calc_abs_steering_delta = fabs(calc_steering_delta); // Update abs_steering_delta with the new steering_delta
|
||||
float steering_speed_fraction = powf(calc_abs_steering_delta / STEERING_SLOW_DELTA, 2.0f); // Fraction of full speed being used
|
||||
//return steering_limit_signed * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
|
||||
if(current_angular_speed < STEERING_STALL_DETECT_ANGULAR_SPEED || steering_speed_fraction < STEERING_SLOW_APPROACH_SPEED) { // Detect motor stall during approach and increase speed to allow for approach
|
||||
steering_speed_fraction = STEERING_SLOW_APPROACH_SPEED;
|
||||
|
||||
if(calc_abs_steering_delta < STEERING_HOVER_RANGE) { // Decrease speed further if the steering is extremely close to the target
|
||||
steering_speed_fraction *= (calc_abs_steering_delta / STEERING_HOVER_RANGE);
|
||||
} else if(abs_steering_delta < STEERING_HOVER_RANGE) {
|
||||
steering_speed_fraction *= (abs_steering_delta / STEERING_HOVER_RANGE);
|
||||
}
|
||||
}
|
||||
if(calc_abs_steering_delta < STEERING_TOLERANCE) { // Stop the steering motors if they are within the tolerance range
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
return calc_steering_limit_signed * steering_speed_fraction; // Apply the direction
|
||||
}
|
||||
}
|
||||
|
||||
@ -234,7 +268,7 @@ swerve_drive rotationDrive(swerve_drive input, float target_speed) // Implementa
|
||||
|
||||
//float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
|
||||
|
||||
out = setTargetSpin(out, 45.0, 135.0, 225.0, 315.0); // Set the target angle for each rotation motor
|
||||
out = setTargetSpin(out, 45.0, 135.0, 315.0, 225.0); // Set the target angle for each rotation motor
|
||||
out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors
|
||||
out = setDriveTargetPower(out, target_speed); // Set the power
|
||||
|
||||
|
||||
@ -81,7 +81,7 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
|
||||
|
||||
swerve_drive updateSwerveCommand(swerve_drive input); // This function calculates the robot's current speed and attempts to modify the current state of the drive towards the target drive state
|
||||
|
||||
float calculateSteeringMotorSpeed(float steering_delta); // Calculate the speed of a steering motor based on its distance from its target angle
|
||||
float calculateSteeringMotorSpeed(float steering_delta, float current_angular_speed); // Calculate the speed of a steering motor based on its distance from its target angle and its current angular speed
|
||||
|
||||
swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder); // Process new encoder data, calculate the speed and angle of the steering motors
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user