Diff: main.cpp

Revision:

5:099cb2e76c7d

Parent:

4:315716ef8178

Child:

6:690db8b5030b

--- a/main.cpp	Wed Jan 20 04:01:37 2016 +0000
+++ b/main.cpp	Mon Jan 25 07:28:40 2016 +0000
@@ -1,10 +1,10 @@
 /**
- * @author Akash Vibhute 
+ * @author Akash Vibhute
  * < akash . roboticist [at] gmail . com >
  *
  * @section LICENSE
  *
- * Copyright (c) 2015 Akash Vibhute 
+ * Copyright (c) 2015 Akash Vibhute
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -25,317 +25,173 @@
  *
  * Virgo_v3 robot controller v1.0 with Virgo_v3 PCB [AV22032015]
  * Robot controller software for SUTD Virgo version 3 robot
- * 
+ *
  */
 
-/** 
+/**
  * Header file for including all necessary functions for robot and defining any
  * custom functions written in the main file.
  */
 #include "main.h"
 
-
-//** General Functions ************************************************************************
-DigitalOut debugLED(debug_LED);
-Serial debug(uart_TX, uart_RX);
-camera camera;
-void Heartbeat_loop(void const *n); //heartbeat loop as a seperate thread
-motorDriver drive;
-
-//---------------------------------------------------------------------------------------------
-
-
-
+/**
+ * Functions, Threads & General Definitions
+ */
+//*****************************************************************************
+//** Drivetrain **
+motorDriver drive;  //motor drive train
+odometer odometer;  //odometer function
+pidControl PID_L, PID_R; //pidcontroller for left and right motors
 
-//** IMU MPU9150 ******************************************************************************
-MPU6050 mpu(i2c_SDA, i2c_SCL);  // sda, scl pin
-InterruptIn mpu_int(imu_INT);   // INT0 pin
-
+Timer motorControl_t;
+float rpm_cmd[2]; //drive motor rpm command
+float pwm_cmd[2]; //drive motor pwm command
 
-const int FIFO_BUFFER_SIZE = 250;
-uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
-uint16_t fifoCount;
-uint16_t packetSize;
-bool dmpReady;
-uint8_t mpuIntStatus;
+/* THREAD */
+void odometry_thread(void const *n);
+void motorControl_thread(void const *n);
+/*   **   */
+//-------------
 
+//** Localization **
+IMU_MPU6050 imu;    //MPU9150 wrapper class
+float imuTime;
+localization localization;  //localization function
 
-struct Offset 
-{
-    int16_t ax, ay, az;
-    int16_t gx, gy, gz;
-}imu_Offset = {150, -350, 1000, -110, 5, 0};    // Measured values
+/* FUNCTION */
+bool imuInit_function();
+/*   **   */
+
+/* THREAD */
+void imu_thread(void const *n);
+/*   **   */
+//-------------
 
-struct MPU6050_DmpData 
-{
-    Quaternion q;
-    VectorFloat gravity;    // g
-    float roll, pitch, yaw;     // rad
-    VectorInt16 aa; //acceleration
-    VectorInt16 aaReal; //linear acceleration after removing gravity
-    VectorInt16 aaWorld; //world acceleration after rotating using pose quaternion
-    float roll_v, pitch_v, yaw_v; //filtered values calculated from DMP packet
-    
-    VectorFloat accW; //aaWorld stored as float after conversion into mm/s2
-}imu_Data;
+//** Communications **
+nRF24_Handler nRFcomm;  //nRF24 radio and network layer handler function
+unsigned int rx_addr, rx_dataLen;
+uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted
 
-int16_t raw_gyro[3]; //variable to store data decoded from dmp packet
-float filt_gyro[3]; //variable to store data recalculated values
-float GyroV_window[3][GyroAcc_MWindowSize];
-volatile uint8_t GyroV_MWindowIndex=0; //moving window average index tracker
+/* THREAD */
+void commPump_thread(void const *n);
+void comm_thread(void const *n);
+/*   **   */
+//-------------
 
-Timer imu_timer;
-float imu_time[2] = {0.00, 0.00}; //index 0 in array holds values to correct wrap around condition
+//** Power Monitor **
+BattGuage  battery; //Battery fuel gauge wrapper
 
-float imu_initial_angles[3]; //array holds initial imu angles to offset
-int8_t unstable_readings = 10;
-bool imu_stabilized=false;
+/* THREAD */
 
