Julio Fajardo / Mbed 2 deprecated FRDM_EMG_Galileo_Hand

Hybrid sEMG + IMU activated controller for Galileo Bionic Hand Prosthesis

Dependencies:   FXAS21000 FXOS8700Q kalman mbed-dsp mbed-rtos mbed

main.cpp@0:2245e978868c, 2015-10-16 (annotated)

Committer:
julioefajardo
Date:
Fri Oct 16 21:29:22 2015 +0000
Revision:
0:2245e978868c
Child:
1:84347af5a1f2
Version 1 - First Test; RTOS; Threads; Mutex; EMG sampler; IMU sampler; Kalman filter; Serial Communication;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
julioefajardo 0:2245e978868c 1 #include "mbed.h"
julioefajardo 0:2245e978868c 2 #include "FXOS8700Q.h"
julioefajardo 0:2245e978868c 3 #include "FXAS21000.h"
julioefajardo 0:2245e978868c 4 #include "rtos.h"
julioefajardo 0:2245e978868c 5 #include "kalman.c"
julioefajardo 0:2245e978868c 6 #include "arm_math.h"
julioefajardo 0:2245e978868c 7
julioefajardo 0:2245e978868c 8 #define TRUE 1
julioefajardo 0:2245e978868c 9 #define FALSE 0
julioefajardo 0:2245e978868c 10 #define RAW 0
julioefajardo 0:2245e978868c 11 #define RECTIFIED 1
julioefajardo 0:2245e978868c 12 #define SMOOTH 2
julioefajardo 0:2245e978868c 13 #define THRESHOLDF 0.25f
julioefajardo 0:2245e978868c 14 #define THRESHOLDE 0.25f
julioefajardo 0:2245e978868c 15 #define FLEXION2 0.0f
julioefajardo 0:2245e978868c 16 #define Rad2Dree 57.295779513082320876798154814105f
julioefajardo 0:2245e978868c 17
julioefajardo 0:2245e978868c 18 Serial pc(USBTX, USBRX);
julioefajardo 0:2245e978868c 19 AnalogIn Ref(A0);
julioefajardo 0:2245e978868c 20 AnalogIn E1(A1);
julioefajardo 0:2245e978868c 21 AnalogIn E2(A2);
julioefajardo 0:2245e978868c 22 AnalogIn E3(A3);
julioefajardo 0:2245e978868c 23 FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR1);
julioefajardo 0:2245e978868c 24 FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR1);
julioefajardo 0:2245e978868c 25 FXAS21000 gyro(D14, D15);
julioefajardo 0:2245e978868c 26 DigitalOut led1(LED1);
julioefajardo 0:2245e978868c 27 InterruptIn sw2(SW2);
julioefajardo 0:2245e978868c 28 Timer GlobalTime;
julioefajardo 0:2245e978868c 29 Timer ProgramTimer;
julioefajardo 0:2245e978868c 30
julioefajardo 0:2245e978868c 31 //Threadas
julioefajardo 0:2245e978868c 32 uint32_t button_pressed;
julioefajardo 0:2245e978868c 33 Thread *thread2;
julioefajardo 0:2245e978868c 34 Thread *thread3;
julioefajardo 0:2245e978868c 35 Thread *thread4;
julioefajardo 0:2245e978868c 36
julioefajardo 0:2245e978868c 37 //EMG samples and DSP variables
julioefajardo 0:2245e978868c 38 float32_t EMG1, EMG2, EMG3;
julioefajardo 0:2245e978868c 39 float32_t samples[25];
julioefajardo 0:2245e978868c 40 float32_t samples2[25];
julioefajardo 0:2245e978868c 41 float32_t samples3[25];
julioefajardo 0:2245e978868c 42 float32_t abs_output[25];
julioefajardo 0:2245e978868c 43 float32_t abs_output2[25];
julioefajardo 0:2245e978868c 44 float32_t abs_output3[25];
julioefajardo 0:2245e978868c 45 float32_t mean = 0.0f;
julioefajardo 0:2245e978868c 46 float32_t mean2 = 0.0f;
julioefajardo 0:2245e978868c 47 float32_t mean3 = 0.0f;
julioefajardo 0:2245e978868c 48
julioefajardo 0:2245e978868c 49 //IMU
julioefajardo 0:2245e978868c 50 float gyro_data[3];
julioefajardo 0:2245e978868c 51 MotionSensorDataUnits adata;
julioefajardo 0:2245e978868c 52 MotionSensorDataUnits mdata;
julioefajardo 0:2245e978868c 53 float R;
julioefajardo 0:2245e978868c 54 float angle[3];
julioefajardo 0:2245e978868c 55 float roll;
julioefajardo 0:2245e978868c 56 float pitch;
julioefajardo 0:2245e978868c 57 kalman filter_pitch;
julioefajardo 0:2245e978868c 58 kalman filter_roll;
julioefajardo 0:2245e978868c 59
julioefajardo 0:2245e978868c 60 //OS
julioefajardo 0:2245e978868c 61 osMutexId el_mutex;
julioefajardo 0:2245e978868c 62 osMutexDef(el_mutex);
julioefajardo 0:2245e978868c 63
julioefajardo 0:2245e978868c 64 //time
julioefajardo 0:2245e978868c 65 unsigned long timer;
julioefajardo 0:2245e978868c 66 long loopStartTime;
julioefajardo 0:2245e978868c 67
julioefajardo 0:2245e978868c 68 void sw2_press(void);
julioefajardo 0:2245e978868c 69 void button_thread(void const *argument);
julioefajardo 0:2245e978868c 70 void imu_thread(void const *argument);
julioefajardo 0:2245e978868c 71 void sampler_thread(void const *argument);
julioefajardo 0:2245e978868c 72 void logger_thread(void const *argument);
julioefajardo 0:2245e978868c 73
julioefajardo 0:2245e978868c 74 void Serial_Oscilloscope(uint8_t activation, uint8_t type);
julioefajardo 0:2245e978868c 75
julioefajardo 0:2245e978868c 76 int main(void)
julioefajardo 0:2245e978868c 77 {
julioefajardo 0:2245e978868c 78 pc.baud(115200); //Serial com at 115200 bauds
julioefajardo 0:2245e978868c 79 Thread thread(imu_thread);
julioefajardo 0:2245e978868c 80 thread2 = new Thread(button_thread);
julioefajardo 0:2245e978868c 81 thread3 = new Thread(sampler_thread);
julioefajardo 0:2245e978868c 82 thread4 = new Thread(logger_thread);
julioefajardo 0:2245e978868c 83 el_mutex = osMutexCreate(osMutex(el_mutex));
julioefajardo 0:2245e978868c 84 GlobalTime.start();
julioefajardo 0:2245e978868c 85
julioefajardo 0:2245e978868c 86 combo_acc.enable();
julioefajardo 0:2245e978868c 87 combo_mag.enable();
julioefajardo 0:2245e978868c 88 pc.printf("FXOS8700 Combo = %X\r\n", combo_acc.whoAmI());
julioefajardo 0:2245e978868c 89 pc.printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI());
julioefajardo 0:2245e978868c 90
julioefajardo 0:2245e978868c 91 kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 0:2245e978868c 92 kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 0:2245e978868c 93
julioefajardo 0:2245e978868c 94 button_pressed = 0;
julioefajardo 0:2245e978868c 95 sw2.fall(&sw2_press);
julioefajardo 0:2245e978868c 96
julioefajardo 0:2245e978868c 97 ProgramTimer.start();
julioefajardo 0:2245e978868c 98 loopStartTime = ProgramTimer.read_us();
julioefajardo 0:2245e978868c 99 timer = loopStartTime;
julioefajardo 0:2245e978868c 100
julioefajardo 0:2245e978868c 101 while (true) {
julioefajardo 0:2245e978868c 102 Thread::wait(5000);
julioefajardo 0:2245e978868c 103 led1 = !led1;
julioefajardo 0:2245e978868c 104 pc.printf("SW2 was pressed (last 5 seconds): %d \n\r", button_pressed);
julioefajardo 0:2245e978868c 105 pc.printf("Roll Angle X: %.6f Pitch Angle Y: %.6f \r\n", Rad2Dree * angle[1], Rad2Dree * angle[0]);
julioefajardo 0:2245e978868c 106 fflush(stdout);
julioefajardo 0:2245e978868c 107 button_pressed = 0;
julioefajardo 0:2245e978868c 108 }
julioefajardo 0:2245e978868c 109 }
julioefajardo 0:2245e978868c 110
julioefajardo 0:2245e978868c 111 void sw2_press(void){
julioefajardo 0:2245e978868c 112 thread2->signal_set(0x1);
julioefajardo 0:2245e978868c 113 }
julioefajardo 0:2245e978868c 114
julioefajardo 0:2245e978868c 115 void button_thread(void const *argument){
julioefajardo 0:2245e978868c 116 while (true) {
julioefajardo 0:2245e978868c 117 Thread::signal_wait(0x1);
julioefajardo 0:2245e978868c 118 button_pressed++;
julioefajardo 0:2245e978868c 119 }
julioefajardo 0:2245e978868c 120 }
julioefajardo 0:2245e978868c 121
julioefajardo 0:2245e978868c 122 void imu_thread(void const *argument){
julioefajardo 0:2245e978868c 123 while (true) {
julioefajardo 0:2245e978868c 124
julioefajardo 0:2245e978868c 125 combo_acc.getAxis(adata); //pc.printf("FXOS8700 Acc: X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
julioefajardo 0:2245e978868c 126 combo_mag.getAxis(mdata); //pc.printf("FXOS8700 Mag: X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
julioefajardo 0:2245e978868c 127 gyro.ReadXYZ(gyro_data); //pc.printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
julioefajardo 0:2245e978868c 128 R = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
julioefajardo 0:2245e978868c 129
julioefajardo 0:2245e978868c 130 kalman_predict(&filter_pitch, gyro_data[0], (ProgramTimer.read_us() - timer));
julioefajardo 0:2245e978868c 131 kalman_update(&filter_pitch, acos(adata.x/R));
julioefajardo 0:2245e978868c 132 kalman_predict(&filter_roll, gyro_data[1], (ProgramTimer.read_us() - timer));
julioefajardo 0:2245e978868c 133 kalman_update(&filter_roll, acos(adata.y/R));
julioefajardo 0:2245e978868c 134
julioefajardo 0:2245e978868c 135 angle[0] = kalman_get_angle(&filter_pitch);
julioefajardo 0:2245e978868c 136 angle[1] = kalman_get_angle(&filter_roll);
julioefajardo 0:2245e978868c 137 Thread::wait(10);
julioefajardo 0:2245e978868c 138 }
julioefajardo 0:2245e978868c 139 }
julioefajardo 0:2245e978868c 140
julioefajardo 0:2245e978868c 141 void sampler_thread(void const *argument){
julioefajardo 0:2245e978868c 142 while (true) {
julioefajardo 0:2245e978868c 143 EMG1 = (E1.read()-Ref.read())*3.3f;
julioefajardo 0:2245e978868c 144 EMG2 = (E2.read()-Ref.read())*3.3f;
julioefajardo 0:2245e978868c 145 EMG3 = (E3.read()-Ref.read())*3.3f;
julioefajardo 0:2245e978868c 146 //osMutexWait(el_mutex, osWaitForever);
julioefajardo 0:2245e978868c 147 for(int j=24;j>0;j--) {
julioefajardo 0:2245e978868c 148 samples[j]=samples[j-1]; //Fill Array
julioefajardo 0:2245e978868c 149 samples2[j]=samples2[j-1]; //Fill Array
julioefajardo 0:2245e978868c 150 samples3[j]=samples3[j-1]; //Fill Array
julioefajardo 0:2245e978868c 151 }
julioefajardo 0:2245e978868c 152 samples[0]=EMG1;
julioefajardo 0:2245e978868c 153 samples2[0]=EMG2;
julioefajardo 0:2245e978868c 154 samples3[0]=EMG3;
julioefajardo 0:2245e978868c 155 //osMutexRelease(el_mutex);
julioefajardo 0:2245e978868c 156 Thread::wait(1);
julioefajardo 0:2245e978868c 157 }
julioefajardo 0:2245e978868c 158 }
julioefajardo 0:2245e978868c 159
julioefajardo 0:2245e978868c 160
julioefajardo 0:2245e978868c 161 void logger_thread(void const *argument){
julioefajardo 0:2245e978868c 162 while (true) {
julioefajardo 0:2245e978868c 163 Thread::wait(50);
julioefajardo 0:2245e978868c 164 }
julioefajardo 0:2245e978868c 165 }
julioefajardo 0:2245e978868c 166
julioefajardo 0:2245e978868c 167 void Serial_Oscilloscope(uint8_t activation, uint8_t type){
julioefajardo 0:2245e978868c 168 if (activation){
julioefajardo 0:2245e978868c 169 switch(type){
julioefajardo 0:2245e978868c 170 case RAW: pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3); break;
julioefajardo 0:2245e978868c 171 case RECTIFIED: pc.printf("%.10f,%.10f,%.10f\n\r",abs_output[0],abs_output2[0],abs_output3[0]);break;
julioefajardo 0:2245e978868c 172 case SMOOTH: pc.printf("%.10f,%.10f,%.10f\n\r",mean,mean2,mean3); break;
julioefajardo 0:2245e978868c 173 default: pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3);
julioefajardo 0:2245e978868c 174 }
julioefajardo 0:2245e978868c 175 }
julioefajardo 0:2245e978868c 176 }