Rong Syuan Lin
/
LSM9DS1_project_2
version2
Fork of LSM9DS1_project by
Diff: main.cpp
- Revision:
- 7:d421b158f1be
- Parent:
- 6:62643ad078c8
diff -r 62643ad078c8 -r d421b158f1be main.cpp --- a/main.cpp Fri Jun 08 14:16:45 2018 +0000 +++ b/main.cpp Sun Jul 22 09:11:00 2018 +0000 @@ -1,32 +1,68 @@ #include "mbed.h" #include "LSM9DS1.h" #include "AS5145.h" +#include "math.h" LSM9DS1 imu2(D5, D7); LSM9DS1 imu3(D3, D6); LSM9DS1 imu(D14, D15); -AnalogIn sEMG(D13); -AnalogIn sEMG2(D1); -AnalogIn sEMG3(D0); -AnalogIn sEMG4(PC_4); -Serial pc(USBTX, USBRX); +//AnalogIn sEMG(D13); +//AnalogIn sEMG2(D1); +//AnalogIn sEMG3(D0); +//AnalogIn sEMG4(PC_4); +Serial uart_1(USBTX, USBRX); +//Serial uart_1(D10,D2); // TX : D10 RX : D2 // serial 1 +AnalogIn FSR(A0); +AnalogIn EMG(A1); + +/********************************************************************/ +// timer +/********************************************************************/ +#define LOOPTIME 1000 // 1 ms Ticker timer1; -Ticker timer2; +Timer t; +float T = 0.001; -float T = 0.001; /********************************************************************/ //function declaration /********************************************************************/ void init_TIMER(void); void timer1_interrupt(void); +//void init_uart(void); +void separate(void); +void uart_send(void); void setup(void); void estimator(float axm[3],float aym[3],float azm[3],float w3[3],float w2[3],float w1[3],float alpha); float lpf(float input, float output_old, float frequency); void angle_fn(float x1_hat[3],float x2_hat[3]); void pitch_dot_fn(float w3[3],float w2[3],float w1[3],float sinroll[3],float cosroll[3]); void pitch_double_dot_fn(float pitch_dot[3],float pitch_dot_old[3]); + /********************************************************************/ -// sensor data +// uart send data +/********************************************************************/ +int display[20]; +char onebytedata[42] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; // char onebytedata[0] , onebytedata[1] : 2 start byte +unsigned long time_flag; +unsigned long uart_flag; +unsigned long Tstart; +unsigned long Tperiod; + +/********************************************************************/ +// FSR +/********************************************************************/ +float Voltage_FSR; + +/********************************************************************/ +// EMG +/********************************************************************/ +//sEMG variable +//float emg_value[4] = {0.0f}; +float Voltage_EMG = 0.0f; + + +/********************************************************************/ +// imu data /********************************************************************/ int16_t Gyro_axis_data[9] = {0}; // X, Y, Z axis int16_t Acce_axis_data[9] = {0}; // X, Y, Z axis @@ -72,43 +108,156 @@ float w2axm_f_old[3] = {0.0f}; float w1aym_f[3] = {0.0f}; float w1aym_f_old[3] = {0.0f}; -//sEMG variable -float emg_value[4] = {0.0f}; -/*int main() +/**********************************************************************************************************************************/ +int main() { - pc.baud(230400); + //Start uart + t.start(); + uart_flag = 0; + Tperiod = 0; + uart_1.baud(115200); + setup(); //Setup sensors AS5145_begin(); //begin encoder init_TIMER(); + while (1) { - //pc.printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d\n",pitch_angle[0],pitch_dot[0],pitch_angle[1],pitch_dot[1],pitch_angle[2],pitch_dot[2],emg_value[0],emg_value[1],emg_value[2],emg_value[3],position[1]*360/4096, position[0]*360/4096); + uart_send(); + //uart_1.printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d\n",pitch_angle[0],pitch_dot[0],pitch_angle[1],pitch_dot[1],pitch_angle[2],pitch_dot[2],emg_value[0],emg_value[1],emg_value[2],emg_value[3],position[1]*360/4096, position[0]*360/4096); wait(0.05); - //pc.printf("%f,%f,%f,%f\n",emg_value[0],emg_value[1],emg_value[2],emg_value[3]); - pc.printf("%f,%f,%f,%f,%f,%d,%d\n", pitch_angle[0], pitch_angle[1], roll_angle[1], pitch_angle[2], roll_angle[2], position[1]*360/4096, position[0]*360/4096); - //pc.printf("IMU: %2f,%2f\r\n", pitch_angle[0], roll_angle[0]); - //pc.printf("IMU2: %2f,%2f\r\n", pitch_angle[1], roll_angle[1]); - //pc.printf("IMU3: %2f,%2f\r\n", pitch_angle[2], roll_angle[2]); - //pc.printf("position: %d,%d\r\n", position[0], position[1]); - //pc.printf("roll_angle: %2f\r\n",roll_angle); - //pc.printf("A: %2f, %2f, %2f\r\n", imu2.ax*Acce_gain_x_2, imu2.ay*Acce_gain_y_2, imu2.az*Acce_gain_z_2); + //uart_1.printf("%f,%f,%f,%f\n",emg_value[0],emg_value[1],emg_value[2],emg_value[3]); + //uart_1.printf("%f,%f,%f,%f,%f,%d,%d\n", pitch_angle[0], pitch_angle[1], roll_angle[1], pitch_angle[2], roll_angle[2], position[1]*360/4096, position[0]*360/4096); + //uart_1.printf("IMU: %2f,%2f\r\n", pitch_angle[0], roll_angle[0]); + //uart_1.printf("IMU2: %2f,%2f\r\n", pitch_angle[1], roll_angle[1]); + //uart_1.printf("IMU3: %2f,%2f\r\n", pitch_angle[2], roll_angle[2]); + //uart_1.printf("position: %d,%d\r\n", position[0], position[1]); + //uart_1.printf("roll_angle: %2f\r\n",roll_angle); + //uart_1.printf("A: %2f, %2f, %2f\r\n", imu2.ax*Acce_gain_x_2, imu2.ay*Acce_gain_y_2, imu2.az*Acce_gain_z_2); } } + void setup() { imu.begin(); imu2.begin(); imu3.begin(); } + /********************************************************************/ // init_TIMER /********************************************************************/ -/*void init_TIMER(void) +void init_TIMER(void) { timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz) - timer2.attach_us(&ReadValue, 1000);//1ms interrupt period (1000 Hz) + timer1.attach_us(&ReadValue, 10000);//10ms interrupt period (100 Hz) +} + +/********************************************************************/ +// uart_send +/********************************************************************/ +int i = 0; //count one byte data +void uart_send(void) +{ + display[0] = ; + display[1] = ; + display[2] = ; + display[3] = ; + display[4] = ; + display[5] = ; + display[6] = ; + display[7] = ; + display[8] = ; + display[9] = ; + display[10] = ; + display[11] = ; + display[12] = ; + display[13] = ; + display[14] = ; + display[15] = ; + display[16] = ; + display[17] = ; + display[18] = Voltage_FSR; + display[19] = Voltage_EMG; + +// wait(0.2); + separate(); + + int j = 0; + int k = 1; + while(k) { + if(uart_1.writeable()) { + uart_1.putc(onebytedata[i]); + i++; + j++; + } + + if(j>1) { // send 2 bytes + k = 0; + j = 0; + } + } + + if(i>42) + i = 0; } + +/********************************************************************/ +// seperate +/********************************************************************/ +void separate(void) +{ + int count = 2; + for(int i = 0; i < 20; i++) + { + onebytedata[count] = display[i] & 0x00ff; + onebytedata[count + 1] = display[i] >> 8; + count = count + 2; + } + +// onebytedata[2] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列 +// onebytedata[3] = display[0] >> 8; // HSB(8bit)資料存入陣列 +// onebytedata[4] = display[1] & 0x00ff; +// onebytedata[5] = display[1] >> 8; +// onebytedata[6] = display[2] & 0x00ff; +// onebytedata[7] = display[2] >> 8; +// onebytedata[8] = display[3] & 0x00ff; +// onebytedata[9] = display[3] >> 8; +// onebytedata[10] = display[4] & 0x00ff; +// onebytedata[11] = display[4] >> 8; +// onebytedata[12] = display[5] & 0x00ff; +// onebytedata[13] = display[5] >> 8; +// onebytedata[14] = display[6] & 0x00ff; +// onebytedata[15] = display[6] >> 8; +// onebytedata[16] = display[7] & 0x00ff; +// onebytedata[17] = display[7] >> 8; +// onebytedata[18] = display[8] & 0x00ff; +// onebytedata[19] = display[8] >> 8; +// onebytedata[20] = display[9] & 0x00ff; +// onebytedata[21] = display[9] >> 8; +// onebytedata[22] = display[10] & 0x00ff; +// onebytedata[23] = display[10] >> 8; +// onebytedata[24] = display[11] & 0x00ff; +// onebytedata[25] = display[11] >> 8; +// onebytedata[26] = display[12] & 0x00ff; +// onebytedata[27] = display[12] >> 8; +// onebytedata[28] = display[13] & 0x00ff; +// onebytedata[29] = display[13] >> 8; +// onebytedata[30] = display[14] & 0x00ff; +// onebytedata[31] = display[14] >> 8; +// onebytedata[32] = display[15] & 0x00ff; +// onebytedata[33] = display[15] >> 8; +// onebytedata[34] = display[16] & 0x00ff; +// onebytedata[35] = display[16] >> 8; +// onebytedata[36] = display[17] & 0x00ff; +// onebytedata[37] = display[17] >> 8; +// onebytedata[38] = display[18] & 0x00ff; +// onebytedata[39] = display[18] >> 8; +// onebytedata[40] = display[19] & 0x00ff; +// onebytedata[41] = display[19] >> 8; +} + /********************************************************************/ // timer1_interrupt /********************************************************************/ @@ -142,7 +291,7 @@ Gyro_axis_data[7] = -imu3.gz*Gyro_gain_y_2; Gyro_axis_data[8] = -imu3.gy*Gyro_gain_z_2; - for(i=0;i<9;i++) + for(i=0;i<9;i++) //低通濾波 { Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15); Acce_axis_data_f_old[i] = Acce_axis_data_f[i]; @@ -150,19 +299,19 @@ Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i]; } - axm[0] = Acce_axis_data_f[0]; + axm[0] = Acce_axis_data_f[0]; //第一顆imu aym[0] = Acce_axis_data_f[1]; azm[0] = Acce_axis_data_f[2]; w1[0] = Gyro_axis_data_f[0]; w2[0] = Gyro_axis_data_f[1]; w3[0] = Gyro_axis_data_f[2]; - axm[1] = Acce_axis_data_f[3]; + axm[1] = Acce_axis_data_f[3]; //第二顆imu aym[1] = Acce_axis_data_f[4]; azm[1] = Acce_axis_data_f[5]; w1[1] = Gyro_axis_data_f[3]; w2[1] = Gyro_axis_data_f[4]; w3[1] = Gyro_axis_data_f[5]; - axm[2] = Acce_axis_data_f[6]; + axm[2] = Acce_axis_data_f[6]; //第三顆imu aym[2] = Acce_axis_data_f[7]; azm[2] = Acce_axis_data_f[8]; w1[2] = Gyro_axis_data_f[6]; @@ -179,10 +328,15 @@ { pitch_dot_old[i] = pitch_dot[i]; } - emg_value[0] = sEMG.read(); - emg_value[1] = sEMG2.read(); - emg_value[2] = sEMG3.read(); - emg_value[3] = sEMG4.read(); +// emg_value[0] = sEMG.read(); +// emg_value[1] = sEMG2.read(); +// emg_value[2] = sEMG3.read(); +// emg_value[3] = sEMG4.read(); + Voltage_FSR = FSR.read(); // Converts and read the analog input value (value from 0.0 to 1.0) + Voltage_EMG = EMG.read(); // Converts and read the analog input value (value from 0.0 to 1.0) + Voltage_FSR = Voltage_FSR * 33; // Change the value to be in the 0 to 3300 range + Voltage_EMG = Voltage_EMG * 33; // Change the value to be in the 0 to 3300 range + time_flag = t.read_ms(); } /********************************************************************/ // estimator @@ -192,17 +346,17 @@ int i; for(i=0;i<3;i++) { - axm_f[i] = lpf(axm[i],axm_f_old[i],alpha); + axm_f[i] = lpf(axm[i],axm_f_old[i],alpha); //axm_f濾波 axm_f_old[i] = axm_f[i]; - w3aym_f[i] = lpf(w3[i]*aym[i],w3aym_f_old[i],alpha); + w3aym_f[i] = lpf(w3[i]*aym[i],w3aym_f_old[i],alpha); //w3aym_f濾波 w3aym_f_old[i] = w3aym_f[i]; - w2azm_f[i] = lpf(w2[i]*azm[i],w2azm_f_old[i],alpha); + w2azm_f[i] = lpf(w2[i]*azm[i],w2azm_f_old[i],alpha); //w2azm_f濾波 w2azm_f_old[i] = w2azm_f[i]; - aym_f[i] = lpf(aym[i],aym_f_old[i],alpha); + aym_f[i] = lpf(aym[i],aym_f_old[i],alpha); //aym_f濾波 aym_f_old[i] = aym_f[i]; - w3axm_f[i] = lpf(w3[i]*axm[i],w3axm_f_old[i],alpha); + w3axm_f[i] = lpf(w3[i]*axm[i],w3axm_f_old[i],alpha); // w3axm_f濾波 w3axm_f_old[i] = w3axm_f[i]; - w1azm_f[i] = lpf(w1[i]*azm[i],w1azm_f_old[i],alpha); + w1azm_f[i] = lpf(w1[i]*azm[i],w1azm_f_old[i],alpha); //w1azm_f濾波 w1azm_f_old[i] = w1azm_f[i]; x1_hat[i] = axm_f[i] + w3aym_f[i]/alpha - w2azm_f[i]/alpha;