From 7da6295adc2147244ae7dfaa9ff71f2e5464a210 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Tue, 28 Nov 2023 13:24:33 +0800 Subject: [PATCH] test++ --- frtos/config/line_sensor_config.h | 4 +-- frtos/config/motor_config.h | 8 ++--- frtos/motor/motor_init.h | 6 ++-- frtos/motor/motor_pid.h | 8 ++--- frtos/rtos_car.c | 52 ++++++++++++++++++------------- 5 files changed, 43 insertions(+), 35 deletions(-) diff --git a/frtos/config/line_sensor_config.h b/frtos/config/line_sensor_config.h index 007cc77..5483e78 100644 --- a/frtos/config/line_sensor_config.h +++ b/frtos/config/line_sensor_config.h @@ -5,7 +5,7 @@ #define LINE_SENSOR_READ_DELAY ( 100 ) -#define LEFT_SENSOR_PIN ( 26 ) -#define RIGHT_SENSOR_PIN ( 27 ) +#define LEFT_SENSOR_PIN ( 21 ) //26 +#define RIGHT_SENSOR_PIN ( 20 ) //27 #endif //CONFIG_H diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 4808885..ad0303d 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -9,11 +9,11 @@ #define SLICE_NUM 0U // IN1, IN2, IN3, IN4 on the L298N -#define DIRECTION_PIN_RIGHT_IN1 11U -#define DIRECTION_PIN_RIGHT_IN2 12U +#define DIRECTION_PIN_RIGHT_IN1 3U //11 +#define DIRECTION_PIN_RIGHT_IN2 2U //12 -#define DIRECTION_PIN_LEFT_IN3 19U -#define DIRECTION_PIN_LEFT_IN4 20U +#define DIRECTION_PIN_LEFT_IN3 7U //19 +#define DIRECTION_PIN_LEFT_IN4 6U //20 // to turn one side #define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2) diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index ce21da1..19cb78c 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -38,9 +38,9 @@ motor_init(car_struct_t *car_struct) g_right_sem = xSemaphoreCreateBinary(); car_struct->p_pid->use_pid = true; - car_struct->p_pid->kp_value = 600.f; - car_struct->p_pid->ki_value = 66.67f; - car_struct->p_pid->kd_value = 1350.f; + car_struct->p_pid->kp_value = 12.f; //25 + car_struct->p_pid->ki_value = 0.f; + car_struct->p_pid->kd_value = 30.f; // initialize the car_struct car_struct->p_left_motor->pwm.level = 0u; diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index a54285d..b9a560d 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -67,16 +67,16 @@ repeating_pid_handler(struct repeating_timer *ppp_timer) if (temp > MAX_PWM_LEVEL) { - temp = MAX_PWM_LEVEL; + temp = 70.0f; } if (temp <= MIN_PWM_LEVEL) { - temp = MIN_PWM_LEVEL; + temp = 60.0f; } - - // set_wheel_speed((uint32_t)temp, car_strut->p_right_motor); set_wheel_speed((uint32_t)temp, car_strut->p_left_motor); + printf("right speed: %f\n", car_strut->p_right_motor->speed.current_cms); + printf("left speed: %f\n", car_strut->p_left_motor->speed.current_cms); return true; } diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index fd8a0ab..4d6a092 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -34,7 +34,7 @@ motor_control_task(void *params) { car_struct_t *car_struct = (car_struct_t *)params; set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(90u, car_struct); + set_wheel_speed_synced(62u, car_struct); for (;;) { @@ -44,17 +44,25 @@ motor_control_task(void *params) default: break; case 0b01: - car_struct->p_right_motor->speed.distance_cm - -= SLOT_DISTANCE_CM; +// car_struct->p_right_motor->speed.distance_cm +// -= SLOT_DISTANCE_CM; + set_wheel_direction(DIRECTION_LEFT_BACKWARD); + distance_to_stop(car_struct, SLOT_DISTANCE_CM); + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(62u, car_struct); break; case 0b10: - car_struct->p_right_motor->speed.distance_cm - += SLOT_DISTANCE_CM; +// car_struct->p_right_motor->speed.distance_cm +// += SLOT_DISTANCE_CM; + set_wheel_direction(DIRECTION_RIGHT_BACKWARD); + distance_to_stop(car_struct, SLOT_DISTANCE_CM); + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(62u, car_struct); break; case 0b11: - turn(DIRECTION_LEFT, 90u, 90u, car_struct); + turn(DIRECTION_LEFT, 90u, 62u, car_struct); set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(90u, car_struct); + set_wheel_speed_synced(62u, car_struct); break; } @@ -172,13 +180,13 @@ main(void) sleep_ms(1000u); // control task -// TaskHandle_t h_motor_turning_task_handle = NULL; -// xTaskCreate(motor_control_task, -// "motor_turning_task", -// configMINIMAL_STACK_SIZE, -// (void *)&car_struct, -// PRIO, -// &h_motor_turning_task_handle); + TaskHandle_t h_motor_turning_task_handle = NULL; + xTaskCreate(motor_control_task, + "motor_turning_task", + configMINIMAL_STACK_SIZE, + (void *)&car_struct, + PRIO, + &h_motor_turning_task_handle); // obs task // TaskHandle_t h_obs_task_handle = NULL; @@ -189,14 +197,14 @@ main(void) // PRIO, // &h_obs_task_handle); - // turn task - TaskHandle_t h_turn_task_handle = NULL; - xTaskCreate(turn_task, - "turn_task", - configMINIMAL_STACK_SIZE, - (void *)&car_struct, - PRIO, - &h_turn_task_handle); +// // turn task +// TaskHandle_t h_turn_task_handle = NULL; +// xTaskCreate(turn_task, +// "turn_task", +// configMINIMAL_STACK_SIZE, +// (void *)&car_struct, +// PRIO, +// &h_turn_task_handle); // PID timer struct repeating_timer pid_timer;