Diff: main.cpp

Revision:

0:964eb6a2ef00

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 18 22:23:48 2015 +0000
@@ -0,0 +1,134 @@
+/* This is the current working code for the FYDP Autowalk
+-only IMU1 works. I don't know why IMU2 works. I'll fix that later
+*/
+
+#include "robot.h"
+#include "bt_shell.h"
+
+RtosTimer *MotorStopTimer;
+RtosTimer *HalfHourTimer;
+
+void HalfHourUpdate(void const *args)
+{
+        long_time += t.read_us();
+        t.reset();
+}
+
+/*
+RtosTimer *OpticalFlowTimer;
+
+void update_opt(void const *args)
+{
+    reckon.updateOpticalFlow();   
+}
+
+void RF_mesh_thread(void const *args)   //this thread handles the mesh network
+{
+    while(1){
+        Thread::wait(10);
+    }
+}
+
+*/
+
+void moveMotors(int Lspeed, int Rspeed, int ms)
+{   
+    *motors.Left = Lspeed;
+    *motors.Right = Rspeed;
+    
+    if(ms > 0)
+        MotorStopTimer->start(ms); 
+}
+
+void stopMotors(void const *args)
+{   
+    *motors.Left = 0;
+    *motors.Right = 0;    
+    send_flag = 0;
+}
+
+void CURRENT_thread(void const *args)
+{
+    float current;
+    while(1){
+        current = current_sensor.get_current();
+        bt.lock();
+        bt.printf("\n\rCurrent: %f mA",current);
+        bt.unlock();
+        Thread::wait(50);
+    }
+}
+
+void optflow_thread(void const *args)
+{    
+    while(true) {
+        reckon.updateOpticalFlow();                   
+        Thread::wait(3);
+    }    
+}
+
+void IMU_thread(void const *args)
+{
+     bt.baud(BT_BAUD_RATE);   //you have to do this for some reason
+    //test_dmp(); 
+    test_dmp2();
+    bt.baud(BT_BAUD_RATE);   //you have to do this for some reason
+    //start_dmp(mpu);
+    start_dmp2(mpu2);
+    //calibrate_1();
+
+    while(1) {
+        /*if(calibrate_flag)
+        {
+            calibrate_1();
+            calibrate_flag = 0;
+        }*/
+        
+        //if(!mpuInterrupt && fifoCount < packetSize);    //interrupt not ready
+        //else {  //mpu interrupt is ready
+        //    update_dmp();
+          update_dmp2();
+            //mpuInterrupt = false;   //this resets the interrupt flag
+            //mpuInterrupt2 = false;
+        //}
+        
+         Thread::yield();
+    }
+}
+
+void bt_shell(void const *args)
+{
+    bt_shell_init();
+    bt.printf("BT shell initialized\r\n");
+    
+    while(true) {
+        bt_shell_run();
+        Thread::yield();
+    }    
+}
+
+int main()
+{
+    initRobot();
+    
+    MotorStopTimer = new RtosTimer(&stopMotors, osTimerOnce, NULL);    
+    HalfHourTimer = new RtosTimer(&HalfHourUpdate, osTimerPeriodic, NULL);
+    
+    HalfHourTimer->start(1800000);  //doesn't work because the timer takes over (high priority)
+    
+    //OpticalFlowTimer = new RtosTimer(&update_opt, osTimerPeriodic, NULL);
+    //OpticalFlowTimer->start(10);  //doesn't work because the timer takes over (high priority)
+        
+
+    Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL);    //if you get a hard fault, increase memory size of this thread (second last value)
+    
+    Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL);
+    
+//    Thread current_sense(CURRENT_thread,NULL,osPriorityNormal,300,NULL);  //this thread for monitoring current is unnecessary. Thread use lotsa rams. Kill it.
+    
+    //Thread optflow_th(optflow_thread, NULL, osPriorityNormal, 500, NULL); 
+    
+    //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL);
+    
+    Thread::wait(osWaitForever);
+}
\ No newline at end of file