diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 07b6578..38a81ea 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -23,14 +23,16 @@ #define DIRECTION_LEFT (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_BACKWARD) #define DIRECTION_RIGHT (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_FORWARD) +#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD) + #define SPEED_PIN_RIGHT 15U #define SPEED_PIN_LEFT 16U -#define PWM_CLK_DIV 250.f -#define PWM_WRAP 5000U +#define PWM_CLK_DIV 50.f +#define PWM_WRAP 100U -#define MAX_SPEED 4900U -#define MIN_SPEED 0U // To be changed +#define MAX_PWM_LEVEL 99U +#define MIN_PWM_LEVEL 0U #define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) #define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 1UL) @@ -95,4 +97,6 @@ typedef struct volatile bool is_running; } distance_to_stop_t; +volatile bool g_use_pid = true; + #endif /* MOTOR_CONFIG_H */ \ No newline at end of file diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index e315635..6c31e29 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -5,6 +5,7 @@ */ #include "motor_init.h" +#include "magnetometer_init.h" /*! * @brief Set the direction of the wheels; can use bitwise OR to set both @@ -19,19 +20,29 @@ void set_wheel_direction(uint32_t direction) { - static const uint32_t mask - = DIRECTION_LEFT_FORWARD | DIRECTION_LEFT_BACKWARD - | DIRECTION_RIGHT_FORWARD | DIRECTION_RIGHT_BACKWARD; - - gpio_put_masked(mask, 0U); + gpio_put_masked(DIRECTION_MASK, 0U); gpio_set_mask(direction); } +/*! + * @brief Set the direction of the wheel to opposite direction using bit mask + */ +void +revert_wheel_direction() +{ + uint32_t current_direction = gpio_get_all(); + + uint32_t reverted_direction = current_direction ^ DIRECTION_MASK; + + gpio_put_masked(DIRECTION_MASK, 0U); + gpio_set_mask(reverted_direction & DIRECTION_MASK); +} + void turn_left_90() { set_wheel_direction(DIRECTION_RIGHT_FORWARD); - set_wheel_speed(3500u, &g_motor_right); + set_wheel_speed(90u, &g_motor_right); float initial = g_motor_right.speed.distance_cm; for (;;) @@ -52,7 +63,7 @@ void turn_right_90() { set_wheel_direction(DIRECTION_LEFT_FORWARD); - set_wheel_speed(3500u, &g_motor_left); + set_wheel_speed(90u, &g_motor_left); float initial = g_motor_left.speed.distance_cm; for (;;) @@ -74,7 +85,7 @@ spin_left_90() { set_wheel_direction(DIRECTION_LEFT); - set_wheel_speed_synced(3500u); + set_wheel_speed_synced(90u); float initial = g_motor_left.speed.distance_cm; for (;;) @@ -96,7 +107,7 @@ spin_right_90() { set_wheel_direction(DIRECTION_RIGHT); - set_wheel_speed_synced(3500u); + set_wheel_speed_synced(90u); float initial = g_motor_right.speed.distance_cm; for (;;) @@ -111,4 +122,38 @@ spin_right_90() vTaskDelay(pdMS_TO_TICKS(1000)); g_motor_right.speed.distance_cm = initial; g_motor_left.speed.distance_cm = initial; +} + +void +spin_to_yaw(float target_yaw) +{ + float initial_yaw = g_direction.yaw; + + // if it will to turn more than 180 degrees, turn the other way + if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f) + || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f))) + { + set_wheel_direction(DIRECTION_LEFT); + } + else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f) + || ((target_yaw < initial_yaw) + && (initial_yaw - target_yaw < 180.f))) + { + set_wheel_direction(DIRECTION_RIGHT); + } + + set_wheel_speed_synced(90u); + + g_use_pid = false; + + for (;;) + { + if (initial_yaw == target_yaw) + { + set_wheel_speed_synced(0u); + break; + } + vTaskDelay(pdMS_TO_TICKS(5)); + } + g_use_pid = true; } \ No newline at end of file diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index 56cb390..7ac9b70 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -23,12 +23,17 @@ motor_t g_motor_left = { .pwm.level = 0u, .pwm.channel = PWM_CHAN_A, .speed.distance_cm = 0.0f }; +// classic ziegler nichols method +// Ku = 1000, Tu = 0.9s, interval = 0.05s +// Kp = 0.6 * Ku = 600 +// Ki = 2 * Kp * 0.05 / Tu = 66.67 +// Kd = Kp * Tu * 0.05 / 8 = 1350 motor_t g_motor_right = { .pwm.level = 0u, .pwm.channel = PWM_CHAN_B, .speed.distance_cm = 0.0f, - .pid.kp_value = 1000.f, - .pid.ki_value = 0.0f, - .pid.kd_value = 10000.0f,}; + .pid.kp_value = 600.f, + .pid.ki_value = 66.67f, + .pid.kd_value = 1350.f,}; void motor_init(void) @@ -62,11 +67,12 @@ motor_init(void) // NOTE: PWM clock is 125MHz for raspberrypi pico w by default - // 125MHz / 250 = 500kHz + // 125MHz / 50 = 2500kHz pwm_set_clkdiv(g_motor_left.pwm.slice_num, PWM_CLK_DIV); pwm_set_clkdiv(g_motor_right.pwm.slice_num, PWM_CLK_DIV); - // have them to be 500kHz / 5000 = 100Hz + // L289N can accept up to 40kHz + // 2500kHz / 100 = 25kHz pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U)); pwm_set_wrap(g_motor_right.pwm.slice_num, (PWM_WRAP - 1U)); diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index 16283f1..1930ed6 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -21,7 +21,7 @@ compute_pid(float *integral, float *prev_error) float error = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; - printf("%f,\n", error); + printf("%f\n", error); *integral += error; @@ -42,7 +42,7 @@ repeating_pid_handler(__unused struct repeating_timer *t) static float integral = 0.0f; static float prev_error = 0.0f; - if ((g_motor_left.pwm.level == 0u) || (g_motor_right.pwm.level == 0u)) + if (!g_use_pid) { return true; } @@ -51,14 +51,14 @@ repeating_pid_handler(__unused struct repeating_timer *t) float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f; - if (temp > MAX_SPEED) + if (temp > MAX_PWM_LEVEL) { - temp = MAX_SPEED; + temp = MAX_PWM_LEVEL; } - if (temp < MIN_SPEED) + if (temp <= MIN_PWM_LEVEL) { - temp = MIN_SPEED + 1u; + temp = MIN_PWM_LEVEL + 1u; } g_motor_right.pwm.level = (uint16_t)temp; @@ -66,8 +66,8 @@ repeating_pid_handler(__unused struct repeating_timer *t) g_motor_right.pwm.channel, g_motor_right.pwm.level); - printf("speed: %f cm/s\n", g_motor_right.speed.current_cms); - printf("distance: %f cm\n", g_motor_right.speed.distance_cm); +// printf("speed: %f cm/s\n", g_motor_right.speed.current_cms); +// printf("distance: %f cm\n", g_motor_right.speed.distance_cm); return true; } \ No newline at end of file diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 4848f83..9b556ec 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -51,8 +51,8 @@ monitor_wheel_speed_task(void *pvParameters) for (;;) { - if (xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100)) - == pdTRUE) + if ((xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100)) + == pdTRUE) && (g_use_pid == true)) { curr_time = time_us_64(); elapsed_time = curr_time - prev_time; @@ -86,11 +86,16 @@ set_wheel_speed(uint32_t pwm_level, motor_t * motor) /*! * @brief Set the speed of the wheels - * @param pwm_level The pwm_level of the wheels, from 0 to 5000 + * @param pwm_level The pwm_level of the wheels, from 0 to 99 */ void set_wheel_speed_synced(uint32_t pwm_level) { + if (pwm_level > MAX_PWM_LEVEL) + { + pwm_level = MAX_PWM_LEVEL; + } + set_wheel_speed(pwm_level, &g_motor_left); set_wheel_speed(pwm_level, &g_motor_right); } diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index b7dbe1f..8899ebb 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -4,6 +4,22 @@ #include "motor_direction.h" #include "motor_pid.h" +void +motor_control_task(__unused void *params) +{ + for (;;) + { + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(90); + + vTaskDelay(pdMS_TO_TICKS(10000)); + + revert_wheel_direction(); + set_wheel_speed_synced(90); + + vTaskDelay(pdMS_TO_TICKS(10000)); + } +} void launch()