Dave Lu / Mbed 2 deprecated FYDP

IMU and knee angle. No servo yet

Dependencies:   mbed

Fork of FYDP_Final2 by Dave Lu

main.cpp@11:425dff6a4af9, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 18:11:09 2015 +0000
Revision:
11:425dff6a4af9
Parent:
10:22c44650c7c1 
Working

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 9:7a8fb72f9a93 5 #include "Sensor.hpp"
tntmarket 9:7a8fb72f9a93 6 #include "control.hpp"
tntmarket 9:7a8fb72f9a93 7 #include "io.h"
majik 0:21019d94ad33 8
tntmarket 9:7a8fb72f9a93 9 Sensor sensor;
tntmarket 9:7a8fb72f9a93 10 DigitalOut btSwitch(PTE30);
majik 3:47461d37adfb 11
tntmarket 11:425dff6a4af9 12
majik 0:21019d94ad33 13 void IMU_thread(void const *args)
majik 0:21019d94ad33 14 {
tntmarket 5:d2e955a94940 15 test_dmp();
majik 0:21019d94ad33 16 start_dmp(mpu);
tntmarket 11:425dff6a4af9 17 while(true) {
tntmarket 11:425dff6a4af9 18 LED_ON
tntmarket 11:425dff6a4af9 19 update_dmp();
tntmarket 11:425dff6a4af9 20 Thread::yield();
tntmarket 11:425dff6a4af9 21 }
tntmarket 11:425dff6a4af9 22 }
tntmarket 11:425dff6a4af9 23 void IMU2_thread(void const *args)
tntmarket 11:425dff6a4af9 24 {
tntmarket 10:22c44650c7c1 25 test_dmp2();
tntmarket 10:22c44650c7c1 26 start_dmp2(mpu2);
tntmarket 11:425dff6a4af9 27 while(true) {
tntmarket 11:425dff6a4af9 28 LED_OFF
tntmarket 10:22c44650c7c1 29 update_dmp2();
tntmarket 6:b3baf0fe5b73 30 Thread::yield();
majik 0:21019d94ad33 31 }
majik 0:21019d94ad33 32 }
tntmarket 9:7a8fb72f9a93 33 void io_thread(void const *args)
majik 0:21019d94ad33 34 {
majik 0:21019d94ad33 35 while(true) {
tntmarket 9:7a8fb72f9a93 36 if(pc.readable() && pc.getc() == 'c') {
tntmarket 9:7a8fb72f9a93 37 sensor.calibrate();
tntmarket 9:7a8fb72f9a93 38 setStateToStop();
tntmarket 9:7a8fb72f9a93 39 }
tntmarket 9:7a8fb72f9a93 40
tntmarket 9:7a8fb72f9a93 41 process();
tntmarket 9:7a8fb72f9a93 42
tntmarket 11:425dff6a4af9 43 pc.printf(
tntmarket 11:425dff6a4af9 44 "%.3f;%.3f;%.3f;%i;%.3f;%.3f;%.3f;\r\n",
tntmarket 11:425dff6a4af9 45 sensor.time(), sensor.thigh(), sensor.shin(), getState(), getPace(), getHip(), getKneeDiff()
tntmarket 11:425dff6a4af9 46 );
tntmarket 11:425dff6a4af9 47
tntmarket 9:7a8fb72f9a93 48 Thread::wait(10);
tntmarket 6:b3baf0fe5b73 49 }
majik 0:21019d94ad33 50 }
majik 0:21019d94ad33 51
tntmarket 10:22c44650c7c1 52 //void control_thread(void const *args)
tntmarket 10:22c44650c7c1 53 //{
tntmarket 10:22c44650c7c1 54 // while(true) {
tntmarket 10:22c44650c7c1 55 // process();
tntmarket 10:22c44650c7c1 56 // Thread::wait(10);
tntmarket 10:22c44650c7c1 57 // }
tntmarket 10:22c44650c7c1 58 //}
tntmarket 10:22c44650c7c1 59
majik 0:21019d94ad33 60 int main()
majik 0:21019d94ad33 61 {
tntmarket 6:b3baf0fe5b73 62 t.start();
tntmarket 11:425dff6a4af9 63
tntmarket 11:425dff6a4af9 64 btSwitch = 1;
tntmarket 11:425dff6a4af9 65 pc.baud(115200);
tntmarket 11:425dff6a4af9 66
tntmarket 11:425dff6a4af9 67 setup(&sensor);
majik 0:21019d94ad33 68
tntmarket 11:425dff6a4af9 69 Thread bt_shell_th(io_thread, NULL, osPriorityNormal, 1024, NULL);
tntmarket 11:425dff6a4af9 70 Thread IMU_th(IMU_thread, NULL, osPriorityNormal, 768, NULL);
tntmarket 11:425dff6a4af9 71 Thread IMU2_th(IMU2_thread, NULL, osPriorityNormal, 768, NULL);
tntmarket 10:22c44650c7c1 72 // Thread control_th(control_thread, NULL, osPriorityNormal, 2548, NULL);
tntmarket 6:b3baf0fe5b73 73
majik 0:21019d94ad33 74 Thread::wait(osWaitForever);
majik 0:21019d94ad33 75 }