Mapping Functions (Untested)

frtos/config/car_config.h

@@ -15,6 +15,17 @@ typedef struct s_obs_struct
 
 } obs_t;
 
+// Define the Map structure
+typedef struct {
+    bool **data;  // 2D array to represent the grid
+    int rows;     // Number of rows in the grid
+    int cols;     // Number of columns in the grid
+    int initial_x;
+    int initial_y;
+    float * distance_array;
+    int distance_array_size;
+} grid_t;
+
 typedef struct
 {
     obs_t       *obs;
@@ -22,6 +33,7 @@ typedef struct
     motor_t     *p_right_motor;
     motor_pid_t *p_pid;
     direction_t *p_direction;
+    grid_t      *p_grid;
 
 } car_struct_t;
 

frtos/config/magnetometer_config.h

@@ -7,14 +7,12 @@
 
 #define DIRECTION_READ_DELAY (200)
 
-#define NUM_READINGS                            ( 10 ) // Number of readings to
+#define NUM_READINGS \
+    (10) // Number of readings to
          // take before
          // calculating
          // direction
 
-// #define ALPHA                                   ( 0.1f ) // Low Pass Filter
-//  Coefficient
-
 // LSM303DLHC temperature compensation coefficients
 #define SCALE_Z  (1.0f) // Scale for Z-axis
 #define OFFSET_Z (0.0f) // Offset for Z-axis
