Dave Lu / Mbed 2 deprecated FYDP

IMU and knee angle. No servo yet

Dependencies:   mbed

Fork of FYDP_Final2 by Dave Lu

main.cpp@6:b3baf0fe5b73, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 11:40:32 2015 +0000
Revision:
6:b3baf0fe5b73
Parent:
5:d2e955a94940
Child:
9:7a8fb72f9a93
Kill dead code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tntmarket 6:b3baf0fe5b73 1 #include "mbed.h"
tntmarket 6:b3baf0fe5b73 2 #include "rtos.h"
majik 0:21019d94ad33 3 #include "robot.h"
tntmarket 6:b3baf0fe5b73 4 #include "DMP.h"
tntmarket 6:b3baf0fe5b73 5
tntmarket 6:b3baf0fe5b73 6 Serial pc(PTA2,PTA1);
tntmarket 6:b3baf0fe5b73 7 DigitalOut btSwitch(PTE30);
tntmarket 6:b3baf0fe5b73 8
tntmarket 6:b3baf0fe5b73 9 void print_all() //call this function to print all sensor data
tntmarket 6:b3baf0fe5b73 10 {
tntmarket 6:b3baf0fe5b73 11 float data[13];
tntmarket 6:b3baf0fe5b73 12 data[0]= getTime();
tntmarket 6:b3baf0fe5b73 13 data[1]= imu_data.ypr[0];
tntmarket 6:b3baf0fe5b73 14 data[2]= imu_data.ypr[1];
tntmarket 6:b3baf0fe5b73 15 data[3]= imu_data.ypr[2];
tntmarket 6:b3baf0fe5b73 16
tntmarket 6:b3baf0fe5b73 17 data[4]= imu2_data.ypr[0];
tntmarket 6:b3baf0fe5b73 18 data[5]= imu2_data.ypr[1];
tntmarket 6:b3baf0fe5b73 19 data[6]= imu2_data.ypr[2];
majik 0:21019d94ad33 20
tntmarket 6:b3baf0fe5b73 21 pc.printf("&;");
tntmarket 6:b3baf0fe5b73 22 pc.printf("%f;",data[0]); //print time as an integer (milliseconds)
tntmarket 6:b3baf0fe5b73 23 for(int i = 1; i<=6; i++) {
tntmarket 6:b3baf0fe5b73 24 pc.printf("%.3f;",data[i]);
tntmarket 6:b3baf0fe5b73 25 }
tntmarket 6:b3baf0fe5b73 26 pc.printf("\n\r");
tntmarket 6:b3baf0fe5b73 27 //commented out the below code because we want to ignore the rear IR sensors
tntmarket 6:b3baf0fe5b73 28 /*dt = t.read() - data[0];
tntmarket 6:b3baf0fe5b73 29 irBack = 1;
tntmarket 6:b3baf0fe5b73 30 if(dt < 50)
tntmarket 6:b3baf0fe5b73 31 Thread::wait(50-dt);
majik 0:21019d94ad33 32
tntmarket 6:b3baf0fe5b73 33 data[6]= irR.read();
tntmarket 6:b3baf0fe5b73 34 data[7]= irL.read();
tntmarket 6:b3baf0fe5b73 35 data[8]= imu_data.ypr[0]*180/PI;
tntmarket 6:b3baf0fe5b73 36 irBack = 0;
tntmarket 6:b3baf0fe5b73 37 */
tntmarket 6:b3baf0fe5b73 38
tntmarket 6:b3baf0fe5b73 39 }
majik 3:47461d37adfb 40
majik 3:47461d37adfb 41 void motor_t(void const *args)
tntmarket 6:b3baf0fe5b73 42 {
tntmarket 6:b3baf0fe5b73 43 // myservo = 0;
majik 0:21019d94ad33 44 }
majik 0:21019d94ad33 45
majik 1:815f16490da8 46 void IMU2_thread(void const *args)
majik 1:815f16490da8 47 {
majik 1:815f16490da8 48 test_dmp2();
majik 1:815f16490da8 49 start_dmp2(mpu2);
tntmarket 6:b3baf0fe5b73 50 while(1) {
majik 1:815f16490da8 51 update_dmp2();
tntmarket 6:b3baf0fe5b73 52 Thread::yield();
majik 1:815f16490da8 53 }
majik 4:05484073a641 54 }
majik 0:21019d94ad33 55 void IMU_thread(void const *args)
majik 0:21019d94ad33 56 {
tntmarket 5:d2e955a94940 57 test_dmp();
majik 0:21019d94ad33 58 start_dmp(mpu);
tntmarket 6:b3baf0fe5b73 59
majik 0:21019d94ad33 60 while(1) {
majik 3:47461d37adfb 61 update_dmp();
tntmarket 6:b3baf0fe5b73 62 Thread::yield();
majik 0:21019d94ad33 63 }
majik 0:21019d94ad33 64 }
majik 0:21019d94ad33 65
majik 0:21019d94ad33 66 void bt_shell(void const *args)
majik 0:21019d94ad33 67 {
tntmarket 6:b3baf0fe5b73 68 btSwitch = 1;
tntmarket 6:b3baf0fe5b73 69 pc.baud(115200);
tntmarket 6:b3baf0fe5b73 70
tntmarket 6:b3baf0fe5b73 71 int command = 0;
majik 0:21019d94ad33 72 while(true) {
tntmarket 6:b3baf0fe5b73 73 // if(pc.readable()) {
tntmarket 6:b3baf0fe5b73 74 // pc.scanf("%d;", &command);
tntmarket 6:b3baf0fe5b73 75 // }
tntmarket 6:b3baf0fe5b73 76 //
tntmarket 6:b3baf0fe5b73 77 // pc.printf("%.3f;%d\r\n", getTime(), command);
tntmarket 6:b3baf0fe5b73 78 print_all();
tntmarket 6:b3baf0fe5b73 79 Thread::wait(50);
tntmarket 6:b3baf0fe5b73 80 }
majik 0:21019d94ad33 81 }
majik 0:21019d94ad33 82
majik 0:21019d94ad33 83 int main()
majik 0:21019d94ad33 84 {
tntmarket 6:b3baf0fe5b73 85 t.start();
majik 0:21019d94ad33 86
tntmarket 5:d2e955a94940 87 Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL);
tntmarket 6:b3baf0fe5b73 88 Thread IMU_th(IMU_thread, NULL, osPriorityNormal, 2048, NULL);
tntmarket 6:b3baf0fe5b73 89 Thread IMU2_th(IMU2_thread, NULL, osPriorityNormal, 2048, NULL);
tntmarket 6:b3baf0fe5b73 90 Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL);
tntmarket 6:b3baf0fe5b73 91
majik 0:21019d94ad33 92 Thread::wait(osWaitForever);
majik 0:21019d94ad33 93 }