program/main_controller/src/i2c/headers/i2c_master.h

@@ -6,7 +6,7 @@
 #define I2C_MASTER_SDA_PIN 16
 #define I2C_MASTER_SCL_PIN 17
 #define I2C_MASTER_INSTANCE i2c0
-#define I2C_MASTER_BAUD_RATE 200 * 1000
+#define I2C_MASTER_BAUD_RATE 300 * 1000
 
 // Init master i2c
 void i2c_master_init(void);

program/main_controller/src/robot.c

@@ -44,41 +44,43 @@ void robot_init(void)
 
         led_state = !led_state;
     }
+    printf("robot.is_running : %d\n", robot.is_running);
 }
 
 static inline void update_time(void)
 {
-    static float last_time_ms = 0.0;
-    float start_time_ms = (float)clock() * 1000.0f / (float)CLOCKS_PER_SEC;
-    robot.delta_time_ms = start_time_ms - last_time_ms;
-    last_time_ms = start_time_ms;
+    static float last_time = 0.0;
+    float start_time = (float)clock() * 1000.0f / (float)CLOCKS_PER_SEC;
+    robot.delta_time_ms = start_time - last_time;
+    last_time = start_time;
 
-    static float elapsed_time_ms = 0.0f;
-    elapsed_time_ms += robot.delta_time_ms;
+    static float elapsed_time = 0.0f;
+    elapsed_time += robot.delta_time_ms;
 
     static bool led_state = false;
-    if(elapsed_time_ms >= 1000.0f)
+    if(elapsed_time >= 1000.0f)
     {
+        elapsed_time = 0.0f;
+
         cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state);
 
-        elapsed_time_ms = 0.0f;
         led_state = !led_state;
     }
 }
 
 void robot_handle_inputs_outputs(void)
 {
-    update_time();
-
     cyw43_arch_poll();
 
-    gyro_update();
+    update_time();
 
-    //update_motion_control();
+    //gyro_update();
+
+    //i2c_update_motion_control();
 
     //i2c_update_servo_motors();
 
-    mcp23017_update();
+    //mcp23017_update();
 
     tight_loop_contents();
 }
@@ -86,8 +88,6 @@ void robot_handle_inputs_outputs(void)
 void robot_deinit(void)
 {
     udp_client_deinit();
-
     cyw43_arch_deinit();
-
     i2c_master_deinit();
 }