Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

main.cpp@0:964eb6a2ef00, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:23:48 2015 +0000
Revision:
0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
majik 0:964eb6a2ef00 1 /* This is the current working code for the FYDP Autowalk
majik 0:964eb6a2ef00 2 -only IMU1 works. I don't know why IMU2 works. I'll fix that later
majik 0:964eb6a2ef00 3 */
majik 0:964eb6a2ef00 4
majik 0:964eb6a2ef00 5 #include "robot.h"
majik 0:964eb6a2ef00 6 #include "bt_shell.h"
majik 0:964eb6a2ef00 7
majik 0:964eb6a2ef00 8 RtosTimer *MotorStopTimer;
majik 0:964eb6a2ef00 9 RtosTimer *HalfHourTimer;
majik 0:964eb6a2ef00 10
majik 0:964eb6a2ef00 11 void HalfHourUpdate(void const *args)
majik 0:964eb6a2ef00 12 {
majik 0:964eb6a2ef00 13 long_time += t.read_us();
majik 0:964eb6a2ef00 14 t.reset();
majik 0:964eb6a2ef00 15 }
majik 0:964eb6a2ef00 16
majik 0:964eb6a2ef00 17 /*
majik 0:964eb6a2ef00 18 RtosTimer *OpticalFlowTimer;
majik 0:964eb6a2ef00 19
majik 0:964eb6a2ef00 20 void update_opt(void const *args)
majik 0:964eb6a2ef00 21 {
majik 0:964eb6a2ef00 22 reckon.updateOpticalFlow();
majik 0:964eb6a2ef00 23 }
majik 0:964eb6a2ef00 24
majik 0:964eb6a2ef00 25 void RF_mesh_thread(void const *args) //this thread handles the mesh network
majik 0:964eb6a2ef00 26 {
majik 0:964eb6a2ef00 27 while(1){
majik 0:964eb6a2ef00 28 Thread::wait(10);
majik 0:964eb6a2ef00 29 }
majik 0:964eb6a2ef00 30 }
majik 0:964eb6a2ef00 31
majik 0:964eb6a2ef00 32 */
majik 0:964eb6a2ef00 33
majik 0:964eb6a2ef00 34 void moveMotors(int Lspeed, int Rspeed, int ms)
majik 0:964eb6a2ef00 35 {
majik 0:964eb6a2ef00 36 *motors.Left = Lspeed;
majik 0:964eb6a2ef00 37 *motors.Right = Rspeed;
majik 0:964eb6a2ef00 38
majik 0:964eb6a2ef00 39 if(ms > 0)
majik 0:964eb6a2ef00 40 MotorStopTimer->start(ms);
majik 0:964eb6a2ef00 41 }
majik 0:964eb6a2ef00 42
majik 0:964eb6a2ef00 43 void stopMotors(void const *args)
majik 0:964eb6a2ef00 44 {
majik 0:964eb6a2ef00 45 *motors.Left = 0;
majik 0:964eb6a2ef00 46 *motors.Right = 0;
majik 0:964eb6a2ef00 47 send_flag = 0;
majik 0:964eb6a2ef00 48 }
majik 0:964eb6a2ef00 49
majik 0:964eb6a2ef00 50 void CURRENT_thread(void const *args)
majik 0:964eb6a2ef00 51 {
majik 0:964eb6a2ef00 52 float current;
majik 0:964eb6a2ef00 53 while(1){
majik 0:964eb6a2ef00 54 current = current_sensor.get_current();
majik 0:964eb6a2ef00 55 bt.lock();
majik 0:964eb6a2ef00 56 bt.printf("\n\rCurrent: %f mA",current);
majik 0:964eb6a2ef00 57 bt.unlock();
majik 0:964eb6a2ef00 58 Thread::wait(50);
majik 0:964eb6a2ef00 59 }
majik 0:964eb6a2ef00 60 }
majik 0:964eb6a2ef00 61
majik 0:964eb6a2ef00 62 void optflow_thread(void const *args)
majik 0:964eb6a2ef00 63 {
majik 0:964eb6a2ef00 64 while(true) {
majik 0:964eb6a2ef00 65 reckon.updateOpticalFlow();
majik 0:964eb6a2ef00 66 Thread::wait(3);
majik 0:964eb6a2ef00 67 }
majik 0:964eb6a2ef00 68 }
majik 0:964eb6a2ef00 69
majik 0:964eb6a2ef00 70 void IMU_thread(void const *args)
majik 0:964eb6a2ef00 71 {
majik 0:964eb6a2ef00 72 bt.baud(BT_BAUD_RATE); //you have to do this for some reason
majik 0:964eb6a2ef00 73 //test_dmp();
majik 0:964eb6a2ef00 74 test_dmp2();
majik 0:964eb6a2ef00 75 bt.baud(BT_BAUD_RATE); //you have to do this for some reason
majik 0:964eb6a2ef00 76 //start_dmp(mpu);
majik 0:964eb6a2ef00 77 start_dmp2(mpu2);
majik 0:964eb6a2ef00 78 //calibrate_1();
majik 0:964eb6a2ef00 79
majik 0:964eb6a2ef00 80 while(1) {
majik 0:964eb6a2ef00 81 /*if(calibrate_flag)
majik 0:964eb6a2ef00 82 {
majik 0:964eb6a2ef00 83 calibrate_1();
majik 0:964eb6a2ef00 84 calibrate_flag = 0;
majik 0:964eb6a2ef00 85 }*/
majik 0:964eb6a2ef00 86
majik 0:964eb6a2ef00 87 //if(!mpuInterrupt && fifoCount < packetSize); //interrupt not ready
majik 0:964eb6a2ef00 88 //else { //mpu interrupt is ready
majik 0:964eb6a2ef00 89 // update_dmp();
majik 0:964eb6a2ef00 90 update_dmp2();
majik 0:964eb6a2ef00 91 //mpuInterrupt = false; //this resets the interrupt flag
majik 0:964eb6a2ef00 92 //mpuInterrupt2 = false;
majik 0:964eb6a2ef00 93 //}
majik 0:964eb6a2ef00 94
majik 0:964eb6a2ef00 95 Thread::yield();
majik 0:964eb6a2ef00 96 }
majik 0:964eb6a2ef00 97 }
majik 0:964eb6a2ef00 98
majik 0:964eb6a2ef00 99 void bt_shell(void const *args)
majik 0:964eb6a2ef00 100 {
majik 0:964eb6a2ef00 101 bt_shell_init();
majik 0:964eb6a2ef00 102 bt.printf("BT shell initialized\r\n");
majik 0:964eb6a2ef00 103
majik 0:964eb6a2ef00 104 while(true) {
majik 0:964eb6a2ef00 105 bt_shell_run();
majik 0:964eb6a2ef00 106 Thread::yield();
majik 0:964eb6a2ef00 107 }
majik 0:964eb6a2ef00 108 }
majik 0:964eb6a2ef00 109
majik 0:964eb6a2ef00 110 int main()
majik 0:964eb6a2ef00 111 {
majik 0:964eb6a2ef00 112 initRobot();
majik 0:964eb6a2ef00 113
majik 0:964eb6a2ef00 114 MotorStopTimer = new RtosTimer(&stopMotors, osTimerOnce, NULL);
majik 0:964eb6a2ef00 115 HalfHourTimer = new RtosTimer(&HalfHourUpdate, osTimerPeriodic, NULL);
majik 0:964eb6a2ef00 116
majik 0:964eb6a2ef00 117 HalfHourTimer->start(1800000); //doesn't work because the timer takes over (high priority)
majik 0:964eb6a2ef00 118
majik 0:964eb6a2ef00 119 //OpticalFlowTimer = new RtosTimer(&update_opt, osTimerPeriodic, NULL);
majik 0:964eb6a2ef00 120 //OpticalFlowTimer->start(10); //doesn't work because the timer takes over (high priority)
majik 0:964eb6a2ef00 121
majik 0:964eb6a2ef00 122
majik 0:964eb6a2ef00 123 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)
majik 0:964eb6a2ef00 124
majik 0:964eb6a2ef00 125 Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL);
majik 0:964eb6a2ef00 126
majik 0:964eb6a2ef00 127 // Thread current_sense(CURRENT_thread,NULL,osPriorityNormal,300,NULL); //this thread for monitoring current is unnecessary. Thread use lotsa rams. Kill it.
majik 0:964eb6a2ef00 128
majik 0:964eb6a2ef00 129 //Thread optflow_th(optflow_thread, NULL, osPriorityNormal, 500, NULL);
majik 0:964eb6a2ef00 130
majik 0:964eb6a2ef00 131 //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL);
majik 0:964eb6a2ef00 132
majik 0:964eb6a2ef00 133 Thread::wait(osWaitForever);
majik 0:964eb6a2ef00 134 }