-bool imu_Initialize();
-void imu_Update(void const *n);
+/*   **   */
+//-------------
 
-Mutex mutex_imuPublish;
-//---------------------------------------------------------------------------------------------
-
-
-//** Motor Control ****************************************************************************
-const float motor_LimitRPM[2] = {motor_LimitRPM_min,motor_LimitRPM_max}; //min max motor RPM limits
-const float motor_Acc = motor_Acc_limit; //RPM / s
+//** Trajectory tracking **
+purePursuit purePursuit;
+Timer camera_t;
 
-const float PIDFf_k[4] = {PIDFf_kP, PIDFf_kI, PIDFf_kD, PIDFf_kFf}; //P, I, D and Ff constants
-const float PIDFf_TermsLimit[2] = {PIDFf_TermsLimit_min, PIDFf_TermsLimit_max}; //min max values of individual terms of controller
-const float control_SummLimit[2] = {control_SummLimit_min, control_SummLimit_max}; //min max value of PIDFf controller summ
+/* THREAD */
+void purePursuit_thread(void const *n);
+void waypointCmd_thread(void const *n);
+/*   **   */
+//-------------
 
-
-PwmOut motor_LeftP(mot_LP), motor_LeftM(mot_LM); //left motor pins
-PwmOut motor_RightP(mot_RP), motor_RightM(mot_RM); //right motor pins
+//** Attitude control **
+attitudeControl attitudeControl;
 
-float motor_CmdRPM[2] = {0.0, 0.0}; //commanded motor RPM
-
-float motor_ControlError[2][3]; //control error left and right for (t) and (t-1) and dE
+/* THREAD */
 
-float PIDFfS_terms[2][6]; //pid controller terms: P, I, D, Ff, Summ, Filtered Summ
+/*   **   */
+//-------------
 
-Timer PID_timer;
-volatile uint16_t dt_PID;
+//** Camera **
+camera camera;  //camera driver
+bool cameraFlag; //flag to enable camera
 
-volatile float pid_MWindowStore[2][pid_MWindowSize];
-volatile float pid_FilterCalc[2];
-volatile uint8_t pid_MWindowIndex=0;
-
-void MotorControl(void const *n);
+/* THREAD */
+void cameraControl_thread(void const *n);
+/*   **   */
+//-------------
 
-Mutex mutex_pidPublish;
-//---------------------------------------------------------------------------------------------
-
+//** Robot data recorder **
+//dataRecorder virgoRDR;
 
-//** Encoder **********************************************************************************
-
-
+/* THREAD */
 
-volatile int32_t encoder_counter[2][2]={{0,0},{0,0}}; //volatile 2x2 array to store left, right counts at time (t) and (t-1)
-float odometry[2][2]={{0.0,0.0},{0.0,0.0}}; //2x2 array to store left, right revolutions and filtered rev/sec
-
-volatile int32_t encoder_MWindowStore[2][encoder_MWindowSize];
-volatile uint8_t encoder_MWindowIndex=0;
-
-const float encoder_SpeedCalcFactor = ((float)(encoder_MWindowSize * encoder_cpr)/(1000.0 * 1000.0));
+/*   **   */
+//-------------
 
-Timer enc_timer;
-volatile int enc_calc_time;
-
+//** Declarations of misc functions **
+DigitalOut debugLED(debug_LED); //led for debugging and heartbeat indication
+Serial debug(uart_TX, uart_RX); //debug serial port
+USBSerial usb; //USB virtual serial port
 
