From 3a6b97d462f5a39fdfe401dc2399c6dde5e705a2 Mon Sep 17 00:00:00 2001
From: Michael Ferguson <mfergs7@gmail.com>
Date: Tue, 2 Apr 2024 16:03:06 -0400
Subject: [PATCH] phase1 working * allow bootloader to always work * fix
 setting of start button * handle accelerometer orientation change

---
 projects/tablebot/main.cpp | 75 +++++++++++++++++++-------------------
 1 file changed, 37 insertions(+), 38 deletions(-)

diff --git a/projects/tablebot/main.cpp b/projects/tablebot/main.cpp
index 3700cca..572434b 100644
--- a/projects/tablebot/main.cpp
+++ b/projects/tablebot/main.cpp
@@ -153,40 +153,39 @@ void udp_callback(void *arg, struct udp_pcb *udp, struct pbuf *p,
   return_port = port;
   last_packet = system_state.time;
 
-  if (tablebot_mode)
+  if (p->len == 8 &&
+      data[4] == 'B' &&
+      data[5] == 'O' &&
+      data[6] == 'O' &&
+      data[7] == 'T')
   {
-    // Send packet with just the table, no laser data
-    udp_send_packet((unsigned char *) &system_state, sizeof(system_state), return_port);
+    // Disable interrupts before jumping
+    __disable_irq();
 
-    if (p->len == 8 &&
-        data[4] == 'B' &&
-        data[5] == 'O' &&
-        data[6] == 'O' &&
-        data[7] == 'T')
-    {
-      // Disable interrupts before jumping
-      __disable_irq();
+    // Disable motor driver
+    m1_pwm::mode(GPIO_INPUT);
+    m2_pwm::mode(GPIO_INPUT);
+    m1_en::mode(GPIO_INPUT);
+    m2_en::mode(GPIO_INPUT);
 
-      // Disable motor driver
-      m1_pwm::mode(GPIO_INPUT);
-      m2_pwm::mode(GPIO_INPUT);
-      m1_en::mode(GPIO_INPUT);
-      m2_en::mode(GPIO_INPUT);
-
-      // Turn off LED
-      act::low();
+    // Turn off LED
+    act::low();
 
-      // Jump into bootloader
-      force_bootloader::mode(GPIO_OUTPUT);
-      force_bootloader::low();
-      uint32_t JumpAddress = *(__IO uint32_t*) (0x08000000 + 4);
-      pFunction Jump_To_Application = (pFunction) JumpAddress;
-      __set_MSP(*(__IO uint32_t*)0x08000000);
-      Jump_To_Application();
-    }
+    // Jump into bootloader
+    force_bootloader::mode(GPIO_OUTPUT);
+    force_bootloader::low();
+    uint32_t JumpAddress = *(__IO uint32_t*) (0x08000000 + 4);
+    pFunction Jump_To_Application = (pFunction) JumpAddress;
+    __set_MSP(*(__IO uint32_t*)0x08000000);
+    Jump_To_Application();
+  }
 
-    // Free buffer
+  if (tablebot_mode)
+  {
+    // Send packet with just the table, no laser data
+    udp_send_packet((unsigned char *) &system_state, sizeof(system_state), return_port);
     pbuf_free(p);
+    return;
   }
 
   // Traditional Etherbotix Mode
@@ -696,7 +695,7 @@ int main(void)
 
   // Check state of start button - if held - enable tablebot mode
   d7::mode(GPIO_INPUT);
-  d7::pullup();
+  d7::high();
   if (d7::value() == 0)
   {
     tablebot_mode = true;
@@ -805,17 +804,17 @@ int main(void)
     {
       if (imu.update(system_state.time))
       {
-        system_state.accel_x = imu.accel_data.x;
-        system_state.accel_y = imu.accel_data.y;
-        system_state.accel_z = imu.accel_data.z;
+        system_state.accel_x = -imu.accel_data.x;
+        system_state.accel_y = -imu.accel_data.z;
+        system_state.accel_z = -imu.accel_data.y;
 
-        system_state.gyro_x = imu.gyro_data.x;
-        system_state.gyro_y = imu.gyro_data.y;
-        system_state.gyro_z = imu.gyro_data.z;
+        system_state.gyro_x = -imu.gyro_data.x;
+        system_state.gyro_y = -imu.gyro_data.z;
+        system_state.gyro_z = -imu.gyro_data.y;
 
-        system_state.mag_x = imu.mag_data.x;
-        system_state.mag_y = imu.mag_data.y;
-        system_state.mag_z = imu.mag_data.z;
+        system_state.mag_x = -imu.mag_data.x;
+        system_state.mag_y = -imu.mag_data.z;
+        system_state.mag_z = -imu.mag_data.y;
       }
 
       system_state.run_state = select_mode();