Dave Lu
/
FYDP
IMU and knee angle. No servo yet
Fork of FYDP_Final2 by
Diff: main.cpp
- Revision:
- 9:7a8fb72f9a93
- Parent:
- 6:b3baf0fe5b73
- Child:
- 10:22c44650c7c1
diff -r 3d5a84b897be -r 7a8fb72f9a93 main.cpp --- a/main.cpp Wed Mar 25 11:44:05 2015 +0000 +++ b/main.cpp Wed Mar 25 14:34:23 2015 +0000 @@ -2,41 +2,12 @@ #include "rtos.h" #include "robot.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]; +#include "Sensor.hpp" +#include "control.hpp" +#include "io.h" - 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; - */ - -} +Sensor sensor; +DigitalOut btSwitch(PTE30); void motor_t(void const *args) { @@ -63,20 +34,25 @@ } } -void bt_shell(void const *args) +void io_thread(void const *args) { btSwitch = 1; pc.baud(115200); + setup(sensor); - int command = 0; while(true) { -// if(pc.readable()) { -// pc.scanf("%d;", &command); -// } -// -// pc.printf("%.3f;%d\r\n", getTime(), command); - print_all(); - Thread::wait(50); + if(pc.readable() && pc.getc() == 'c') { + sensor.calibrate(); + setStateToStop(); + } + + process(); + + pc.printf( + "%.3f;%.3f;%.3f;%i;%.3f;%.3f;%.3f;\r\n", + sensor.time(), sensor.thigh(), sensor.shin(), getState(), getPace(), 0, 0 + ); + Thread::wait(10); } } @@ -84,7 +60,7 @@ { t.start(); - Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL); + Thread bt_shell_th(io_thread, 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);