-void OdometryUpdate(void const *n);
-
-Mutex mutex_OdometryPublish;
-
-//---------------------------------------------------------------------------------------------
+/* THREAD */
+void heartbeat_thread(void const *n); //heartbeat loop as an individual thread
+void print_thread(void const *n); //debug printing thread
+/*   **   */
+//-------------
+//-----------------------------------------------------------------------------
 
 
-//** Localization *****************************************************************************
-float robot_pos[2] = {0.0,0.0}; //calculated position of robot
-float robot_speed;
-
-void LocalizationUpdate(); 
-//---------------------------------------------------------------------------------------------
-
-
-//** Pure-Pursuit *****************************************************************************
-//float robotFrame_target[3]= {0.0,0.0,0.0}; //target waypoint rotated to robot frame x, y, distance
-
-
-Mutex mutex_PurePursuitPublish;
-
-void PurePursuitUpdate(void const *n); 
-//---------------------------------------------------------------------------------------------
-
-
-//** Drivetrain commander *********************************************************************
-float drive_velocity[2]= {0.0, 0.0};//left and right velocity in mm/s
-uint16_t motor_cmd_cnt=0; //command updater for testing step response of PID
-
-Mutex mutex_DrivetrainCmdPublish;
-
-void DrivetrainCmd(); 
-//---------------------------------------------------------------------------------------------
-
-
-//** Waypoint commander ***********************************************************************
-float waypoint_target[3]= {0.0, 0.0, 0.0};//x,y, velocity
-
-Mutex mutex_WaypointCmdPublish;
-
-void WaypointCmdUpdate(void const *n); 
-
-const uint16_t waypoint_len = 2;
-float waypoint_set[waypoint_len][3] =   {{0.0, 10000.0, 200.0},
-                                         {0.0, 50000.0, 300.0}}; //x,y, velocity in mm
-
-//---------------------------------------------------------------------------------------------
-
-//** Print Loop thread ************************************************************************
-void print_loop(void const *n);
-Mutex mutex_PrintLoopThread;
-
-//---------------------------------------------------------------------------------------------
-
-//** Communications Loop thread ***************************************************************
-RF24 nrf24_wireless(spi_MOSI, spi_MISO, spi_SCK, nrf_CSN, nrf_CE); //nrf object declaration
-RF24Network virgo_network(nrf24_wireless); // Network uses that radio
-const uint16_t cmd_node = 0; // Address of the commander node
-const uint16_t virgo3_nodeAddr = virgo3_robot_address; // Address of this robot node
-const uint16_t network_channel = virgo3_network_channel; // wireless channel of our network
-uint16_t uploadRateMS = nRfDefault_uploadRateMS; //ms // How often to send data to the other unit
-volatile bool transmit_status;
-
-// Structure of transmit payload
-struct TxPayload_t {
-    float timestamp;
-    uint8_t parameter;
-    float TxVector[3];
-};
-
-// Structure of receive payload
-struct RxPayload_t {
-    uint8_t cmd;
-    float RxVector[4];
-};
-
-RxPayload_t RxData;
-TxPayload_t TxData;
-
-const uint8_t TxChunks=4;
-float TxPackets[TxChunks][3];
-
-Timer comm_timer; // Timer to maintain communication loop rate
-
-void comm_initialize(); //initialization of comm radio
-void comm_loop(void const *n);
-
-
-//---------------------------------------------------------------------------------------------
-
-//** Attitude Control function ****************************************************************
-float filt_WheelV[2];
-float WheelV_window[2][wheelV_MWindowSize];
-volatile uint8_t wheelV_MWindowIndex;
-
-void attitudeController(float* w_L, float* w_R, int flag, float tc,
-                        float beta, float dbeta, float gamma, float dgamma,
-                        float theta_L, float theta_R, float dtheta_L, float dtheta_R,
-                        float ref_ddbeta, float ref_dbeta, float ref_beta,
-                        float ref_ddtheta, float ref_dtheta, float ref_theta,
-                        float ref_ddgamma,  float ref_dgamma, float ref_gamma);
-                        
-//attitude controller
-    float W_l, W_r; //left, right wheel velocity in rad/s. direction inverted to correspond to IMU XYZ frame
-    float pitch_th, pitch_om; //pitch angle, velocity
-    float yaw_th, yaw_om; //heading angle, velocity
-    float wheelTh_l, wheelTh_r; //wheel position left, right
-    float wheelOm_l, wheelOm_r; //wheel velocity left, right                        
-                        
-//---------------------------------------------------------------------------------------------
-
-//** Camera control thread ********************************************************************
-void CameraControl_loop(void const *n);
-DigitalOut camEN(cam_EN);
-uint32_t camera_OnRateS = camera_OnRateMinutes * 60; //camera switch on frequency in sec
-uint32_t camera_RunTimeS = camera_OnS(camera_CaptureTimeS); //camera total runtime in sec
-
-Timer Camera_timer;
-volatile uint32_t dt_Camera;
-
-//---------------------------------------------------------------------------------------------
-
-// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //
-
-
-int main() 
+int main()
 {
     debug.baud(PC_BAUDRATE);
     debugLED =1;
     drive.setPWM_L(0);
     drive.setPWM_R(0);
 
+    debug.printf("** Starting Virgo v3 Routines *************\n\n");
 
-    
-    
-    debug.printf("** Starting Virgo v3 Routines *************\n\n");
-    
-    
-    //** start main loop as a thread **********************************************************
-    Thread Heartbeat_function(Heartbeat_loop, NULL, osPriorityNormal);
-    debug.printf("* Hearbeat loop started *\n"); 
+    //** start Hearbeat loop as a thread **
+    Thread Heartbeat_function(heartbeat_thread, NULL, osPriorityNormal);
+    debug.printf("* Hearbeat loop started *\n");
+
+    //** start IMU funtion as Thread **
+    Thread IMU_function(imu_thread, NULL, osPriorityHigh);
+    debug.printf("* IMU routine started *\n");
+
+    //** start OdometryUpdate function as Thread **
+    Thread Odometry_function(odometry_thread, NULL, osPriorityNormal);
+    debug.printf("* Odometry routine started *\n");
+
+    //** start MotorControl function as Thread **
+    Thread MotorControl_function(motorControl_thread, NULL, osPriorityNormal);
+    debug.printf("* Motor control routine started *\n");
 
-    //** start IMU funtion as Thread **********************************************************
-    MBED_ASSERT(imu_Initialize() == true);
-    Thread IMU_function(imu_Update, NULL, osPriorityHigh);
-    debug.printf("* IMU routine started *\n");
-        
-    //** start OdometryUpdate function as Thread **********************************************
-    enc_timer.start();
-    Thread Odometry_function(OdometryUpdate, NULL, osPriorityNormal);
-    debug.printf("* Odometry routine started *\n");
-    
-    //** start MotorControl function as Thread ************************************************
-    PID_timer.start();
-    Thread MotorControl_function(MotorControl, NULL, osPriorityNormal);
-    debug.printf("* Motor control routine started *\n");    
+    //** start PurePursuit controller as Thread **
+    Thread PurePursuitUpdate_function(purePursuit_thread, NULL, osPriorityNormal);
+    debug.printf("* PurePursuit controller routine started *\n");
+
+    //** start Waypoint commander function as Thread **
+    Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal);
+    debug.printf("* Waypoint commander routine started *\n");
+
+    //** start comm loop as a thread **
+    nRFcomm.commInit();
+    Thread CommPump_function(commPump_thread, NULL, osPriorityNormal);
+    Thread CommLoop_function(comm_thread, NULL, osPriorityNormal);
+    debug.printf("* Communications loop started *\n");
+
+    //** start camera control loop as a thread **
+    Thread CameraControl_function(cameraControl_thread, NULL, osPriorityNormal);
+    debug.printf("* Camera control loop started *\n");
+
+    //** start debug print loop as a thread **
+    Thread PrintLoop_function(print_thread, NULL, osPriorityNormal);
+    debug.printf("* Print loop started *\n\n\n");
+
+    debug.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");
     