@@ -50,7 +48,9 @@ typedef enum
  */
 typedef enum
 {
+    FORWARD  = 0,
     UP       = 0,
+    BACKWARD = 1,
     DOWN     = 1,
     LEFT     = 2,
     RIGHT    = 3

frtos/magnetometer/magnetometer_direction.h

@@ -80,7 +80,8 @@ calculate_yaw_magnetometer(int16_t magnetometer[3])
  * @return              Compensated Yaw
  */
 float
-compensate_magnetometer(float yaw_mag, int16_t temperature) {
+compensate_magnetometer(float yaw_mag, int16_t temperature)
+{
     // Calculate temperature difference from the reference temperature
     uint delta_temp = temperature - TEMPERATURE_OFFSET;
 
@@ -301,7 +302,7 @@ print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle)
 }
 
 void
-updateDirection(volatile direction_t * g_direction)
+updateDirection(volatile direction_t *g_direction)
 {
     int16_t magnetometer[3];
     int16_t accelerometer[3];
@@ -317,62 +318,13 @@ updateDirection(volatile direction_t * g_direction)
     read_direction(accelerometer, magnetometer, g_direction);
 
     print_orientation_data(*g_direction);
-
-    // Temperature in degrees Celsius
-    //    printf("Temperature: %d\n", temperature[0]);
-
-    //    print_orientation_data();
-
-    //    printf("Direction: ");
-
-    //    print_direction(g_direction.orientation);
-
-    switch (g_direction->orientation)
-    {
-        case NORTH:
-            cur_y++;
-            break;
-        case EAST:
-            cur_x++;
-            break;
-        case SOUTH:
-            cur_y--;
-            break;
-        case WEST:
-            cur_x--;
-            break;
-        case NORTH_EAST:
-            cur_x++;
-            cur_y++;
-            break;
-        case SOUTH_EAST:
-            cur_x++;
-            cur_y--;
-            break;
-        case SOUTH_WEST:
-            cur_x--;
-            cur_y--;
-            break;
-        case NORTH_WEST:
-            cur_x--;
-            cur_y++;
-            break;
-    }
-
-    // Update the map based on the direction of the car (N, E, S, W)
-    //    update_map(g_direction.orientation, cur_x, cur_y);
-
-    //    printf("Current Position: (%d, %d)\n", cur_x, cur_y);
-    //    print_map();
-
-    //    print_roll_and_pitch(g_direction.roll_angle, g_direction.pitch_angle);
 }
 
 void
 monitor_direction_task(void *pvParameters)
 {
     volatile direction_t *p_direction = NULL;
-    p_direction = (direction_t *) pvParameters;
+    p_direction                       = (direction_t *)pvParameters;
 
     for (;;)
     {
@@ -388,7 +340,7 @@ magnetometer_tasks_init(car_struct_t *car_struct)
     xTaskCreate(monitor_direction_task,
                 "Direction Task",
                 configMINIMAL_STACK_SIZE,
-                (void *) car_struct->p_direction,
+                (void *)car_struct->p_direction,
                 PRIO,
                 &h_direction_task);
 }

frtos/magnetometer/map.h

@@ -9,120 +9,116 @@
 #ifndef TEST_PROJECT_MAP_H
 #define TEST_PROJECT_MAP_H
 
-
-// Define the grid structure
-typedef struct {
-    bool **data;  // 2D array to represent the grid
-    int rows;     // Number of rows in the grid
-    int cols;     // Number of columns in the grid
-} Grid;
-
-// Global grid to track the car's path
-Grid *car_path_grid;
-
 // Function to create and initialize a grid
-Grid *create_grid(int rows, int cols) {
+void
-    Grid *grid = (Grid *) malloc(sizeof(Grid));
+map_init(int rows, int cols, grid_t *p_grid, int initial_x, int initial_y)
-    grid->rows = rows;
+{
-    grid->cols = cols;
+    p_grid->rows      = rows;
+    p_grid->cols      = cols;
+    p_grid->initial_x = initial_x;
+    p_grid->initial_y = initial_y;
 
-    // Allocate memory for the 2D array
+    // Allocate memory for the grid
-    grid->data = (bool **) malloc(rows * sizeof(bool *));
+    p_grid->data = (bool **)malloc(rows * sizeof(bool *));
-    for (int i = 0; i < rows; i++) {
+    for (int i = 0; i < rows; i++)
-        grid->data[i] = (bool *) malloc(cols * sizeof(bool));
+    {
-        for (int j = 0; j < cols; j++) {
+        p_grid->data[i] = (bool *)malloc(cols * sizeof(bool));
-            grid->data[i][j] = false;  // Initialize to 'false' (unvisited)
+        for (int j = 0; j < cols; j++)
+        {
+            p_grid->data[i][j] = false; // Initialize to 'false' (unvisited)
         }
     }
-
-    return grid;
 }
 
 // Function to mark a cell as visited
-void mark_cell(Grid *grid, int row, int col) {
+void
-    if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) {
+mark_cell(grid_t *grid, int row, int col)
+{
+    if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols)
+    {
         grid->data[row][col] = true;
     }
 }
 
 // Function to check if a cell has been visited
-bool is_cell_visited(Grid *grid, int row, int col) {
+bool
-    if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) {
+is_cell_visited(grid_t *grid, int row, int col)
+{
+    if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols)
+    {
         return grid->data[row][col];
     }
     return false; // Consider out-of-bounds as unvisited
 }
 
 // Function to destroy the grid and free memory
-void destroy_grid(Grid *grid) {
+void
-    for (int i = 0; i < grid->rows; i++) {
+destroy_grid(grid_t *grid)
+{
+    for (int i = 0; i < grid->rows; i++)
+    {
         free(grid->data[i]);
     }
     free(grid->data);
     free(grid);
 }
 
-// Function to update the map based on car's current orientation
+// Function to update the map based on the car's current orientation and
-// Function to update the map based on car's current orientation and position
+// position
-void update_map(int orientation, int cur_x, int cur_y) {
+void
-    // Define offsets for different orientations
+update_map(grid_t *car_path_grid, uint32_t direction, float distance)
+{
+    int cur_x = car_path_grid->initial_x;
+    int cur_y = car_path_grid->initial_y;
+
+    // Define offsets for different forward and turn directions
     int offset_x = 0;
     int offset_y = 0;
 
-    switch (orientation) {
+    // Update the current position based on the forward direction
-        case NORTH:
+    switch (direction)
-            offset_y = 1;
+    {
-            break;
+        case DIRECTION_LEFT:
-        case EAST:
-            offset_x = 1;
-            break;
-        case SOUTH:
-            offset_y = -1;
-            break;
-        case WEST:
             offset_x = -1;
             break;
-        case NORTH_EAST:
+        case DIRECTION_RIGHT:
             offset_x = 1;
+            break;
+        case DIRECTION_FORWARD:
             offset_y = 1;
             break;
-        case SOUTH_EAST:
+        case DIRECTION_BACKWARD:
-            offset_x = 1;
             offset_y = -1;
             break;
-        case SOUTH_WEST:
-            offset_x = -1;
-            offset_y = -1;
-            break;
-        case NORTH_WEST:
-            offset_x = -1;
-            offset_y = 1;
-            break;
     }
 
-    // Update the map based on the car's current position and orientation
+    cur_x += offset_x;
+    cur_y += offset_y;
+
+    // Mark the current and next position as visited
     mark_cell(car_path_grid, cur_x, cur_y);
-    mark_cell(car_path_grid, cur_x + offset_x, cur_y + offset_y);
+    car_path_grid->initial_x = cur_x;
+    car_path_grid->initial_y = cur_y;
+    car_path_grid->distance_array[car_path_grid->distance_array_size]
+        = distance;
+    car_path_grid->distance_array_size++;
+
+
 }
 
-
 // Function to print the map
-void print_map() {
+void
+print_map(grid_t *car_path_grid)
+{
     // Invert the map, 0,0 is at the Middle
     // Print 1 for visited cells and 0 for unvisited cells
-    for (int i = car_path_grid->rows - 1; i >= 0; i--) {
+    for (int i = car_path_grid->rows - 1; i >= 0; i--)
-        for (int j = 0; j < car_path_grid->cols; j++) {
+    {
+        for (int j = 0; j < car_path_grid->cols; j++)
+        {
             (car_path_grid->data[j][i]) ? printf("1 ") : printf("0 ");
-//                case false:
-//                    printf("0 ");
-//                    break;
-//                case true:
-//                    printf("1 ");
-//                    break;
-//            }
         }
         printf("\n");
     }
 }
-
+#endif // TEST_PROJECT_MAP_H
-#endif //TEST_PROJECT_MAP_H

frtos/motor/motor_direction.h

@@ -70,7 +70,7 @@ check_direction(float current_direction, float target_direction, float range)
 }
 
 /*!
- * @brief Spin the car to a certain yaw specifically
+ * @brief Spin the car to a certain yaw specifically, update the map after
  * @param direction The direction to turn or spin
  * @param target_yaw The target yaw to spin to
  * @param pwm_level The pwm_level of the wheels, from 0 to 99
@@ -84,6 +84,8 @@ turn_to_yaw(uint32_t      direction,
 {
     pp_car_struct->p_pid->use_pid = false;
 
+    float current_distance = pp_car_struct->p_right_motor->speed.distance_cm;
+
     set_wheel_direction(direction);
 
     set_wheel_speed_synced(pwm_level, pp_car_struct);
@@ -99,6 +101,13 @@ turn_to_yaw(uint32_t      direction,
         }
     }
 
+    float distance_traveled
+        = pp_car_struct->p_right_motor->speed.distance_cm - current_distance;
+
+    // Update Map after turning
+    update_map(pp_car_struct->p_grid, direction, distance_traveled);
+
+
     pp_car_struct->p_pid->use_pid = true;
     vTaskDelay(pdMS_TO_TICKS(50));
 }