Dave Lu
/
FYDP
IMU and knee angle. No servo yet
Fork of FYDP_Final2 by
Diff: main.cpp
- Revision:
- 6:b3baf0fe5b73
- Parent:
- 5:d2e955a94940
- Child:
- 9:7a8fb72f9a93
--- a/main.cpp Wed Mar 25 09:58:50 2015 +0000 +++ b/main.cpp Wed Mar 25 11:40:32 2015 +0000 @@ -1,69 +1,93 @@ +#include "mbed.h" +#include "rtos.h" #include "robot.h" -#include "bt_shell.h" +#include "DMP.h" + +Serial pc(PTA2,PTA1); +DigitalOut btSwitch(PTE30); + +void print_all() //call this function to print all sensor data +{ + float data[13]; + data[0]= getTime(); + data[1]= imu_data.ypr[0]; + data[2]= imu_data.ypr[1]; + data[3]= imu_data.ypr[2]; + + data[4]= imu2_data.ypr[0]; + data[5]= imu2_data.ypr[1]; + data[6]= imu2_data.ypr[2]; + pc.printf("&;"); + pc.printf("%f;",data[0]); //print time as an integer (milliseconds) + for(int i = 1; i<=6; i++) { + pc.printf("%.3f;",data[i]); + } + pc.printf("\n\r"); + //commented out the below code because we want to ignore the rear IR sensors + /*dt = t.read() - data[0]; + irBack = 1; + if(dt < 50) + Thread::wait(50-dt); + data[6]= irR.read(); + data[7]= irL.read(); + data[8]= imu_data.ypr[0]*180/PI; + irBack = 0; + */ + +} void motor_t(void const *args) -{ - double angle = 1; - while(1){ - //move the servo so it reflects the IMU ROLL - if (((t.read()- prev)*1000) > write_rate){ - angle = imu_data.ypr[1]/90; - myservo = angle; - //angle = angle - 0.01; - //if (angle < 0) - // angle = 1; - prev = t.read(); - } - Thread::wait(10); - } +{ +// myservo = 0; } void IMU2_thread(void const *args) { test_dmp2(); start_dmp2(mpu2); - while(1) - { + while(1) { update_dmp2(); - Thread::yield(); + Thread::yield(); } } void IMU_thread(void const *args) { - bt.baud(BT_BAUD_RATE); test_dmp(); - bt.baud(BT_BAUD_RATE); start_dmp(mpu); - + while(1) { update_dmp(); - Thread::yield(); + Thread::yield(); } } void bt_shell(void const *args) { - bt_shell_init(); - bt.printf("BT shell initialized\r\n"); - + btSwitch = 1; + pc.baud(115200); + + int command = 0; while(true) { - bt_shell_run(); - Thread::wait(10); - } +// if(pc.readable()) { +// pc.scanf("%d;", &command); +// } +// +// pc.printf("%.3f;%d\r\n", getTime(), command); + print_all(); + Thread::wait(50); + } } int main() { - initRobot(); + t.start(); Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL); - - Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL); - - Thread IMU2_th(IMU2_thread,NULL,osPriorityNormal, 2048,NULL); - Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL); - + Thread IMU_th(IMU_thread, NULL, osPriorityNormal, 2048, NULL); + Thread IMU2_th(IMU2_thread, NULL, osPriorityNormal, 2048, NULL); + Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL); + Thread::wait(osWaitForever); } \ No newline at end of file