Rong Syuan Lin / Mbed 2 deprecated LSM9DS1_project_5_zerotorque

20181105

Dependencies:   MX28 mbed

Fork of LSM9DS1_project_5_zerotorque by Chen Wei Ting

main.cpp@9:07de3af99031, 2018-11-05 (annotated)

Committer:
nylon0212
Date:
Mon Nov 05 09:29:20 2018 +0000
Revision:
9:07de3af99031
Parent:
8:9c3b291b3288 
20181105

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nylon0212 9:07de3af99031 1 #define DEBUG_STDIO 1
nylon0212 9:07de3af99031 2
nylon0212 9:07de3af99031 3 #include "mbed.h" //不用gyro的data，只要輸出兩個imu的acce的xyz資料即可
JJting 0:c23e915f255b 4 #include "encoder.h"
nylon0212 9:07de3af99031 5 //#include "Mx28.h"
nylon0212 9:07de3af99031 6 //#include "PID.h"
JJting 4:f19e7669d1b5 7 #include "LSM9DS1.h"
JJting 0:c23e915f255b 8
JJting 0:c23e915f255b 9 //********************* Dynamxiel ******************************
JJting 0:c23e915f255b 10 #define SERVO_ID 0x01 // ID of which we will set Dynamixel too
JJting 0:c23e915f255b 11 #define SERVO_ControlPin A2 // Control pin of buffer chip, NOTE: this does not matter becasue we are not using a half to full contorl buffer.
JJting 0:c23e915f255b 12 #define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps)
JJting 0:c23e915f255b 13 #define TxPin A0
JJting 0:c23e915f255b 14 #define RxPin A1
JJting 0:c23e915f255b 15 #define CW_LIMIT_ANGLE 1 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
JJting 0:c23e915f255b 16 #define CCW_LIMIT_ANGLE 4095 // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
JJting 4:f19e7669d1b5 17 #define _PI 3.14159265f
JJting 0:c23e915f255b 18 //***************************************************************
JJting 0:c23e915f255b 19
JJting 0:c23e915f255b 20 Serial uart(USBTX, USBRX);
nylon0212 9:07de3af99031 21 //Serial uart(D10,D2); // TX : D10 RX : D2 // bluetooth
JJting 0:c23e915f255b 22 DigitalOut LED(A4); // check if the code is running
nylon0212 9:07de3af99031 23 DigitalOut led2(PA_5); // check if the code is running interrupt
nylon0212 9:07de3af99031 24 uint8_t led2f; //測試用
nylon0212 9:07de3af99031 25 AnalogIn EMG(PC_4); //肌電訊號
JJting 4:f19e7669d1b5 26 DigitalOut test(PC_8);
nylon0212 9:07de3af99031 27 DigitalOut CS(PA_8);
JJting 4:f19e7669d1b5 28 //AnalogOut test2(PA_5);
nylon0212 9:07de3af99031 29 /*IMU*******************************************************************/
JJting 4:f19e7669d1b5 30 LSM9DS1 imu(D14, D15); //SDA SCL
nylon0212 9:07de3af99031 31 //LSM9DS1 imu2(PC_9,PA_8); //SDA SCL
JJting 4:f19e7669d1b5 32 void init_IMU();
JJting 4:f19e7669d1b5 33 int16_t Gyro_axis_data[6] = {0}; // X, Y, Z axis
JJting 4:f19e7669d1b5 34 int16_t Acce_axis_data[6] = {0}; // X, Y, Z axis
nylon0212 9:07de3af99031 35 float Acce_axis_data_f[6] = {0}; //_f代表經過低通濾波的資料
JJting 4:f19e7669d1b5 36 float Acce_axis_data_f_old[6] = {0};
JJting 4:f19e7669d1b5 37 float Gyro_axis_data_f[6] = {0};
JJting 4:f19e7669d1b5 38 float Gyro_axis_data_f_old[6] = {0};
JJting 4:f19e7669d1b5 39
JJting 0:c23e915f255b 40
JJting 0:c23e915f255b 41 // Timer
JJting 0:c23e915f255b 42 Ticker timer1;
nylon0212 9:07de3af99031 43 float ITR_time1 = 100.0; //timer interrupt unit：us 多少us計時一次 10毫秒
nylon0212 9:07de3af99031 44 float Ts = ITR_time1/1000000; //控馬達的某個時間參數 不用理他
JJting 0:c23e915f255b 45 uint8_t flag;
JJting 0:c23e915f255b 46
JJting 3:98cdee5d9b63 47 // EMG
nylon0212 9:07de3af99031 48 float lpf(float input, float output_old, float frequency); //low pass filter
JJting 3:98cdee5d9b63 49 float emg_value;
JJting 3:98cdee5d9b63 50 float emg_value_f;
JJting 3:98cdee5d9b63 51 float emg_value_f_old;
nylon0212 9:07de3af99031 52 float Tf = ITR_time1/1000000; // 低通濾波的採樣週期
JJting 3:98cdee5d9b63 53
JJting 0:c23e915f255b 54 // uart_tx
nylon0212 9:07de3af99031 55 union splitter { //將data切割為兩個byte
JJting 0:c23e915f255b 56 short j;
JJting 0:c23e915f255b 57 char C[2];
JJting 0:c23e915f255b 58 // C[0] is lowbyte of j, C[1] is highbyte of j
JJting 0:c23e915f255b 59 };
nylon0212 9:07de3af99031 60 uint8_t T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
nylon0212 9:07de3af99031 61 // T[16]中的16是2+14，2為start btye 255 255，14為要傳7個參數到電腦，拆成high byte和low byte，所以是7*2=14
nylon0212 9:07de3af99031 62 // 若要更改傳到電腦的參數個數，則改x*2=y，x為要傳的參數個數，y為char T[]裡面要有多少個0
JJting 0:c23e915f255b 63 int i = 0;
JJting 0:c23e915f255b 64
JJting 0:c23e915f255b 65 // function
JJting 0:c23e915f255b 66 void init_UART();
JJting 0:c23e915f255b 67 void init_TIMER();
JJting 0:c23e915f255b 68 void timer1_ITR();
nylon0212 9:07de3af99031 69 void uart_tx(); // 傳data到電腦
nylon0212 9:07de3af99031 70 /**************************************************************************/
JJting 0:c23e915f255b 71 int main()
JJting 0:c23e915f255b 72 {
nylon0212 9:07de3af99031 73 // LED = 1; // darken
nylon0212 9:07de3af99031 74 // wait_ms(500);
JJting 0:c23e915f255b 75 // initial sensor
nylon0212 9:07de3af99031 76 // init_SPI_encoder();
nylon0212 9:07de3af99031 77 // init_encoder();
JJting 4:f19e7669d1b5 78 init_IMU();
JJting 0:c23e915f255b 79 // initial uart
JJting 0:c23e915f255b 80 init_UART();
JJting 0:c23e915f255b 81
nylon0212 9:07de3af99031 82 // wait_ms(500);
JJting 0:c23e915f255b 83
nylon0212 9:07de3af99031 84 // led2 = 1;
JJting 1:2823a39f70a9 85 // led2f = 0;
nylon0212 9:07de3af99031 86 // LED = 0; // lighten
JJting 0:c23e915f255b 87
JJting 0:c23e915f255b 88 init_TIMER();
JJting 0:c23e915f255b 89
nylon0212 9:07de3af99031 90 while(1) {}
JJting 0:c23e915f255b 91 }
JJting 0:c23e915f255b 92
JJting 4:f19e7669d1b5 93 void init_IMU()
JJting 4:f19e7669d1b5 94 {
nylon0212 9:07de3af99031 95 imu.begin(); //imu.G_SCALE_245DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_238_BW_14,imu.A_ODR_119,imu.M_ODR_0625
nylon0212 9:07de3af99031 96 // imu2.begin(imu.G_SCALE_500DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_476_BW_21,imu.A_ODR_476,imu.M_ODR_0625);
JJting 0:c23e915f255b 97 }
JJting 0:c23e915f255b 98
JJting 0:c23e915f255b 99 void init_UART()
JJting 0:c23e915f255b 100 {
JJting 0:c23e915f255b 101 uart.baud(115200);
JJting 0:c23e915f255b 102 }
JJting 0:c23e915f255b 103
JJting 0:c23e915f255b 104 void init_TIMER()
JJting 0:c23e915f255b 105 {
JJting 0:c23e915f255b 106 timer1.attach_us(&timer1_ITR, ITR_time1);
JJting 0:c23e915f255b 107 }
JJting 0:c23e915f255b 108
nylon0212 9:07de3af99031 109 void timer1_ITR() //開始讀取資料
JJting 0:c23e915f255b 110 {
nylon0212 9:07de3af99031 111 // test = 1;
JJting 5:4dbed091ec5a 112 led2 = !led2;
JJting 5:4dbed091ec5a 113 // EMG
nylon0212 9:07de3af99031 114 //emg_value = EMG.read();
nylon0212 9:07de3af99031 115 // emg_value_f = lpf(emg_value,emg_value_f_old,15);
nylon0212 9:07de3af99031 116 // emg_value_f_old = emg_value_f;
JJting 5:4dbed091ec5a 117
nylon0212 9:07de3af99031 118 CS = 1;
JJting 5:4dbed091ec5a 119 // IMU
nylon0212 9:07de3af99031 120 imu.readAccel();
nylon0212 9:07de3af99031 121 // imu.readGyro();
nylon0212 9:07de3af99031 122 // imu2.readAccel();
nylon0212 9:07de3af99031 123 // imu2.readGyro();
nylon0212 9:07de3af99031 124
nylon0212 9:07de3af99031 125 //Acce_axis_data[0] = imu.ax*Acce_gain_x;
nylon0212 9:07de3af99031 126 // Acce_axis_data[1] = imu.ay*Acce_gain_y;
nylon0212 9:07de3af99031 127 // Acce_axis_data[2] = imu.az*Acce_gain_z;
nylon0212 9:07de3af99031 128 // Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2;
nylon0212 9:07de3af99031 129 // Acce_axis_data[4] = imu2.az*Acce_gain_y_2;
nylon0212 9:07de3af99031 130 // Acce_axis_data[5] = imu2.ay*Acce_gain_z_2;
JJting 5:4dbed091ec5a 131
nylon0212 9:07de3af99031 132 //Gyro_axis_data[0] = imu.gx*Gyro_gain_x;
nylon0212 9:07de3af99031 133 // Gyro_axis_data[1] = imu.gy*Gyro_gain_y;
nylon0212 9:07de3af99031 134 // Gyro_axis_data[2] = imu.gz*Gyro_gain_z;
nylon0212 9:07de3af99031 135 // Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2;
nylon0212 9:07de3af99031 136 // Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2;
nylon0212 9:07de3af99031 137 // Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2;
JJting 5:4dbed091ec5a 138
nylon0212 9:07de3af99031 139 //for(i=0; i<6; i++) {
nylon0212 9:07de3af99031 140 // Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15);
nylon0212 9:07de3af99031 141 // Acce_axis_data_f_old[i] = Acce_axis_data_f[i];
nylon0212 9:07de3af99031 142 // Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15);
nylon0212 9:07de3af99031 143 // Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i];
nylon0212 9:07de3af99031 144 // }
nylon0212 9:07de3af99031 145 uart.printf("x: %7.4f, y: %7.4f, z: %7.4f\n\r",imu.ax, imu.ay, imu.az);
nylon0212 9:07de3af99031 146 //uart_tx();
JJting 5:4dbed091ec5a 147
nylon0212 9:07de3af99031 148 // flag = 0;
JJting 7:5cb292622961 149 // wait_ms(1);
nylon0212 9:07de3af99031 150 // test = 0;
JJting 0:c23e915f255b 151 }
JJting 0:c23e915f255b 152
nylon0212 9:07de3af99031 153 void uart_tx() //分割資料
JJting 0:c23e915f255b 154 {
JJting 0:c23e915f255b 155 splitter s1;
JJting 0:c23e915f255b 156 splitter s2;
JJting 0:c23e915f255b 157 splitter s3;
JJting 0:c23e915f255b 158 splitter s4;
JJting 0:c23e915f255b 159 splitter s5;
JJting 0:c23e915f255b 160 splitter s6;
JJting 0:c23e915f255b 161 splitter s7;
nylon0212 9:07de3af99031 162 // splitter s8;
JJting 0:c23e915f255b 163
nylon0212 9:07de3af99031 164 s1.j = Acce_axis_data_f[0]; // 0x6161;//
nylon0212 9:07de3af99031 165 s2.j = Acce_axis_data_f[1];
nylon0212 9:07de3af99031 166 s3.j = Acce_axis_data_f[2];
nylon0212 9:07de3af99031 167 s4.j = Acce_axis_data_f[3];
nylon0212 9:07de3af99031 168 s5.j = Acce_axis_data_f[4];
nylon0212 9:07de3af99031 169 s6.j = Acce_axis_data_f[5];
nylon0212 9:07de3af99031 170 s7.j = emg_value_f;
nylon0212 9:07de3af99031 171 // s8.j = 1; // 若要傳的參數不足8個，則隨意打一個常數即可
JJting 0:c23e915f255b 172
nylon0212 9:07de3af99031 173 T[2] = Acce_axis_data[0]; //0x5A;
nylon0212 9:07de3af99031 174 T[3] = Acce_axis_data[0] >>8;
JJting 0:c23e915f255b 175 T[4] = s2.C[0];
JJting 0:c23e915f255b 176 T[5] = s2.C[1];
JJting 0:c23e915f255b 177 T[6] = s3.C[0];
JJting 0:c23e915f255b 178 T[7] = s3.C[1];
JJting 0:c23e915f255b 179 T[8] = s4.C[0];
JJting 0:c23e915f255b 180 T[9] = s4.C[1];
JJting 0:c23e915f255b 181 T[10] = s5.C[0];
JJting 0:c23e915f255b 182 T[11] = s5.C[1];
JJting 0:c23e915f255b 183 T[12] = s6.C[0];
JJting 0:c23e915f255b 184 T[13] = s6.C[1];
JJting 0:c23e915f255b 185 T[14] = s7.C[0];
JJting 0:c23e915f255b 186 T[15] = s7.C[1];
nylon0212 9:07de3af99031 187
nylon0212 9:07de3af99031 188
nylon0212 9:07de3af99031 189
nylon0212 9:07de3af99031 190
JJting 7:5cb292622961 191
nylon0212 9:07de3af99031 192 // uart.putc(T[0]);
nylon0212 9:07de3af99031 193 // uart.putc(T[1]);
nylon0212 9:07de3af99031 194 // uart.putc(T[2]);
nylon0212 9:07de3af99031 195 // uart.putc(T[3]);
nylon0212 9:07de3af99031 196 // uart.putc(T[4]);
nylon0212 9:07de3af99031 197 // uart.putc(T[5]);
nylon0212 9:07de3af99031 198 // uart.putc(T[6]);
nylon0212 9:07de3af99031 199 // uart.putc(T[7]);
nylon0212 9:07de3af99031 200 // uart.putc(T[8]);
nylon0212 9:07de3af99031 201 // uart.putc(T[9]);
nylon0212 9:07de3af99031 202 // uart.putc(T[10]);
nylon0212 9:07de3af99031 203 // uart.putc(T[11]);
nylon0212 9:07de3af99031 204 // uart.putc(T[12]);
nylon0212 9:07de3af99031 205 // uart.putc(T[13]);
nylon0212 9:07de3af99031 206 // uart.putc(T[14]);
nylon0212 9:07de3af99031 207 // uart.putc(T[15]);
JJting 1:2823a39f70a9 208
nylon0212 9:07de3af99031 209 // while(1) { //開始傳到USB
nylon0212 9:07de3af99031 210 // if (uart.writeable() == 1) {
nylon0212 9:07de3af99031 211 // uart.putc(T[i]);
nylon0212 9:07de3af99031 212 // i++;
nylon0212 9:07de3af99031 213 // }
nylon0212 9:07de3af99031 214 // if (i >= sizeof(T)) {
nylon0212 9:07de3af99031 215 // i = 0;
nylon0212 9:07de3af99031 216 // break;
nylon0212 9:07de3af99031 217 // }
JJting 6:f48c51662e27 218 // }
JJting 1:2823a39f70a9 219 }
JJting 3:98cdee5d9b63 220
JJting 3:98cdee5d9b63 221 float lpf(float input, float output_old, float frequency)
JJting 3:98cdee5d9b63 222 {
JJting 3:98cdee5d9b63 223 float output = 0;
JJting 3:98cdee5d9b63 224
JJting 3:98cdee5d9b63 225 output = (output_old + frequency*Tf*input) / (1 + frequency*Tf);
JJting 3:98cdee5d9b63 226
JJting 3:98cdee5d9b63 227 return output;
nylon0212 9:07de3af99031 228 }
nylon0212 9:07de3af99031 229
nylon0212 9:07de3af99031 230 //有估測器有delay，丟資料給機器學習時就不是及時動作，所以之後決定直接丟資料