Skip to content

Updated the Motor PID source files #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
JoshuaO956 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Updated the Motor PID source files #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
186 changes: 92 additions & 94 deletions projects/drv-modules/motor-base-PID/RUDDERPID.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,17 +13,17 @@
void DAC_STEP(int step) {

/*
* Adjusts the DAC output in steps of 1/50th - used for testing the minimum power required to drive
* certain loads attached to the motor
*
* @ params
* step - an int value between 0 and 50 e.g. passing 2 means 2/50 output of the DAC
*
*/

uint32_t Motor_DAC = (uint32_t)(i * 4095.0f / 50); // Keep DAC value as 12-bit resolution
HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, Motor_DAC); // Set DAC output
float voltage = (i * 3.3f) /100; // Convert DAC value to voltage
* Adjusts the DAC output in steps of 1/50th - used for testing the minimum power required to drive
* certain loads attached to the motor
*
* @ params
* step - an int value between 0 and 50 e.g. passing 2 means 2/50 output of the DAC
*
*/

uint8_t Motor_DAC = (uint8_t)(step * 4095.0f / 50); // Keep DAC value as 12-bit resolution
HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_2, DAC_ALIGN_12B_R, Motor_DAC); // Set DAC output
float voltage = (step * 3.3f) / 50; // Convert DAC value to voltage

printf("%u\r\n", Motor_DAC);
printf("V: %d.%03d V\r\n", (int)voltage, (int)(fabsf(voltage * 1000)) % 1000);
Expand All @@ -45,18 +45,20 @@ void Set_Motor(float_t Motor_Control)

uint32_t Motor_DAC = (uint32_t)(fabsf(Motor_Control) * 4095.0f);

HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, Motor_DAC); // Manually sets the output of the DAC
// (uint32_t)(fabsf(Motor_Control) * 4095.0f);

HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_2, DAC_ALIGN_12B_R, Motor_DAC); // Manually sets the output of the DAC


// Sets PA7 high if Motor_Control is positive, otherwise low

if (Motor_Control < 0.0f)
{
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOF, GPIO_PIN_13, GPIO_PIN_RESET);
}
else
{
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOF, GPIO_PIN_13, GPIO_PIN_SET);
}

return;
Expand All @@ -65,106 +67,102 @@ void Set_Motor(float_t Motor_Control)

