![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
20181123
Diff: main.cpp
- Revision:
- 6:f1c0b9476c88
- Parent:
- 5:131450b16ce3
--- a/main.cpp Fri Aug 31 01:41:04 2018 +0000 +++ b/main.cpp Fri Nov 23 04:43:52 2018 +0000 @@ -1,7 +1,8 @@ -#include "mbed.h" +#include "mbed.h" //不用gyro的data,只要輸出兩個imu的acce的xyz資料即可 #include "encoder.h" #include "Mx28.h" #include "PID.h" +#include "LSM9DS1.h" //********************* Dynamxiel ****************************** #define SERVO_ID 0x01 // ID of which we will set Dynamixel too @@ -12,32 +13,6 @@ #define CW_LIMIT_ANGLE 1 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode #define CCW_LIMIT_ANGLE 4095 // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode #define PI 3.14159265f -//*************************************************************** - -Serial uart(USBTX, USBRX); -//Serial uart(D10,D2); // TX : D10 RX : D2 // blueteeth -DigitalOut LED(A4); // check if the code is running -DigitalOut led2(A5); // check if the code is running interrupt -uint8_t led2f; - -// Timer -Ticker timer1; -float ITR_time1 = 4000.0; // unit:us -float Ts = ITR_time1/1000000; -uint8_t flag; - -// uart_tx -union splitter { - short j; - char C[2]; - // C[0] is lowbyte of j, C[1] is highbyte of j -}; -char T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; -int i = 0; - -// PID -PID motor_pid(1500, 0, 40, Ts);// 6.4 0.13 7.2 0.13 8.4 6.5, 0, 0.19 -float PIDout = 0.0f; // Dynamixel DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin); @@ -53,7 +28,7 @@ float torque_measured = 0.0; float ks = 2.6393*2; //spring constant //int angle_dif = 0; -float torque_ref = 0.0; +float torque_ref = 0.0; //可不令為0,而是以范僑芯的學習結果為input。令為0可使馬達扭力輸出為0,抵抗減速機摩擦力,使穿戴者感到跟沒有戴一樣。 //float friction = 0.0f; float friction = 0.18f; float rate = 0.5; @@ -61,17 +36,61 @@ //float check = 0.0f; float Angle_Dif; short rotation_; +//*************************************************************** + +Serial uart(USBTX, USBRX); +//Serial uart(D10,D2); // TX : D10 RX : D2 // bluetooth +AnalogIn EMG(PC_4); // +DigitalOut LED(A4); // check if the code is running +DigitalOut led2(A5); // check if the code is running interrupt +uint8_t led2f; +/*IMU*******************************************************************/ +LSM9DS1 imu(D14, D15); //SDA SCL +LSM9DS1 imu2(PC_9,PA_8); //SDA SCL +void init_IMU(); +int16_t Gyro_axis_data[6] = {0}; // X, Y, Z axis +int16_t Acce_axis_data[6] = {0}; // X, Y, Z axis +float Acce_axis_data_f[6] = {0}; //_f代表經過低通濾波的資料 +float Acce_axis_data_f_old[6] = {0}; +float Gyro_axis_data_f[6] = {0}; +float Gyro_axis_data_f_old[6] = {0}; + +// Timer +Ticker timer1; +float ITR_time1 = 4000.0; //timer interrupt unit:us 多少us計時一次 4毫秒 +float Ts = ITR_time1/1000000; //控馬達的某個時間參數 不用理他 + +// PID +PID motor_pid(1500, 0, 40, Ts);// 6.4 0.13 7.2 0.13 8.4 6.5, 0, 0.19 +float PIDout = 0.0f; + +// EMG +float lpf(float input, float output_old, float frequency); //low pass filter +float emg_value; +float emg_value_f; +float emg_value_f_old; +float Tf = ITR_time1/1000000; // 低通濾波的採樣週期 + +// uart_tx +union splitter { //將data切割為兩個byte + short j; + char C[2]; + // C[0] is lowbyte of j, C[1] is highbyte of j +}; +uint8_t T[40] = {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}; +// T[16]中的16是2+14,2為start btye 255 255,14為要傳7個參數到電腦,拆成high byte和low byte,所以是7*2=14 +// 若要更改傳到電腦的參數個數,則改x*2=y,x為要傳的參數個數,y為char T[]裡面要有多少個0 +int i = 0; // function void init_UART(); +void uart_tx(); // 傳data到電腦 void init_TIMER(); void timer1_ITR(); -void uart_tx(); - void init_DYNAMIXEL(); void D_angle_measure(); void find_torque(); - +/**************************************************************************/ int main() { LED = 1; // darken @@ -79,27 +98,73 @@ // initial sensor init_SPI_encoder(); init_encoder(); + init_UART(); init_DYNAMIXEL(); - // initial uart - init_UART(); - - wait_ms(500); - - led2 = 1; -// led2f = 0; - LED = 0; // lighten - + init_IMU(); init_TIMER(); while(1) { + // EMG + //emg_value = EMG.read(); +// emg_value_f = lpf(emg_value,emg_value_f_old,15); +// emg_value_f_old = emg_value_f; + // IMU + imu.readAccel(); + imu.readGyro(); + imu2.readAccel(); + imu2.readGyro(); + + Acce_axis_data[0] = imu.ax*Acce_gain_x; + Acce_axis_data[1] = imu.ay*Acce_gain_y; + Acce_axis_data[2] = imu.az*Acce_gain_z; + Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2; + Acce_axis_data[4] = imu2.az*Acce_gain_y_2; + Acce_axis_data[5] = imu2.ay*Acce_gain_z_2; + + Gyro_axis_data[0] = imu.gx*Gyro_gain_x; + Gyro_axis_data[1] = imu.gy*Gyro_gain_y; + Gyro_axis_data[2] = imu.gz*Gyro_gain_z; + Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2; + Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2; + Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2; + + for(i=0; i<6; 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]; + Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15); + Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i]; } +// int16_t shit2 = imu.ax_raw; +// uart.printf("%d\n\r", shit2); +// uart.printf("x: %.4f, y: %.4f, z: %.4f\n\r", imu.ax, imu.ay, imu.az); + uart_tx(); + init_TIMER(); + while(1) {} + +// wait_ms(500); + } +} + +void init_IMU() +{ + uint16_t shit; + 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 +// 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); + if(shit != 0x683D) { + while(1) { + uart.printf("IMU error\n\r"); + } + } + uart.printf("IMU init success\n\r"); + +// uart.printf("%d\n\r", shit); } void init_DYNAMIXEL() { // dynamixelClass.torqueMode(SERVO_ID, 1); - dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0); + dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0); //Wheel:= continuously rotating; others: torque, position wait_ms(1); } @@ -113,7 +178,7 @@ timer1.attach_us(&timer1_ITR, ITR_time1); } -void timer1_ITR() +void timer1_ITR() //開始讀取資料 { led2 = !led2; angle_measure(); @@ -121,28 +186,7 @@ find_torque(); motor_pid.Compute(torque_ref, torque_measured); PIDout = motor_pid.output; -// servo_cmd = -PIDout*121.8f; // 1023/8.4Nm = 121.7857 - servo_cmd = -PIDout; // 1023/8.4Nm = 121.7857 -/* - // 電流控制 - if (servo_cmd > 0) { - // servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f; - servo_cmd = servo_cmd; - if (servo_cmd >= 1023) - servo_cmd = 1023; - } else { - // servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f; - servo_cmd = -servo_cmd + 1024; - if (servo_cmd >= 2047) - servo_cmd = 2047; - } - - if (servo_cmd >= 1023) { - row_cmd = -(servo_cmd-1023); - } else { - row_cmd = servo_cmd; - } -*/ + servo_cmd = -PIDout; // 1023/8.4Nm = 121.7857 可視情況調整負號 // 速度控制 if (servo_cmd > 0) { row_cmd = servo_cmd; @@ -165,12 +209,12 @@ } // dynamixelClass.torque(SERVO_ID, servo_cmd); - dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd); //0~1023 (rotation) + dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd); //0~1023 (rotation) 12V之下的最高轉速為45rpm,切割為等分 // dynamixelClass.wheel(SERVO_ID, 0, 150); //0~1023 (rotation) uart_tx(); } -void uart_tx() +void uart_tx() //分割資料 { splitter s1; splitter s2; @@ -179,17 +223,38 @@ splitter s5; splitter s6; splitter s7; + splitter s8; + splitter s9; + splitter s10; + splitter s11; + splitter s12; + splitter s13; + splitter s14; + splitter s15; + splitter s16; + splitter s17; + splitter s18; + splitter s19; - s1.j = torque_ref*1000; - s2.j = torque_measured*1000; - s3.j = Angle_Dif/4096*3600; -// s3.j = servo_cmd; -// s4.j = 1; -// s5.j = 3; - s4.j = D_Angle; - s5.j = Angle*3; - s6.j = row_cmd; - s7.j = 1; + s1.j = Acce_axis_data_f[0]; // 0x6161;// + s2.j = Acce_axis_data_f[1]; + s3.j = Acce_axis_data_f[2]; + s4.j = Acce_axis_data_f[3]; + s5.j = Acce_axis_data_f[4]; + s6.j = Acce_axis_data_f[5]; + s7.j = Gyro_axis_data_f[0]; + s8.j = Gyro_axis_data_f[1]; + s9.j = Gyro_axis_data_f[2]; + s10.j = Gyro_axis_data_f[3]; + s11.j = Gyro_axis_data_f[4]; + s12.j = Gyro_axis_data_f[5]; + s13.j = emg_value_f; + s14.j = torque_ref*1000; //即reference input指令 + s15.j = torque_measured*1000; //由angle_difference*ks而來 + s16.j = Angle_Dif/4096*3600; + s17.j = D_Angle; + s18.j = Angle*3; //經過減速比 + s19.j = row_cmd; T[2] = s1.C[0]; T[3] = s1.C[1]; @@ -205,19 +270,46 @@ T[13] = s6.C[1]; T[14] = s7.C[0]; T[15] = s7.C[1]; + T[16] = s8.C[0]; + T[17] = s8.C[1]; + T[18] = s9.C[0]; + T[19] = s9.C[1]; + T[20] = s10.C[0]; + T[21] = s10.C[1]; + T[22] = s11.C[0]; + T[23] = s11.C[1]; + T[24] = s12.C[0]; + T[25] = s12.C[1]; + T[26] = s13.C[0]; + T[27] = s13.C[1]; + T[28] = s14.C[0]; + T[29] = s14.C[1]; + T[30] = s15.C[0]; + T[31] = s15.C[1]; + T[32] = s16.C[0]; + T[33] = s16.C[1]; + T[34] = s17.C[0]; + T[35] = s17.C[1]; + T[36] = s18.C[0]; + T[37] = s18.C[1]; + T[38] = s19.C[0]; + T[39] = s19.C[1]; - while(1) { - if (uart.writeable() == 1) { - uart.putc(T[i]); - i++; - } - if (i >= sizeof(T)) { - i = 0; - break; - } + for(int k=0;k<40;k++) + { + uart.putc(T[k]); } } +float lpf(float input, float output_old, float frequency) +{ + float output = 0; + + output = (output_old + frequency*Tf*input) / (1 + frequency*Tf); + + return output; +} + void D_angle_measure() { D_angle = dynamixelClass.readPosition(SERVO_ID); @@ -251,3 +343,4 @@ torque_measured = angle_difference*ks; } +