diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index c4e5c20..f094e46 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -77,23 +77,23 @@ motor_init(car_struct_t *car_struct) gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM); car_struct->p_left_motor->pwm.slice_num - = pwm_gpio_to_slice_num(PWM_PIN_LEFT); + = pwm_gpio_to_slice_num(0); car_struct->p_right_motor->pwm.slice_num - = pwm_gpio_to_slice_num(PWM_PIN_RIGHT); + = pwm_gpio_to_slice_num(0); // NOTE: PWM clock is 125MHz for raspberrypi pico w by default // 125MHz / 50 = 2500kHz pwm_set_clkdiv(car_struct->p_left_motor->pwm.slice_num, PWM_CLK_DIV); - pwm_set_clkdiv(car_struct->p_right_motor->pwm.slice_num, PWM_CLK_DIV); +// pwm_set_clkdiv(car_struct->p_right_motor->pwm.slice_num, PWM_CLK_DIV); // L289N can accept up to 40kHz // 2500kHz / 100 = 25kHz pwm_set_wrap(car_struct->p_left_motor->pwm.slice_num, (PWM_WRAP - 1U)); - pwm_set_wrap(car_struct->p_right_motor->pwm.slice_num, (PWM_WRAP - 1U)); +// pwm_set_wrap(car_struct->p_right_motor->pwm.slice_num, (PWM_WRAP - 1U)); pwm_set_enabled(car_struct->p_left_motor->pwm.slice_num, true); - pwm_set_enabled(car_struct->p_right_motor->pwm.slice_num, true); +// pwm_set_enabled(car_struct->p_right_motor->pwm.slice_num, true); } /*! diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index b5182e4..21c6d22 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -8,14 +8,16 @@ motor_control_task(void *params) for (;;) { set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(90u, car_struct); + set_wheel_speed_synced(99u, car_struct); + vTaskDelay(pdMS_TO_TICKS(1000)); + set_wheel_speed_synced(50u, car_struct); - vTaskDelay(pdMS_TO_TICKS(10000)); + vTaskDelay(pdMS_TO_TICKS(5000)); revert_wheel_direction(); set_wheel_speed_synced(90u, car_struct); - vTaskDelay(pdMS_TO_TICKS(10000)); + vTaskDelay(pdMS_TO_TICKS(5000)); } } diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index fd8a0ab..8c19622 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -33,30 +33,37 @@ void motor_control_task(void *params) { car_struct_t *car_struct = (car_struct_t *)params; + car_struct->p_pid->use_pid = false; set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(90u, car_struct); + // set_wheel_speed_synced(50u, car_struct); + pwm_set_chan_level(car_struct->p_left_motor->pwm.slice_num, + car_struct->p_left_motor->pwm.channel, + 50u); +// pwm_set_chan_level(car_struct->p_right_motor->pwm.slice_num, +// car_struct->p_right_motor->pwm.channel, +// 50u); for (;;) { - uint8_t temp = check_line_touch(car_struct); - switch (temp) - { - default: - break; - case 0b01: - car_struct->p_right_motor->speed.distance_cm - -= SLOT_DISTANCE_CM; - break; - case 0b10: - car_struct->p_right_motor->speed.distance_cm - += SLOT_DISTANCE_CM; - break; - case 0b11: - turn(DIRECTION_LEFT, 90u, 90u, car_struct); - set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(90u, car_struct); - break; - } +// uint8_t temp = check_line_touch(car_struct); +// switch (temp) +// { +// default: +// break; +// case 0b01: +// car_struct->p_right_motor->speed.distance_cm +// -= SLOT_DISTANCE_CM; +// break; +// case 0b10: +// car_struct->p_right_motor->speed.distance_cm +// += SLOT_DISTANCE_CM; +// break; +// case 0b11: +// turn(DIRECTION_LEFT, 90u, 90u, car_struct); +// set_wheel_direction(DIRECTION_FORWARD); +// set_wheel_speed_synced(90u, car_struct); +// break; +// } vTaskDelay(pdMS_TO_TICKS(25)); } @@ -172,13 +179,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; @@ -190,13 +197,13 @@ main(void) // &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); +// 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;