-    //** start PurePursuit controller as Thread ***********************************************
-    Thread PurePursuitUpdate_function(PurePursuitUpdate, NULL, osPriorityNormal);
-    debug.printf("* PurePursuit controller routine started *\n"); 
-    
-    //** start Waypoint commander function as Thread ******************************************
-    Thread WaypointCmdUpdate_function(WaypointCmdUpdate, NULL, osPriorityNormal);
-    debug.printf("* Waypoint commander routine started *\n");  
-    
-    //** start comm loop as a thread **********************************************************
-    //comm_initialize();
-    //Thread CommLoop_function(comm_loop, NULL, osPriorityNormal);
-    //debug.printf("* Communications loop started *\n"); 
-    
-    //** start ccamera control loop as a thread ***********************************************
-    //Thread CameraControl_function(CameraControl_loop, NULL, osPriorityNormal);
-    //debug.printf("* Camera control loop started *\n"); 
-
-    //** start print loop as a thread **********************************************************
-    //Thread PrintLoop_function(print_loop, NULL, osPriorityNormal);
-    //debug.printf("* Print loop started *\n\n\n");
-    
-    //debug.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");
-    printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");
-    
-    camEN = 1;
-    
-    while(1) //Program does not halt for any runtime errors, atleast for now. Plan to add a watchdog later.
-    { 
-        virgo_network.update(); // Pump the network regularly
+    while(1) {
+        
     }
 }
 
-//heatbeat loop as a seperate thread
-void Heartbeat_loop(void const *n)
+/**
+ * heartbeat loop as an individual thread
+ */
+void heartbeat_thread(void const *n)
 {
     while(true) {
         debugLED = !debugLED;
@@ -343,36 +199,166 @@
     }
 }
 
