Diff: main.cpp

Revision:

12:82b8fe254222

Parent:

11:0b9098ec11c7

--- a/main.cpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/main.cpp	Sun Jan 05 15:42:22 2020 +0000
@@ -17,14 +17,15 @@
 #include <motordriver.h>
 #include "math.h"
 #include "Motors.hpp"
-//#include "VL6180.hpp"
+#include "VL6180.hpp"
 
 DigitalOut debug_LED(LED1);
 
 // Threads
 Thread ROS_Thread(osPriorityRealtime);   // Create THREAD with highest priority for ROS
+Thread TOF_Thread(osPriorityNormal);
 //Thread Debug_Thread(osPriorityNormal);
-//Thread Movement_Thread(osPriorityNormal);
+//Thread Motor_Thread(osPriorityNormal);
 
 // Thread ID for the Main function (CMSIS API)
 osThreadId tidMain;
@@ -32,20 +33,20 @@
 // main starts all threads
 int main(void)
 {
-    //TOF_Thread();
     // Main thread ID
     tidMain = Thread::gettid();
 
     // Start each thread
     ROS_Thread.start(ROS_Handler);
+    TOF_Thread.start(TOF_Handler);
+    //Motor_Thread.start(Motor_Handler);
     //Debug_Thread.start(TerminalThread);
-    //Thread2.start(main3);
     
     // Creates an object to monitor and handle changes in battery level using an RGB LED as an OutPut
-    Battery_Monitor VBatt_Monitor(ADC_VBAT, 0.5f);
+    Battery_Monitor VBatt_Monitor(ADC_VBAT, 1.0f);
 
     /**************************************************************************/
-    initialize(); //initialize(); only needs to be called once, at the start of the program.
+    //initialize(); //initialize(); only needs to be called once, at the start of the program.
     //the encoder interrupts should be able to handle themselves from there, and do not require
     //resetting.
     /**************************************************************************/
@@ -54,21 +55,8 @@
     {
         //Flag_Error(warning, "Flashing LED\n\r");
         debug_LED = 1;
-        wait(1);
+        wait_ms(500);
         debug_LED = 0;
-        wait(1);
-
-        
-        /**********************************************************************/
-        //careful, putting this in a while loop will mean the bot moves in an infinite square!
-        /**********************************************************************/
-        //tempMove(40);
-        //tempRotate(-90);
-        //tempMove(40);
-        //tempRotate(-90);
-        //tempMove(40);
-        //tempRotate(-90);
-        //tempMove(40);
-        //tempRotate(-90);
+        wait_ms(500);
     }
 }