diff --git a/projects/drv-modules/motor-base-PID/RUDDERPID.c b/projects/drv-modules/motor-base-PID/RUDDERPID.c index f418061..ed50e3e 100644 --- a/projects/drv-modules/motor-base-PID/RUDDERPID.c +++ b/projects/drv-modules/motor-base-PID/RUDDERPID.c @@ -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); @@ -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; @@ -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; - } \ No newline at end of file diff --git a/projects/drv-modules/motor-base-PID/RUDDERPID.h b/projects/drv-modules/motor-base-PID/RUDDERPID.h index f8e56da..a074e3b 100644 --- a/projects/drv-modules/motor-base-PID/RUDDERPID.h +++ b/projects/drv-modules/motor-base-PID/RUDDERPID.h @@ -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