Dave Lu / Mbed 2 deprecated FYDP

IMU and knee angle. No servo yet

Dependencies:   mbed

Fork of FYDP_Final2 by Dave Lu

main.cpp@4:05484073a641, 2015-03-22 (annotated)

Committer:
majik
Date:
Sun Mar 22 06:34:30 2015 +0000
Revision:
4:05484073a641
Parent:
3:47461d37adfb
Child:
5:d2e955a94940
BOTH IMUs WORK NOW. Put them in separate threads. Servo is included.

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 3:47461d37adfb 8 RtosTimer *MotorCMDTimer;
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 3:47461d37adfb 34
majik 3:47461d37adfb 35 void moveMotors(int i, int j, int k)
majik 3:47461d37adfb 36 {
majik 3:47461d37adfb 37 //This function does nothing. LEAVE BLANK
majik 3:47461d37adfb 38 //myservo = 1;
majik 3:47461d37adfb 39 Thread::yield();
majik 3:47461d37adfb 40 }
majik 3:47461d37adfb 41
majik 3:47461d37adfb 42 void motor_t(void const *args)
majik 0:21019d94ad33 43 {
majik 3:47461d37adfb 44
majik 3:47461d37adfb 45 double angle = 1;
majik 3:47461d37adfb 46 while(1){
majik 3:47461d37adfb 47 //move the servo so it reflects the IMU ROLL
majik 3:47461d37adfb 48 if (((t.read()- prev)*1000) > write_rate){
majik 3:47461d37adfb 49 angle = imu_data.ypr[1]/90;
majik 3:47461d37adfb 50 myservo = angle;
majik 3:47461d37adfb 51 //angle = angle - 0.01;
majik 3:47461d37adfb 52 //if (angle < 0)
majik 3:47461d37adfb 53 // angle = 1;
majik 3:47461d37adfb 54 prev = t.read();
majik 3:47461d37adfb 55 }
majik 3:47461d37adfb 56 Thread::wait(10);
majik 3:47461d37adfb 57 }
majik 0:21019d94ad33 58 }
majik 0:21019d94ad33 59
majik 0:21019d94ad33 60 void stopMotors(void const *args)
majik 0:21019d94ad33 61 {
majik 0:21019d94ad33 62 *motors.Left = 0;
majik 0:21019d94ad33 63 *motors.Right = 0;
majik 0:21019d94ad33 64 send_flag = 0;
majik 0:21019d94ad33 65 }
majik 0:21019d94ad33 66
majik 0:21019d94ad33 67 void CURRENT_thread(void const *args)
majik 0:21019d94ad33 68 {
majik 0:21019d94ad33 69 float current;
majik 0:21019d94ad33 70 while(1){
majik 0:21019d94ad33 71 current = current_sensor.get_current();
majik 0:21019d94ad33 72 bt.lock();
majik 0:21019d94ad33 73 bt.printf("\n\rCurrent: %f mA",current);
majik 0:21019d94ad33 74 bt.unlock();
majik 0:21019d94ad33 75 Thread::wait(50);
majik 0:21019d94ad33 76 }
majik 0:21019d94ad33 77 }
majik 0:21019d94ad33 78
majik 0:21019d94ad33 79 void optflow_thread(void const *args)
majik 0:21019d94ad33 80 {
majik 0:21019d94ad33 81 while(true) {
majik 0:21019d94ad33 82 reckon.updateOpticalFlow();
majik 0:21019d94ad33 83 Thread::wait(3);
majik 0:21019d94ad33 84 }
majik 0:21019d94ad33 85 }
majik 4:05484073a641 86
majik 1:815f16490da8 87 void IMU2_thread(void const *args)
majik 1:815f16490da8 88 {
majik 3:47461d37adfb 89 //This thread does not work
majik 1:815f16490da8 90 test_dmp2();
majik 1:815f16490da8 91 start_dmp2(mpu2);
majik 1:815f16490da8 92 while(1)
majik 1:815f16490da8 93 {
majik 1:815f16490da8 94 update_dmp2();
majik 1:815f16490da8 95 Thread::yield();
majik 1:815f16490da8 96 }
majik 4:05484073a641 97 }
majik 0:21019d94ad33 98 void IMU_thread(void const *args)
majik 0:21019d94ad33 99 {
majik 3:47461d37adfb 100 //For some reason, using 2 IMUs causes one of them to be laggy and
majik 3:47461d37adfb 101 //the connection fails after a while (buffer overflow??)
majik 3:47461d37adfb 102 bt.baud(BT_BAUD_RATE); //you have to do this for some reason
majik 3:47461d37adfb 103 test_dmp(); //test IMU1
majik 3:47461d37adfb 104 //test_dmp2(); //test IMU2
majik 0:21019d94ad33 105 bt.baud(BT_BAUD_RATE); //you have to do this for some reason
majik 0:21019d94ad33 106 start_dmp(mpu);
majik 3:47461d37adfb 107 // start_dmp2(mpu2);
majik 3:47461d37adfb 108
majik 0:21019d94ad33 109 while(1) {
majik 3:47461d37adfb 110 update_dmp();
majik 3:47461d37adfb 111 //update_dmp2();
majik 3:47461d37adfb 112 //mpuInterrupt = false; //this resets the interrupt flag
majik 3:47461d37adfb 113 //mpuInterrupt2 = false;
majik 2:7b3822eacad8 114 //Thread::wait(10);
majik 0:21019d94ad33 115 Thread::yield();
majik 0:21019d94ad33 116 }
majik 0:21019d94ad33 117 }
majik 0:21019d94ad33 118
majik 0:21019d94ad33 119 void bt_shell(void const *args)
majik 0:21019d94ad33 120 {
majik 0:21019d94ad33 121 bt_shell_init();
majik 0:21019d94ad33 122 bt.printf("BT shell initialized\r\n");
majik 0:21019d94ad33 123
majik 0:21019d94ad33 124 while(true) {
majik 0:21019d94ad33 125 bt_shell_run();
majik 2:7b3822eacad8 126 Thread::wait(10);
majik 2:7b3822eacad8 127 //Thread::yield();
majik 0:21019d94ad33 128 }
majik 0:21019d94ad33 129 }
majik 0:21019d94ad33 130
majik 0:21019d94ad33 131 int main()
majik 0:21019d94ad33 132 {
majik 0:21019d94ad33 133 initRobot();
majik 0:21019d94ad33 134
majik 3:47461d37adfb 135 //MotorCMDTimer = new RtosTimer(&stopMotors, osTimerOnce, NULL);
majik 3:47461d37adfb 136 //HalfHourTimer = new RtosTimer(&HalfHourUpdate, osTimerPeriodic, NULL);
majik 0:21019d94ad33 137
majik 3:47461d37adfb 138 //HalfHourTimer->start(1800000); //doesn't work because the timer takes over (high priority)
majik 0:21019d94ad33 139
majik 0:21019d94ad33 140 //OpticalFlowTimer = new RtosTimer(&update_opt, osTimerPeriodic, NULL);
majik 0:21019d94ad33 141 //OpticalFlowTimer->start(10); //doesn't work because the timer takes over (high priority)
majik 0:21019d94ad33 142
majik 0:21019d94ad33 143
majik 0:21019d94ad33 144 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 145
majik 0:21019d94ad33 146 Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL);
majik 0:21019d94ad33 147
majik 4:05484073a641 148 Thread IMU2_th(IMU2_thread,NULL,osPriorityNormal, 2048,NULL); //Putting IMU2 in a separate thread does not work
majik 1:815f16490da8 149
majik 0:21019d94ad33 150 // 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 151
majik 3:47461d37adfb 152 Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL);
majik 0:21019d94ad33 153
majik 0:21019d94ad33 154 //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL);
majik 0:21019d94ad33 155
majik 0:21019d94ad33 156 Thread::wait(osWaitForever);
majik 0:21019d94ad33 157 }