void PI_Motor(int32_t desired_heading, int32_t current_heading,
float* integral_error, uint32_t* last_time_stamp,
int32_t* past_encoder_heading, int8_t* past_motor_direction)
int32_t* past_encoder_heading)
{

/*
* Calculates the error in boat's heading and adjusts the rudder accordingly
*
* @params
* desired_heading - target rudder angle (sent by sailing conditions state machine) - E.D.
* current_heading - current rudder angle (sent by encoder) - C.C.
* integral_error - sums up past errors for PI control
* last_time_stamp - saves the last times the PI_controller was called to calculate change in time
/*
* Calculates the error in boat's heading and adjusts the rudder accordingly
*
* @params
* desired_heading - target rudder angle (sent by sailing conditions state machine) - E.D.
* current_heading - current rudder angle (sent by encoder) - C.C.
* integral_error - sums up past errors for PI control
* last_time_stamp - saves the last times the PI_controller was called to calculate change in time
* past_encoder_heading - stores the previous encoder reading to check if the motor is still moving
* past_motor_direction - stores last motor direction to prevent immediate reversals
*/

*/

uint32_t current_time_stamp = HAL_GetTick(); // Returns number of milliseconds since startup
uint32_t current_time_stamp = HAL_GetTick(); // Returns number of milliseconds since startup

// Uncomment the below statements for debug print statements
// Uncomment the below statements for debug print statements to check current and desired heading

/* printf("CH: %li\r\n",current_heading);
printf("DH: %li\r\n",desired_heading);
/*
printf("CH: %li\r\n", current_heading);
printf("DH: %li\r\n", desired_heading);
*/

// Unsigned casting arithmetic handles if timer overflows - Divide by 1000 to convert from ms to s
float del_time = (current_time_stamp - *last_time_stamp)/1000.0f;


// Handle event if del_time = 0 - handle zero division error
if (del_time < 0.001f) {
del_time = 0.001f; // Minimum integration time step
}

int32_t error = desired_heading - current_heading;
printf("%li\r\n",error);
int8_t direction = (error > 0) - (error < 0); // Determine direction (-1, 0, or 1)
float Angular_Velocity = (current_heading-*past_encoder_heading)/del_time;
// Unsigned casting arithmetic handles if timer overflows - Divide by 1000 to convert from ms to s
float del_time = (current_time_stamp - *last_time_stamp) / 1000.0f;

// Handle event if del_time = 0 - handle zero division error
if (del_time < 0.001f) {
del_time = 0.001f; // Minimum integration time step
}

// Check if error is within the acceptable threshold
if (fabsf(error) < ERROR_THRESHOLD)
{
Set_Motor(0); // Stop the motor
*integral_error = 0.0f; // Reset the integral term
*last_time_stamp = HAL_GetTick(); // Update timestamp
*past_encoder_heading = current_heading; // Store last encoder value
// *past_motor_direction = 0; // Mark motor as stopped
//printf("No err.\r\n");
return;
}


// Ensure motor is fully stopped before allowing direction change
if ((Angular_Velocity > 0 && direction < 0) || (Angular_Velocity < 0 && direction > 0)){
Set_Motor(0);
*integral_error = 0.0f; // Reset the integral term
*last_time_stamp = HAL_GetTick(); // Update timestamp
*past_encoder_heading = current_heading; // Store last encoder value
return;
}

// Ensure that you cannot request an angle greater than the intended design
if (fabsf(desired_heading) > MAX_ANGLE) {
desired_heading = desired_heading < 0 ? -MAX_ANGLE : MAX_ANGLE;
}

// Routine for when error is greater than set threshold or if the direction has not been changed
// Calculate error
int16_t error = desired_heading - current_heading;

// Calculate angular velocity in deg/sec (or units/sec)
float angular_velocity = (current_heading - *past_encoder_heading) / del_time;

// Update the integral term
*integral_error += (float)error * (float)del_time;
*integral_error = fminf(fmaxf(*integral_error, -INTEGRAL_LIMIT), INTEGRAL_LIMIT); // Clamp the integral term
// Calculate current motor direction based on desired movement
int8_t new_motor_direction = (error > 0) - (error < 0);

// Compute the PI controller output
float motor_output = (PROPORTIONAL_GAIN * error) + (INTEGRAL_GAIN * (*integral_error));
// Check if error is within the acceptable threshold
if (fabsf(error) < ERROR_THRESHOLD) {
Set_Motor(0); // Stop the motor
*integral_error = 0.0f; // Reset the integral term
*last_time_stamp = current_time_stamp; // Update timestamp
*past_encoder_heading = current_heading; // Store latest encoder reading
return;
}

// If angular velocity and desired direction are opposite, stop motor - required for motor driver
if ((angular_velocity > 0 && direction < 0) || (angular_velocity < 0 && direction > 0)) {
Set_Motor(0); // Stop motor before reversing
*integral_error = 0.0f; // Reset integral to avoid wind-up
*last_time_stamp = current_time_stamp;
*past_encoder_heading = current_heading;
return;
}

// Update the integral term
*integral_error += (float)error * del_time;
*integral_error = fminf(fmaxf(*integral_error, -INTEGRAL_LIMIT), INTEGRAL_LIMIT); // Clamp the integral term

// Compute the PI controller output
float motor_output = (PROPORTIONAL_GAIN * error) + (INTEGRAL_GAIN * (*integral_error));


// Sets the motor to minimum power for movement if in dead zone - temporary value for the no-load motor case
// Use DAC_Step function to determine MIN_MOTOR
if (fabsf(motor_output) < MIN_MOTOR) {
motor_output = new_motor_direction * MIN_MOTOR;
}

// Sets the motor to minimum power for movement if in dead zone - temporary value for the no-load motor case
if (fabsf(motor_output) < 0.06){
motor_output = direction * 0.06;
}
motor_output = fminf(fmaxf(motor_output, -MAX_MOTOR), MAX_MOTOR); // Clamp the motor output

motor_output = fminf(fmaxf(motor_output, -MAX_MOTOR), MAX_MOTOR); // clamp the motor output if it's greater than 1.0
Set_Motor(motor_output); // Actuate the motor

// Uncomment the below statements for debug print statements
*last_time_stamp = current_time_stamp; // Update the timestamp for the next iteration
*past_encoder_heading = current_heading; // Store current encoder value to compute angular velocity next loop

// Uncomment the below statements for debug print statements - place where required
/*
printf("Dir: %li\r\n",direction);
printf("PG: %li\r\n",PROPORTIONAL_GAIN);
printf("IG: %li\r\n",INTEGRAL_GAIN);
printf("Err: %li\r\n",error);
printf("t: %d.%03d\r\n", (int)del_time, (int)(fabsf(del_time*1000)) % 1000);
printf("IE: %d.%03d\r\n", (int)*integral_error, (int)(fabsf(*integral_error * 1000)) % 1000);
printf("MO: %d.%03d\r\n", (int)motor_output, (int)(fabsf(motor_output * 1000)) % 1000);
printf("\r\n");

*/

printf("Dir: %li\r\n",direction);
printf("PG: %li\r\n",PROPORTIONAL_GAIN);
printf("IG: %li\r\n",INTEGRAL_GAIN);
printf("Err: %li\r\n",error);
printf("t: %d.%03d\r\n", (int)del_time, (int)(fabsf(del_time*1000)) % 1000);
printf("IE: %d.%03d\r\n", (int)*integral_error, (int)(fabsf(*integral_error * 1000)) % 1000);
printf("MO: %d.%03d\r\n", (int)motor_output, (int)(fabsf(motor_output * 1000)) % 1000);
printf("\r\n");

Set_Motor(motor_output); // Actuate the motor

*last_time_stamp = current_time_stamp; // Update the timestamp for the next iteration
*past_encoder_heading = current_heading; // Store the last encoder reading
*past_motor_direction = direction; // Store the last applied direction
*/

return;

}
17 changes: 10 additions & 7 deletions projects/drv-modules/motor-base-PID/RUDDERPID.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,18 +28,21 @@
#include "stm32u5xx_hal.h