-//CameraControl loop as a seperate thread
-void CameraControl_loop(void const *n)
+/**
+ * imu initialization function
+ */
+bool imuInit_function()
+{
+    return (imu.imuInit());
+}
+
+/**
+ * imu update loop as an individual thread
+ */
+void imu_thread(void const *n)
 {
-    camEN = 0;
-    Camera_timer.start();
+    while(true) {
+        imu.imuUpdate();
+        
+        //imu.Pose[0, 1, 2]; //euler x, y, z
+        //imu.AngVel[0, 1, 2]; //AngVel x, y, z
+        //imu.LinAcc[0, 1, 2]; //LinAcc x, y, z
+        //imu.Quat[0, 1, 2, 3]; //Quaternion w, x, y, z
+
+        imuTime = imu.time_s;
+
+        Thread::wait(imu_UpdatePeriodMS);
+    }
+}
+
+/**
+ * odometry update loop as an individual thread
+ */
+void odometry_thread(void const *n)
+{
+    while(true) { 
+        odometer.update();
+        
+        //odometer.revolutions[0, 1]; //revolutions left, right
+        //odometer.rpm[0, 1]; //rpm left, right
+
+        Thread::wait(odometry_UpdatePeriodMS);
+    }
+}
+
+/**
+ * motor control loop as an individual thread
+ */
+void motorControl_thread(void const *n)
+{
+    motorControl_t.start();
+
     while(true) {
-        if(Camera_timer.read() <= camera_RunTimeS)
-            camEN = 1;
+        /*
+        attitudeControl.GenWheelVelocities(float* w_L, float* w_R, int flag, float tc,
+                                           float beta, float dbeta, float gamma, float dgamma,
+                                           float theta_L, float theta_R, float dtheta_L, float dtheta_R,
+                                           float ref_ddbeta, float ref_dbeta, float ref_beta,
+                                           float ref_ddtheta, float ref_dtheta, float ref_theta,
+                                           float ref_ddgamma,  float ref_dgamma, float ref_gamma);
+        */
+        
+        pwm_cmd[0]=PID_L.calcOutput(rpm_cmd[0], odometer.rpm[0], motorControl_t.read());
+        pwm_cmd[1]=PID_R.calcOutput(rpm_cmd[1], odometer.rpm[1], motorControl_t.read());
+
+        motorControl_t.reset();
+
+        drive.setPWM_L(pwm_cmd[0]);
+        drive.setPWM_R(pwm_cmd[1]);
+
+        Thread::wait(motorControl_UpdatePeriodMS);
+    }
+}
+
+/**
+ * pure pursuit loop as an individual thread
+ */
+void purePursuit_thread(void const *n)
+{
+    while(true) { 
+    /*
+        purePursuit.GenVW(float* purePursuit_velocity, float* purePursuit_omega,
+                        float target_waypoint[2], float target_velocity,
+                        float current_position[2], float current_heading);
+    */
+        Thread::wait(PurePursuit_UpdatePeriodMS);
+    }
+}
+
+/**
+ * waypoint command update loop as an individual thread
+ */
+void waypointCmd_thread(void const *n)
+{
+    while(true) { 
+
+        Thread::wait(WaypointCmd_UpdatePeriodMS);
+    }
+}
+
+/**
+ * nRF network communications pump as an individual thread
+ */
+void commPump_thread(void const *n)
+{
+    while(true) { 
+        nRFcomm.commUpdate();
+        
+        Thread::wait(CommPump_UpdatePeriodMS);
+    }
+}
+
+/**
+ * communications loop as an individual thread
+ */
+void comm_thread(void const *n)
+{
+    dataSend_flag=0;
+    
+    while(true) { 
+        if(nRFcomm.commReceive(&rx_addr, &rx_dataLen)) {
+            //nRFcomm.parameter_in[0, 1];
+            //nRFcomm.data_in[0, 1, ..., n];
             
-        if((Camera_timer.read() > camera_RunTimeS) && (Camera_timer.read() <= (camera_OnRateS+camera_RunTimeS)))
-            camEN = 0;
-            
-        if(Camera_timer.read() > (camera_OnRateS+camera_RunTimeS))
-            Camera_timer.reset();
-            
+            //process received data
+        }
+        
+        if(dataSend_flag) {
+            //nRFcomm.commSend(unsigned int toAddress, unsigned int parameter[2], float data[], unsigned int dataLen);
+        }
+        
+        Thread::wait(CommLoop_UpdatePeriodMS);
+    }
+}
+
+/**
+ * CameraControl loop as an individual thread
+ */
+void cameraControl_thread(void const *n)
+{
+    cameraFlag=0;
+    camera.setFrequency(camera_CaptureTimeS*1.0, camera_cycleMinutes*60.0);
+
+    while(true) {
+        camera.updateState();
+        camera.setState(camera.camFlag);
+
         Thread::wait(100); //proocess thread every 100ms
     }
 }
 
