181 lines
5.3 KiB
C
181 lines
5.3 KiB
C
/**
|
|
* @file line_sensor.h
|
|
* @brief Monitor the line sensor and update the car state accordingly
|
|
* @author Woon Jun Wei
|
|
*/
|
|
|
|
#include "line_sensor_init.h"
|
|
|
|
/**
|
|
* @brief Monitor the left sensor
|
|
*
|
|
* This function will monitor the left sensor and send the state to the
|
|
* left sensor message buffer, used to calculate the direction of the car
|
|
*
|
|
* @param params
|
|
*/
|
|
//void
|
|
//monitor_left_sensor_task(__unused void *params) {
|
|
// for (;;)
|
|
// {
|
|
// if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
|
|
// {
|
|
// if (left_sensor_triggered == pdTRUE)
|
|
// {
|
|
// printf("left sensor triggered\n");
|
|
// // Get Current State
|
|
// state_t state = gpio_get(LEFT_SENSOR_PIN);
|
|
//
|
|
// xMessageBufferSend(left_sensor_msg_buffer,
|
|
// &state,
|
|
// sizeof(state_t),
|
|
// 0);
|
|
// // Reset the flag
|
|
// left_sensor_triggered = pdFALSE;
|
|
// }
|
|
// }
|
|
// }
|
|
//}
|
|
|
|
void
|
|
monitor_left_sensor_task(__unused void *params) {
|
|
for (;;)
|
|
{
|
|
if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
|
|
{
|
|
printf("left sensor triggered\n");
|
|
// Get Current State
|
|
state_t state = gpio_get(LEFT_SENSOR_PIN);
|
|
|
|
xMessageBufferSend(left_sensor_msg_buffer,
|
|
&state,
|
|
sizeof(state_t),
|
|
0);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Monitor the right sensor
|
|
*
|
|
* This function will monitor the right sensor and send the state to the
|
|
* right sensor message buffer, used to calculate the direction of the car
|
|
*
|
|
* @param params
|
|
*/
|
|
//void
|
|
//monitor_right_sensor_task(void *params) {
|
|
// for (;;) {
|
|
// if (xSemaphoreTake(g_right_sensor_sem, portMAX_DELAY) == pdTRUE) {
|
|
// // Check the flag or receive the message
|
|
// if (right_sensor_triggered == pdTRUE) {
|
|
// printf("right sensor triggered\n");
|
|
// // Get Current State
|
|
// state_t state = gpio_get(RIGHT_SENSOR_PIN);
|
|
//
|
|
// xMessageBufferSend(right_sensor_msg_buffer,
|
|
// &state,
|
|
// sizeof(state_t),
|
|
// 0);
|
|
// // Reset the flag
|
|
// right_sensor_triggered = pdFALSE;
|
|
// }
|
|
// }
|
|
// }
|
|
//}
|
|
|
|
void
|
|
monitor_right_sensor_task(void *params) {
|
|
for (;;) {
|
|
if (xSemaphoreTake(g_right_sensor_sem, portMAX_DELAY) == pdTRUE) {
|
|
// Check the flag or receive the message
|
|
printf("right sensor triggered\n");
|
|
// Get Current State
|
|
state_t state = gpio_get(RIGHT_SENSOR_PIN);
|
|
|
|
xMessageBufferSend(right_sensor_msg_buffer,
|
|
&state,
|
|
sizeof(state_t),
|
|
0);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Monitor the direction and Oritentation of the car
|
|
*
|
|
* This function will monitor the direction and orientation of the car
|
|
* and update the car state accordingly
|
|
*
|
|
* @param params
|
|
*/
|
|
void
|
|
monitor_direction_task(__unused void *params) {
|
|
state_t left_state;
|
|
state_t right_state;
|
|
|
|
for (;;)
|
|
{
|
|
// Receive from Buffer
|
|
xMessageBufferReceive(left_sensor_msg_buffer,
|
|
&left_state,
|
|
sizeof(state_t),
|
|
portMAX_DELAY);
|
|
|
|
xMessageBufferReceive(right_sensor_msg_buffer,
|
|
&right_state,
|
|
sizeof(state_t),
|
|
portMAX_DELAY);
|
|
|
|
// g_car_state.current_direction = (left_state << 1) | right_state;
|
|
|
|
// switch (g_car_state.current_direction)
|
|
// {
|
|
// case FORWARD:
|
|
// break;
|
|
// case RIGHT:
|
|
// g_car_state.orientation = (g_car_state.orientation + 1) & 0x03;
|
|
// break;
|
|
// case LEFT:
|
|
// g_car_state.orientation = (g_car_state.orientation - 1) & 0x03;
|
|
// break;
|
|
// default:
|
|
// break;
|
|
// }
|
|
//
|
|
// switch (g_car_state.current_direction)
|
|
// {
|
|
// case FORWARD:
|
|
// printf("Direction: Forward\n");
|
|
// break;
|
|
// case RIGHT:
|
|
// printf("Direction: Right\n");
|
|
// break;
|
|
// case LEFT:
|
|
// printf("Direction: Left\n");
|
|
// break;
|
|
// default:
|
|
// printf("Direction: Error\n");
|
|
// break;
|
|
// }
|
|
//
|
|
// switch (g_car_state.orientation)
|
|
// {
|
|
// case NORTH:
|
|
// printf("Orientation: North\n");
|
|
// break;
|
|
// case EAST:
|
|
// printf("Orientation: East\n");
|
|
// break;
|
|
// case SOUTH:
|
|
// printf("Orientation: South\n");
|
|
// break;
|
|
// case WEST:
|
|
// printf("Orientation: West\n");
|
|
// break;
|
|
// default:
|
|
// printf("Orientation: Error\n");
|
|
// break;
|
|
// }
|
|
}
|
|
} |