// Constants
#define PROPORTIONAL_GAIN 0.0017f
#define INTEGRAL_GAIN 0.0000004f
#define ERROR_THRESHOLD 1.0f
#define INTEGRAL_LIMIT 15000
#define MOTOR_STOP 0
#define MAX_MOTOR 1.0f

#define PROPORTIONAL_GAIN 0.0017 // Proportional gain tuning parameter
#define INTEGRAL_GAIN 0.0000004 // Integral gain tuning parameter
#define ERROR_THRESHOLD 1.0f // Desired error threshold
#define INTEGRAL_LIMIT 15000 // Set for integral clamp to prevent integral error from growing too large
#define MOTOR_STOP 0 // Zero output from DAC
#define MAX_MOTOR 1.0f // Full output of the DAC
#define MAX_ANGLE 45.0f // Maximum/minimum range of motion of the rudder
#define MIN_MOTOR 0.65 // Sets the minimum DAC output to move the motor if needed

// Function prototypes
void DAC_STEP(int step);
void Set_Motor(float Motor_Control);
void PI_Motor(int32_t desired_heading, int32_t current_heading,
float* integral_error, uint32_t* last_time_stamp,
int32_t* past_encoder_heading, int8_t* past_motor_direction);
int32_t* past_encoder_heading);

#endif // MOTOR_PID_H