Diff: main.cpp

Revision:

4:315716ef8178

Parent:

2:761e3c932ce0

Child:

5:099cb2e76c7d

--- a/main.cpp	Mon Jan 18 09:07:39 2016 +0000
+++ b/main.cpp	Wed Jan 20 04:01:37 2016 +0000
@@ -38,25 +38,13 @@
 //** General Functions ************************************************************************
 DigitalOut debugLED(debug_LED);
 Serial debug(uart_TX, uart_RX);
-
-DigitalOut cameraEN(cam_EN);
-
-
+camera camera;
 void Heartbeat_loop(void const *n); //heartbeat loop as a seperate thread
+motorDriver drive;
 
 //---------------------------------------------------------------------------------------------
 
-//** Function exectution rates ****************************************************************
-const uint8_t Hearbeat_RateMS = 1000/Hearbeat_RateHz;// Heartbeat update rate in ms
-const uint8_t imu_UpdatePeriodMS = 1000/imu_UpdateRateHz;// imu update rate in ms
-const uint8_t motorControl_UpdatePeriodMS = 1000/motorControl_UpdateRateHz;// motorControl update rate in ms
-const uint8_t odometry_UpdatePeriodMS = 1000/odometry_UpdateRateHz;//odometry update rate in ms
-const uint8_t PrintLoop_PeriodMS = 1000/PrintLoop_RateHz; //print loop run rate in ms
-const uint8_t PurePursuit_UpdatePeriodMS = 1000/PurePursuit_UpdateRateHz; //Pure pursuit update run rate in ms
-const uint8_t WaypointCmd_UpdatePeriodMS = 1000/WaypointCmd_UpdateRateHz; //Waypoint commander update rate in ms
-const uint8_t CommLoop_UpdatePeriodMS = 1000/CommLoop_UpdateRateHz; //Communications loop run rate in ms
 
-//---------------------------------------------------------------------------------------------
 
 
 //** IMU MPU9150 ******************************************************************************
@@ -93,7 +81,7 @@
 
 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][GyroV_MWindowSize];
+float GyroV_window[3][GyroAcc_MWindowSize];
 volatile uint8_t GyroV_MWindowIndex=0; //moving window average index tracker
 
 Timer imu_timer;
@@ -284,18 +272,17 @@
 
 int main() 
 {
-    motor_LeftP = 1;
-    motor_LeftM = 1;
-            
-    motor_RightP = 1;
-    motor_RightM = 1;
+    debug.baud(PC_BAUDRATE);
+    debugLED =1;
+    drive.setPWM_L(0);
+    drive.setPWM_R(0);
 
 
-    debug.baud(PC_BAUDRATE);
-    debugLED =1;
+    
     
     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"); 
@@ -336,7 +323,8 @@
     //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");
+    //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;
     
@@ -383,24 +371,24 @@
     
     //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
     }
 
-    debug.printf("************ VIRGO v3: Status Monitor *************\n\n");
-    for(int l=print_lines; l>0; l--) debug.printf("\n");
-    debug.printf("\n===================================================");
-    debug.printf("\e[1A"); //go up 1 line
+    printf("************ VIRGO v3: Status Monitor *************\n\n");
+    for(int l=print_lines; l>0; l--) printf("\n");
+    printf("\n===================================================");
+    printf("\e[1A"); //go up 1 line
 
     while(true) {
         //move cursor up # of lines printed to create a static display and clear the first line
-        for(int l=print_lines; l>0; l--) debug.printf("\e[1A");
-        debug.printf("\e[K");
+        for(int l=print_lines; l>0; l--) printf("\e[1A");
+        printf("\e[K");
         
         //mutex_PrintLoopThread.lock();      
 
         
-        debug.printf("Elapsed time: %.2f s\n\e[K", imu_time[1]);
+        printf("Elapsed time: %.2f s\n\e[K", imu_time[1]);
         //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]);