Dave Lu / Mbed 2 deprecated FYDP

IMU and knee angle. No servo yet

Dependencies:   mbed

Fork of FYDP_Final2 by Dave Lu

main.cpp@10:22c44650c7c1, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 16:36:11 2015 +0000
Revision:
10:22c44650c7c1
Parent:
9:7a8fb72f9a93
Child:
11:425dff6a4af9
running out of RAM?

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