Dave Lu / Mbed 2 deprecated FYDP

IMU and knee angle. No servo yet

Dependencies:   mbed

Fork of FYDP_Final2 by Dave Lu

main.cpp@9:7a8fb72f9a93, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 14:34:23 2015 +0000
Revision:
9:7a8fb72f9a93
Parent:
6:b3baf0fe5b73
Child:
10:22c44650c7c1
Working except for actuator

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