SHENG-HEN HSIEH
/
LSM9DS1_project_5_zerotorque
By pOOPOO
Fork of LSM9DS1_project_5_zerotorque by
main.cpp@10:6a9de32601b1, 2018-11-05 (annotated)
- Committer:
- open4416
- Date:
- Mon Nov 05 12:14:58 2018 +0000
- Revision:
- 10:6a9de32601b1
- Parent:
- 9:07de3af99031
TEMP
Who changed what in which revision?
User | Revision | Line number | New 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 |
open4416 | 10:6a9de32601b1 | 24 | AnalogIn EMG(PC_4); // |
JJting | 4:f19e7669d1b5 | 25 | DigitalOut test(PC_8); |
nylon0212 | 9:07de3af99031 | 26 | DigitalOut CS(PA_8); |
open4416 | 10:6a9de32601b1 | 27 | uint8_t led2f; //測試用 |
open4416 | 10:6a9de32601b1 | 28 | |
JJting | 4:f19e7669d1b5 | 29 | //AnalogOut test2(PA_5); |
nylon0212 | 9:07de3af99031 | 30 | /*IMU*******************************************************************/ |
JJting | 4:f19e7669d1b5 | 31 | LSM9DS1 imu(D14, D15); //SDA SCL |
nylon0212 | 9:07de3af99031 | 32 | //LSM9DS1 imu2(PC_9,PA_8); //SDA SCL |
JJting | 4:f19e7669d1b5 | 33 | void init_IMU(); |
JJting | 4:f19e7669d1b5 | 34 | int16_t Gyro_axis_data[6] = {0}; // X, Y, Z axis |
JJting | 4:f19e7669d1b5 | 35 | int16_t Acce_axis_data[6] = {0}; // X, Y, Z axis |
nylon0212 | 9:07de3af99031 | 36 | float Acce_axis_data_f[6] = {0}; //_f代表經過低通濾波的資料 |
JJting | 4:f19e7669d1b5 | 37 | float Acce_axis_data_f_old[6] = {0}; |
JJting | 4:f19e7669d1b5 | 38 | float Gyro_axis_data_f[6] = {0}; |
JJting | 4:f19e7669d1b5 | 39 | float Gyro_axis_data_f_old[6] = {0}; |
JJting | 4:f19e7669d1b5 | 40 | |
JJting | 0:c23e915f255b | 41 | |
JJting | 0:c23e915f255b | 42 | // Timer |
JJting | 0:c23e915f255b | 43 | Ticker timer1; |
nylon0212 | 9:07de3af99031 | 44 | float ITR_time1 = 100.0; //timer interrupt unit:us 多少us計時一次 10毫秒 |
nylon0212 | 9:07de3af99031 | 45 | float Ts = ITR_time1/1000000; //控馬達的某個時間參數 不用理他 |
JJting | 0:c23e915f255b | 46 | uint8_t flag; |
JJting | 0:c23e915f255b | 47 | |
JJting | 3:98cdee5d9b63 | 48 | // EMG |
nylon0212 | 9:07de3af99031 | 49 | float lpf(float input, float output_old, float frequency); //low pass filter |
JJting | 3:98cdee5d9b63 | 50 | float emg_value; |
JJting | 3:98cdee5d9b63 | 51 | float emg_value_f; |
JJting | 3:98cdee5d9b63 | 52 | float emg_value_f_old; |
nylon0212 | 9:07de3af99031 | 53 | float Tf = ITR_time1/1000000; // 低通濾波的採樣週期 |
JJting | 3:98cdee5d9b63 | 54 | |
JJting | 0:c23e915f255b | 55 | // uart_tx |
nylon0212 | 9:07de3af99031 | 56 | union splitter { //將data切割為兩個byte |
JJting | 0:c23e915f255b | 57 | short j; |
JJting | 0:c23e915f255b | 58 | char C[2]; |
JJting | 0:c23e915f255b | 59 | // C[0] is lowbyte of j, C[1] is highbyte of j |
JJting | 0:c23e915f255b | 60 | }; |
nylon0212 | 9:07de3af99031 | 61 | uint8_t T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; |
nylon0212 | 9:07de3af99031 | 62 | // T[16]中的16是2+14,2為start btye 255 255,14為要傳7個參數到電腦,拆成high byte和low byte,所以是7*2=14 |
nylon0212 | 9:07de3af99031 | 63 | // 若要更改傳到電腦的參數個數,則改x*2=y,x為要傳的參數個數,y為char T[]裡面要有多少個0 |
JJting | 0:c23e915f255b | 64 | int i = 0; |
JJting | 0:c23e915f255b | 65 | |
JJting | 0:c23e915f255b | 66 | // function |
JJting | 0:c23e915f255b | 67 | void init_UART(); |
JJting | 0:c23e915f255b | 68 | void init_TIMER(); |
JJting | 0:c23e915f255b | 69 | void timer1_ITR(); |
nylon0212 | 9:07de3af99031 | 70 | void uart_tx(); // 傳data到電腦 |
nylon0212 | 9:07de3af99031 | 71 | /**************************************************************************/ |
JJting | 0:c23e915f255b | 72 | int main() |
JJting | 0:c23e915f255b | 73 | { |
nylon0212 | 9:07de3af99031 | 74 | // LED = 1; // darken |
nylon0212 | 9:07de3af99031 | 75 | // wait_ms(500); |
JJting | 0:c23e915f255b | 76 | // initial sensor |
nylon0212 | 9:07de3af99031 | 77 | // init_SPI_encoder(); |
nylon0212 | 9:07de3af99031 | 78 | // init_encoder(); |
open4416 | 10:6a9de32601b1 | 79 | init_UART(); |
open4416 | 10:6a9de32601b1 | 80 | |
JJting | 4:f19e7669d1b5 | 81 | init_IMU(); |
JJting | 0:c23e915f255b | 82 | // initial uart |
open4416 | 10:6a9de32601b1 | 83 | |
open4416 | 10:6a9de32601b1 | 84 | |
JJting | 0:c23e915f255b | 85 | |
nylon0212 | 9:07de3af99031 | 86 | // wait_ms(500); |
JJting | 0:c23e915f255b | 87 | |
nylon0212 | 9:07de3af99031 | 88 | // led2 = 1; |
JJting | 1:2823a39f70a9 | 89 | // led2f = 0; |
nylon0212 | 9:07de3af99031 | 90 | // LED = 0; // lighten |
JJting | 0:c23e915f255b | 91 | |
open4416 | 10:6a9de32601b1 | 92 | // init_TIMER(); |
open4416 | 10:6a9de32601b1 | 93 | |
open4416 | 10:6a9de32601b1 | 94 | while(1) { |
open4416 | 10:6a9de32601b1 | 95 | // test = 1; |
open4416 | 10:6a9de32601b1 | 96 | led2 = !led2; |
open4416 | 10:6a9de32601b1 | 97 | // EMG |
open4416 | 10:6a9de32601b1 | 98 | //emg_value = EMG.read(); |
open4416 | 10:6a9de32601b1 | 99 | // emg_value_f = lpf(emg_value,emg_value_f_old,15); |
open4416 | 10:6a9de32601b1 | 100 | // emg_value_f_old = emg_value_f; |
open4416 | 10:6a9de32601b1 | 101 | |
open4416 | 10:6a9de32601b1 | 102 | // IMU |
open4416 | 10:6a9de32601b1 | 103 | imu.readAccel(); |
open4416 | 10:6a9de32601b1 | 104 | // imu.readGyro(); |
open4416 | 10:6a9de32601b1 | 105 | // imu2.readAccel(); |
open4416 | 10:6a9de32601b1 | 106 | // imu2.readGyro(); |
open4416 | 10:6a9de32601b1 | 107 | |
open4416 | 10:6a9de32601b1 | 108 | Acce_axis_data[0] = imu.ax*Acce_gain_x; |
open4416 | 10:6a9de32601b1 | 109 | Acce_axis_data[1] = imu.ay*Acce_gain_y; |
open4416 | 10:6a9de32601b1 | 110 | Acce_axis_data[2] = imu.az*Acce_gain_z; |
open4416 | 10:6a9de32601b1 | 111 | // Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2; |
open4416 | 10:6a9de32601b1 | 112 | // Acce_axis_data[4] = imu2.az*Acce_gain_y_2; |
open4416 | 10:6a9de32601b1 | 113 | // Acce_axis_data[5] = imu2.ay*Acce_gain_z_2; |
JJting | 0:c23e915f255b | 114 | |
open4416 | 10:6a9de32601b1 | 115 | Gyro_axis_data[0] = imu.gx*Gyro_gain_x; |
open4416 | 10:6a9de32601b1 | 116 | Gyro_axis_data[1] = imu.gy*Gyro_gain_y; |
open4416 | 10:6a9de32601b1 | 117 | Gyro_axis_data[2] = imu.gz*Gyro_gain_z; |
open4416 | 10:6a9de32601b1 | 118 | // Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2; |
open4416 | 10:6a9de32601b1 | 119 | // Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2; |
open4416 | 10:6a9de32601b1 | 120 | // Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2; |
open4416 | 10:6a9de32601b1 | 121 | |
open4416 | 10:6a9de32601b1 | 122 | // for(i=0; i<6; i++) { |
open4416 | 10:6a9de32601b1 | 123 | // Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15); |
open4416 | 10:6a9de32601b1 | 124 | // Acce_axis_data_f_old[i] = Acce_axis_data_f[i]; |
open4416 | 10:6a9de32601b1 | 125 | // Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15); |
open4416 | 10:6a9de32601b1 | 126 | // Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i]; |
open4416 | 10:6a9de32601b1 | 127 | // } |
open4416 | 10:6a9de32601b1 | 128 | // int16_t shit2 = imu.ax_raw; |
open4416 | 10:6a9de32601b1 | 129 | // uart.printf("%d\n\r", shit2); |
open4416 | 10:6a9de32601b1 | 130 | uart.printf("x: %.2f, y: %.2f, z: %.2f\n\r",imu.ax, imu.ay, imu.az); |
open4416 | 10:6a9de32601b1 | 131 | // uart_tx(); |
open4416 | 10:6a9de32601b1 | 132 | |
open4416 | 10:6a9de32601b1 | 133 | // flag = 0; |
open4416 | 10:6a9de32601b1 | 134 | wait_ms(500); |
open4416 | 10:6a9de32601b1 | 135 | // test = 0; |
open4416 | 10:6a9de32601b1 | 136 | } |
JJting | 0:c23e915f255b | 137 | } |
JJting | 0:c23e915f255b | 138 | |
JJting | 4:f19e7669d1b5 | 139 | void init_IMU() |
JJting | 4:f19e7669d1b5 | 140 | { |
open4416 | 10:6a9de32601b1 | 141 | uint16_t shit; |
open4416 | 10:6a9de32601b1 | 142 | shit = 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 | 143 | // 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); |
open4416 | 10:6a9de32601b1 | 144 | if(shit != 0x683D) { |
open4416 | 10:6a9de32601b1 | 145 | while(1) { |
open4416 | 10:6a9de32601b1 | 146 | uart.printf("IMU error\n\r"); |
open4416 | 10:6a9de32601b1 | 147 | } |
open4416 | 10:6a9de32601b1 | 148 | } |
open4416 | 10:6a9de32601b1 | 149 | uart.printf("IMU init success\n\r"); |
open4416 | 10:6a9de32601b1 | 150 | |
open4416 | 10:6a9de32601b1 | 151 | // uart.printf("%d\n\r", shit); |
JJting | 0:c23e915f255b | 152 | } |
JJting | 0:c23e915f255b | 153 | |
JJting | 0:c23e915f255b | 154 | void init_UART() |
JJting | 0:c23e915f255b | 155 | { |
JJting | 0:c23e915f255b | 156 | uart.baud(115200); |
JJting | 0:c23e915f255b | 157 | } |
JJting | 0:c23e915f255b | 158 | |
JJting | 0:c23e915f255b | 159 | void init_TIMER() |
JJting | 0:c23e915f255b | 160 | { |
JJting | 0:c23e915f255b | 161 | timer1.attach_us(&timer1_ITR, ITR_time1); |
JJting | 0:c23e915f255b | 162 | } |
JJting | 0:c23e915f255b | 163 | |
nylon0212 | 9:07de3af99031 | 164 | void timer1_ITR() //開始讀取資料 |
JJting | 0:c23e915f255b | 165 | { |
nylon0212 | 9:07de3af99031 | 166 | // test = 1; |
JJting | 5:4dbed091ec5a | 167 | led2 = !led2; |
JJting | 5:4dbed091ec5a | 168 | // EMG |
nylon0212 | 9:07de3af99031 | 169 | //emg_value = EMG.read(); |
nylon0212 | 9:07de3af99031 | 170 | // emg_value_f = lpf(emg_value,emg_value_f_old,15); |
nylon0212 | 9:07de3af99031 | 171 | // emg_value_f_old = emg_value_f; |
JJting | 5:4dbed091ec5a | 172 | |
nylon0212 | 9:07de3af99031 | 173 | CS = 1; |
JJting | 5:4dbed091ec5a | 174 | // IMU |
nylon0212 | 9:07de3af99031 | 175 | imu.readAccel(); |
nylon0212 | 9:07de3af99031 | 176 | // imu.readGyro(); |
nylon0212 | 9:07de3af99031 | 177 | // imu2.readAccel(); |
nylon0212 | 9:07de3af99031 | 178 | // imu2.readGyro(); |
nylon0212 | 9:07de3af99031 | 179 | |
nylon0212 | 9:07de3af99031 | 180 | //Acce_axis_data[0] = imu.ax*Acce_gain_x; |
nylon0212 | 9:07de3af99031 | 181 | // Acce_axis_data[1] = imu.ay*Acce_gain_y; |
nylon0212 | 9:07de3af99031 | 182 | // Acce_axis_data[2] = imu.az*Acce_gain_z; |
nylon0212 | 9:07de3af99031 | 183 | // Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2; |
nylon0212 | 9:07de3af99031 | 184 | // Acce_axis_data[4] = imu2.az*Acce_gain_y_2; |
nylon0212 | 9:07de3af99031 | 185 | // Acce_axis_data[5] = imu2.ay*Acce_gain_z_2; |
JJting | 5:4dbed091ec5a | 186 | |
nylon0212 | 9:07de3af99031 | 187 | //Gyro_axis_data[0] = imu.gx*Gyro_gain_x; |
nylon0212 | 9:07de3af99031 | 188 | // Gyro_axis_data[1] = imu.gy*Gyro_gain_y; |
nylon0212 | 9:07de3af99031 | 189 | // Gyro_axis_data[2] = imu.gz*Gyro_gain_z; |
nylon0212 | 9:07de3af99031 | 190 | // Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2; |
nylon0212 | 9:07de3af99031 | 191 | // Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2; |
nylon0212 | 9:07de3af99031 | 192 | // Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2; |
JJting | 5:4dbed091ec5a | 193 | |
nylon0212 | 9:07de3af99031 | 194 | //for(i=0; i<6; i++) { |
nylon0212 | 9:07de3af99031 | 195 | // Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15); |
nylon0212 | 9:07de3af99031 | 196 | // Acce_axis_data_f_old[i] = Acce_axis_data_f[i]; |
nylon0212 | 9:07de3af99031 | 197 | // Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15); |
nylon0212 | 9:07de3af99031 | 198 | // Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i]; |
nylon0212 | 9:07de3af99031 | 199 | // } |
open4416 | 10:6a9de32601b1 | 200 | // uart.printf("x: %7.4f, y: %7.4f, z: %7.4f\n\r",imu.ax, imu.ay, imu.az); |
nylon0212 | 9:07de3af99031 | 201 | //uart_tx(); |
JJting | 5:4dbed091ec5a | 202 | |
nylon0212 | 9:07de3af99031 | 203 | // flag = 0; |
JJting | 7:5cb292622961 | 204 | // wait_ms(1); |
nylon0212 | 9:07de3af99031 | 205 | // test = 0; |
JJting | 0:c23e915f255b | 206 | } |
JJting | 0:c23e915f255b | 207 | |
nylon0212 | 9:07de3af99031 | 208 | void uart_tx() //分割資料 |
JJting | 0:c23e915f255b | 209 | { |
JJting | 0:c23e915f255b | 210 | splitter s1; |
JJting | 0:c23e915f255b | 211 | splitter s2; |
JJting | 0:c23e915f255b | 212 | splitter s3; |
JJting | 0:c23e915f255b | 213 | splitter s4; |
JJting | 0:c23e915f255b | 214 | splitter s5; |
JJting | 0:c23e915f255b | 215 | splitter s6; |
JJting | 0:c23e915f255b | 216 | splitter s7; |
nylon0212 | 9:07de3af99031 | 217 | // splitter s8; |
JJting | 0:c23e915f255b | 218 | |
nylon0212 | 9:07de3af99031 | 219 | s1.j = Acce_axis_data_f[0]; // 0x6161;// |
nylon0212 | 9:07de3af99031 | 220 | s2.j = Acce_axis_data_f[1]; |
nylon0212 | 9:07de3af99031 | 221 | s3.j = Acce_axis_data_f[2]; |
nylon0212 | 9:07de3af99031 | 222 | s4.j = Acce_axis_data_f[3]; |
nylon0212 | 9:07de3af99031 | 223 | s5.j = Acce_axis_data_f[4]; |
nylon0212 | 9:07de3af99031 | 224 | s6.j = Acce_axis_data_f[5]; |
nylon0212 | 9:07de3af99031 | 225 | s7.j = emg_value_f; |
nylon0212 | 9:07de3af99031 | 226 | // s8.j = 1; // 若要傳的參數不足8個,則隨意打一個常數即可 |
JJting | 0:c23e915f255b | 227 | |
nylon0212 | 9:07de3af99031 | 228 | T[2] = Acce_axis_data[0]; //0x5A; |
nylon0212 | 9:07de3af99031 | 229 | T[3] = Acce_axis_data[0] >>8; |
JJting | 0:c23e915f255b | 230 | T[4] = s2.C[0]; |
JJting | 0:c23e915f255b | 231 | T[5] = s2.C[1]; |
JJting | 0:c23e915f255b | 232 | T[6] = s3.C[0]; |
JJting | 0:c23e915f255b | 233 | T[7] = s3.C[1]; |
JJting | 0:c23e915f255b | 234 | T[8] = s4.C[0]; |
JJting | 0:c23e915f255b | 235 | T[9] = s4.C[1]; |
JJting | 0:c23e915f255b | 236 | T[10] = s5.C[0]; |
JJting | 0:c23e915f255b | 237 | T[11] = s5.C[1]; |
JJting | 0:c23e915f255b | 238 | T[12] = s6.C[0]; |
JJting | 0:c23e915f255b | 239 | T[13] = s6.C[1]; |
JJting | 0:c23e915f255b | 240 | T[14] = s7.C[0]; |
JJting | 0:c23e915f255b | 241 | T[15] = s7.C[1]; |
open4416 | 10:6a9de32601b1 | 242 | |
open4416 | 10:6a9de32601b1 | 243 | |
open4416 | 10:6a9de32601b1 | 244 | |
open4416 | 10:6a9de32601b1 | 245 | |
JJting | 7:5cb292622961 | 246 | |
nylon0212 | 9:07de3af99031 | 247 | // uart.putc(T[0]); |
nylon0212 | 9:07de3af99031 | 248 | // uart.putc(T[1]); |
nylon0212 | 9:07de3af99031 | 249 | // uart.putc(T[2]); |
nylon0212 | 9:07de3af99031 | 250 | // uart.putc(T[3]); |
nylon0212 | 9:07de3af99031 | 251 | // uart.putc(T[4]); |
nylon0212 | 9:07de3af99031 | 252 | // uart.putc(T[5]); |
nylon0212 | 9:07de3af99031 | 253 | // uart.putc(T[6]); |
nylon0212 | 9:07de3af99031 | 254 | // uart.putc(T[7]); |
nylon0212 | 9:07de3af99031 | 255 | // uart.putc(T[8]); |
nylon0212 | 9:07de3af99031 | 256 | // uart.putc(T[9]); |
nylon0212 | 9:07de3af99031 | 257 | // uart.putc(T[10]); |
nylon0212 | 9:07de3af99031 | 258 | // uart.putc(T[11]); |
nylon0212 | 9:07de3af99031 | 259 | // uart.putc(T[12]); |
nylon0212 | 9:07de3af99031 | 260 | // uart.putc(T[13]); |
nylon0212 | 9:07de3af99031 | 261 | // uart.putc(T[14]); |
nylon0212 | 9:07de3af99031 | 262 | // uart.putc(T[15]); |
JJting | 1:2823a39f70a9 | 263 | |
nylon0212 | 9:07de3af99031 | 264 | // while(1) { //開始傳到USB |
nylon0212 | 9:07de3af99031 | 265 | // if (uart.writeable() == 1) { |
nylon0212 | 9:07de3af99031 | 266 | // uart.putc(T[i]); |
nylon0212 | 9:07de3af99031 | 267 | // i++; |
nylon0212 | 9:07de3af99031 | 268 | // } |
nylon0212 | 9:07de3af99031 | 269 | // if (i >= sizeof(T)) { |
nylon0212 | 9:07de3af99031 | 270 | // i = 0; |
nylon0212 | 9:07de3af99031 | 271 | // break; |
nylon0212 | 9:07de3af99031 | 272 | // } |
JJting | 6:f48c51662e27 | 273 | // } |
JJting | 1:2823a39f70a9 | 274 | } |
JJting | 3:98cdee5d9b63 | 275 | |
JJting | 3:98cdee5d9b63 | 276 | float lpf(float input, float output_old, float frequency) |
JJting | 3:98cdee5d9b63 | 277 | { |
JJting | 3:98cdee5d9b63 | 278 | float output = 0; |
JJting | 3:98cdee5d9b63 | 279 | |
JJting | 3:98cdee5d9b63 | 280 | output = (output_old + frequency*Tf*input) / (1 + frequency*Tf); |
JJting | 3:98cdee5d9b63 | 281 | |
JJting | 3:98cdee5d9b63 | 282 | return output; |
nylon0212 | 9:07de3af99031 | 283 | } |
nylon0212 | 9:07de3af99031 | 284 | |
nylon0212 | 9:07de3af99031 | 285 | //有估測器有delay,丟資料給機器學習時就不是及時動作,所以之後決定直接丟資料 |