-
-//print loop as a seperate thread
+/**
+ * debug data print loop as an individual thread
+ */
 #define print_lines 2 //number of info lines being printed on screen
-void print_loop(void const *n)
+void print_thread(void const *n)
 {
-    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
+    while(!imu.imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
     
     //clear 14 lines while going up, these are the initilization lines printed on screen
     for(int l=14; l>0; l--) {
-        //debug.printf("\e[1A"); //go up 1 line
-        //debug.printf("\e[K"); //clear line
+        debug.printf("\e[1A"); //go up 1 line
+        debug.printf("\e[K"); //clear line
     }
 
     printf("************ VIRGO v3: Status Monitor *************\n\n");
@@ -388,7 +374,7 @@
         //mutex_PrintLoopThread.lock();      
 
         
-        printf("Elapsed time: %.2f s\n\e[K", imu_time[1]);
+        printf("Elapsed time: %.2f s\n\e[K", imuTime);
         //debug.printf("Status: Heading %.2f, CmdRPM <%.1f , %.1f>\n\e[K", RAD_TO_DEG(imu_Data.yaw), motor_CmdRPM[0], motor_CmdRPM[1]);
         //debug.printf("Orientation (YPR): (%.2f , %.2f , %.2f)\n\e[K", RAD_TO_DEG(imu_Data.yaw), RAD_TO_DEG(imu_Data.pitch), RAD_TO_DEG(imu_Data.roll));
         //debug.printf("Command: Position <%.1f , %.1f> \tVelocity <%.1f>\n\e[K", waypoint_target[0], waypoint_target[1], waypoint_target[2]);
@@ -414,110 +400,6 @@
         
         //mutex_PrintLoopThread.unlock();
         
-
-        
         Thread::wait(PrintLoop_PeriodMS);
     }
-}
-
-void comm_initialize()
-{
-
-    
-    comm_timer.start();
-}
-
-//Communications thread running at CommLoop_UpdateRateHz, data transmission at uploadRateMS
-void comm_loop(void const *n)
-{
-    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
-    while(true) {
-
-        Thread::wait(CommLoop_UpdatePeriodMS);
-    }
-}
-
-//Pure pursuit controller thread running at PurePursuit_UpdateRateHz
-void PurePursuitUpdate(void const *n)
-{
-    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
-    while(true) {
-        
-        
-        DrivetrainCmd();
-        mutex_PurePursuitPublish.unlock();
-        
-        Thread::wait(PurePursuit_UpdatePeriodMS);
-    }
-}
-
-//Drive train commander and kinematics solver
-void DrivetrainCmd()
-{
-    
-    //attitude controller
-    
-    
-    //motor_CmdRPM[0];
-    //motor_CmdRPM[1];
-}
-
-void WaypointCmdUpdate(void const *n)
-{
-    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
-    while(true) {
-        mutex_WaypointCmdPublish.lock();
-
-        for(int i=0; i<3; i++) waypoint_target[i] = waypoint_set[0][i];
-
-        mutex_WaypointCmdPublish.unlock();
-        
-        Thread::wait(WaypointCmd_UpdatePeriodMS);
-    }
-}
-
-//Motor control thread running at motorControl_UpdateRateHz
-void MotorControl(void const *n)
-{
-    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
-    while(true) {
-        
-        Thread::wait(motorControl_UpdatePeriodMS);
-    }
-}
-
-
-void OdometryUpdate(void const *n)
-{
-    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
-    
-    /***/while(true)/***/ { 
-        
-
-        Thread::wait(odometry_UpdatePeriodMS);
-    }
-}
-
-float distance_increment;
-//Localization loop called after every odometry update
-void LocalizationUpdate()
-{
-    
-}
-
-bool imu_Initialize()
-{
-    
-
-    return true;
-}
-
-void imu_Update(void const *n)
-{
-    /***/while(true) { /***/
-    
-    
-        Thread::wait(imu_UpdatePeriodMS);
-    }
-}
-
+}
\ No newline at end of file