Vito Lin / Mbed 2 deprecated Rong_Sensor

rrr

Dependencies:   mbed MX28

main.cpp@12:a197e7eed8ab, 2018-12-19 (annotated)

Committer:
ChengHan
Date:
Wed Dec 19 12:35:03 2018 +0000
Revision:
12:a197e7eed8ab
Parent:
11:cd28cb84fd81 
rrr

Who changed what in which revision?

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