Mark Vandermeulen / FYDP_Final2

This is for our FYDP project. 2 MPU6050s are used

Dependencies:   Servo mbed

main.cpp@0:21019d94ad33, 2015-03-21 (annotated)

Committer:
majik
Date:
Sat Mar 21 21:31:29 2015 +0000
Revision:
0:21019d94ad33
Child:
1:815f16490da8
Both IMUs work now

Who changed what in which revision?

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