Rong Syuan Lin
version2
Fork of
LSM9DS1_project
by 
Rong Syuan Lin
Diff: main.cpp
- Revision:
- 7:d421b158f1be
- Parent:
- 6:62643ad078c8
--- 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;