Dave Lu
/
FYDP
IMU and knee angle. No servo yet
Fork of FYDP_Final2 by
main.cpp
- Committer:
- tntmarket
- Date:
- 2015-03-25
- Revision:
- 5:d2e955a94940
- Parent:
- 4:05484073a641
- Child:
- 6:b3baf0fe5b73
File content as of revision 5:d2e955a94940:
#include "robot.h" #include "bt_shell.h" 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); } } void IMU2_thread(void const *args) { test_dmp2(); start_dmp2(mpu2); while(1) { update_dmp2(); 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(); } } void bt_shell(void const *args) { bt_shell_init(); bt.printf("BT shell initialized\r\n"); while(true) { bt_shell_run(); Thread::wait(10); } } int main() { initRobot(); 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::wait(osWaitForever); }