diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index 6cbc5aa..d92c31f 100644 --- a/frtos/config/magnetometer_config.h +++ b/frtos/config/magnetometer_config.h @@ -8,7 +8,7 @@ #define DIRECTION_READ_DELAY (200) #define NUM_READINGS \ - (10) // Number of readings to + (5) // Number of readings to // take before // calculating // direction diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index 1b7de60..f817334 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -316,7 +316,7 @@ updateDirection(volatile direction_t * g_direction) read_direction(accelerometer, magnetometer, g_direction); -// print_orientation_data(*g_direction); + print_orientation_data(*g_direction); // // switch (g_direction->orientation) // { diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 8d830e5..3cb2e65 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -10,6 +10,7 @@ #include "motor_init.h" #include "magnetometer_init.h" #include "magnetometer_direction.h" +#include "motor_speed.h" #include "math.h" /*! @@ -48,21 +49,18 @@ revert_wheel_direction() * @param range acceptable range * @return true if the current direction is within the range of the target */ -bool -check_direction(float current_direction, float target_direction, float range) +bool check_direction(float current_direction, float target_direction, float range) { // Normalize directions to be within 0 to 360 degrees current_direction = fmod(current_direction, 360.0f); - if (current_direction < 0) - current_direction += 360.0f; + current_direction += (current_direction < 0) ? 360.0f : 0.0f; target_direction = fmod(target_direction, 360.0f); - if (target_direction < 0) - target_direction += 360.0f; + target_direction += (target_direction < 0) ? 360.0f : 0.0f; - // Check if current_direction is within ±1 degree of target_direction - if (fabs(current_direction - target_direction) <= range - || fabs(current_direction - target_direction) >= (360 - range)) + // Check if current_direction is within ±range degrees of target_direction + float difference = fabs(current_direction - target_direction); + if (difference <= range || difference >= (360.0f - range)) { return true; } @@ -91,6 +89,7 @@ turn_to_yaw(uint32_t direction, for (;;) { updateDirection(pp_car_struct->p_direction); + print_orientation_data(*pp_car_struct->p_direction); if (check_direction(pp_car_struct->p_direction->yaw, target_yaw, 1)) { set_wheel_direction(DIRECTION_MASK); diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index ba33e67..d36b630 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -4,15 +4,6 @@ #include "car_config.h" #include "motor_init.h" -bool -check_collision(void *params) -{ - car_struct_t *car_struct = (car_struct_t *)params; - return ((car_struct->obs->left_sensor_detected << 1) - | (car_struct->obs->right_sensor_detected)) - || car_struct->obs->ultrasonic_detected; -} - /*! * @brief Check if the car is on the line * @param params @@ -27,6 +18,17 @@ check_line_touch(void *params) | (car_struct->obs->right_sensor_detected); } +bool +check_collision(void *params) +{ + car_struct_t *car_struct = (car_struct_t *)params; + // return ((car_struct->obs->left_sensor_detected << 1) + // | (car_struct->obs->right_sensor_detected)) + // || car_struct->obs->ultrasonic_detected; + return check_line_touch(car_struct) || + car_struct->obs->ultrasonic_detected; +} + void motor_control_task(void *params) { @@ -45,12 +47,12 @@ motor_control_task(void *params) break; case 0b01: car_struct->p_right_motor->speed.current_cms - += SLOT_DISTANCE_CM*1000.f; - break; + += SLOT_DISTANCE_CM * 1000.f; + break; case 0b10: car_struct->p_right_motor->speed.current_cms - -= SLOT_DISTANCE_CM*1000.f; - break; + -= SLOT_DISTANCE_CM * 1000.f; + break; case 0b11: // set_wheel_direction(DIRECTION_MASK); // set_wheel_speed_synced(0u, car_struct); @@ -85,6 +87,26 @@ obs_task(void *params) } } +void +turn_task(void *params) +{ + car_struct_t *car_struct = (car_struct_t *)params; + + for (;;) + { + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(90u, car_struct); + + distance_to_stop(car_struct, 50.f); + vTaskDelay(pdMS_TO_TICKS(1000)); + +// turn_to_yaw(DIRECTION_LEFT, 230.f, 80u, car_struct); + + turn(DIRECTION_RIGHT, 50.f, 90u, car_struct); + vTaskDelay(pdMS_TO_TICKS(1000)); + } +} + void h_main_irq_handler(void) { @@ -126,6 +148,14 @@ main(void) .obs = &obs, .p_direction = &direction }; + // Magnetometer + magnetometer_init(&car_struct); +// magnetometer_tasks_init(&car_struct); + // updateDirection(car_struct.p_direction); + printf("Magnetometer initialized!\n"); + +// sleep_ms(1000); + // ultra ultrasonic_init(&car_struct); ultrasonic_task_init(&car_struct); @@ -141,12 +171,6 @@ main(void) motor_tasks_init(&car_struct, &h_main_irq_handler); printf("Motor initialized!\n"); - // Magnetometer - magnetometer_init(&car_struct); - magnetometer_tasks_init(&car_struct); - updateDirection(car_struct.p_direction); - printf("Magnetometer initialized!\n"); - sleep_ms(1000u); // control task @@ -158,14 +182,23 @@ main(void) // PRIO, // &h_motor_turning_task_handle); - // obs task - TaskHandle_t h_obs_task_handle = NULL; - xTaskCreate(obs_task, - "obs_task", - configMINIMAL_STACK_SIZE, - (void *)&car_struct, - PRIO, - &h_obs_task_handle); + // obs task + TaskHandle_t h_obs_task_handle = NULL; + xTaskCreate(obs_task, + "obs_task", + configMINIMAL_STACK_SIZE, + (void *)&car_struct, + 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); // PID timer struct repeating_timer pid_timer;