![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Sensor correction
Fork of MX28_Sensor_Correction by
Revision 2:bd98f7a4e231, committed 2017-02-13
- Comitter:
- cpul5338
- Date:
- Mon Feb 13 14:35:01 2017 +0000
- Parent:
- 1:3643ef2599cd
- Commit message:
- update
Changed in this revision
LSM9DS0.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/LSM9DS0.lib Mon Feb 13 05:05:52 2017 +0000 +++ b/LSM9DS0.lib Mon Feb 13 14:35:01 2017 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/teams/LDSC_Robotics_TAs/code/MX28_Calibration/#993011c773be +https://developer.mbed.org/users/cpul5338/code/LSM9DS0/#c2241ae1eb3f
--- a/main.cpp Mon Feb 13 05:05:52 2017 +0000 +++ b/main.cpp Mon Feb 13 14:35:01 2017 +0000 @@ -13,39 +13,36 @@ #define CCW_LIMIT_ANGLE 0xFFF // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode #define pi 3.14 -#define Ts 5 // sampling time +#define Ts 1 // sampling time #define A 20 // angle -#define f 0.2 // Hz - -unsigned long t = 0; +#define f 0.5 // Hz +unsigned long tt = 0; double deg = 0; int Pos = 0; int zero = 190; -char mode = 'a'; - -Serial pc(SERIAL_TX, SERIAL_RX); - +char mode = 'c'; DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin); -// DigitalIn mybutton(USER_BUTTON); -// sampling time Lowpass Filter +// +// Interrupt +//Ticker timer1; +// timer 1 +#define LOOPTIME 1000 // 1 ms +unsigned long lastMilli = 0; +// sampling time float T = 0.001; -// Interrupt -Ticker timer1; +Timer t; +DigitalOut tim1(PC_1); +DigitalOut tim2(PC_0); +Serial USB(USBTX, USBRX); // TX : D10 RX : D2 // serial 1 -// UART -Serial uart_1(D10,D2); // TX : D10 RX : D2 // serial 1 - -// void init_uart(void); void separate(void); void uart_send(void); -void init_TIMER(void); -void timer1_interrupt(void); +//void timer1_interrupt(void); void change_mode(); - float lpf(float input, float output_old, float frequency); // uart send data @@ -54,7 +51,6 @@ void init_sensor(void); void read_sensor(void); -void sensor_filter(void); LSM9DS0 sensor(SPI_MODE, D9, D6); // SPI_CS1 : D7 , SPI_CS2 : D6 @@ -64,67 +60,147 @@ // sensor data int16_t Gyro_axis_data[3] = {0}; // X, Y, Z axis int16_t Acce_axis_data[3] = {0}; // X, Y, Z axis +int16_t Magn_axis_data[3] = {0}; // X, Y, Z axis // sensor gain -#define Gyro_gain_x 0 -#define Gyro_gain_y 0 -#define Gyro_gain_z 0 // datasheet : 70 mdeg/digit -#define Acce_gain_x 0 -#define Acce_gain_y 0 -#define Acce_gain_z 0 +#define Gyro_gain_x 0.002422966362920f +#define Gyro_gain_y 0.002422966362920f +#define Gyro_gain_z 0.002422966362920f // datasheet : 70 mdeg/digit +#define Acce_gain_x -0.002404245422390f // -9.81 / ( 3317.81 - (-764.05) ) +#define Acce_gain_y -0.002344293490135f // -9.81 / ( 3476.34 - (-676.806)) +#define Acce_gain_z -0.002307390759185f // -9.81 / ( 4423.63 - (285.406) ) +#define Magn_gain 0 // sensor filter correction data float Gyro_axis_data_f[3]; float Gyro_axis_data_f_old[3]; float Gyro_scale[3]; // scale = (data - zero) * gain -float Gyro_axis_zero[3] = {0,0,0}; +float Gyro_axis_zero[3] = {50.095,56.249,1.862}; +//Gyro offset +//********************************************** +//51.33333333 57.76969697 7.508080808 +//49.26363636 55.94646465 -4.662626263 +//49.68848849 55.03183183 2.74044044 + +//********************************************** + float Acce_axis_data_f[3]; float Acce_axis_data_f_old[3]; float Acce_scale[3]; // scale = (data - zero) * gain -float Acce_axis_zero[3] = {0,0,0}; +float Acce_axis_zero[3] = {-651.6106,-697.9139,218.8919}; +//Acce offset +//********************************************** +//-670.5135135 -669.5905906 4470.447447 +//-632.7077077 3486.715716 186.8898899 +//3428.671672 -726.2372372 250.8938939 + +//********************************************** + + +float Magn_axis_data_f[3]; +float Magn_axis_data_f_old[3]; +float Magn_scale[3]; // scale = (data - zero) * gain +float Magn_axis_zero[3] = {0,0,0}; int main() { - pc.baud(115200); dynamixelClass.setMode(SERVO_ID, SERVO, CW_LIMIT_ANGLE, CCW_LIMIT_ANGLE); // set mode to SERVO and set angle limits +// timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz) + USB.attach(&change_mode); + t.start(); init_uart(); init_sensor(); - init_TIMER(); - uart_1.attach(&change_mode); - + while(1) { - pc.printf("%d\n",Pos); - } // while(1) end -} + // timer 1 + if((t.read_us() - lastMilli) >= LOOPTIME) // 2000 us = 2 ms + { + tim1 = 1; + lastMilli = t.read_us(); + + // sensor initial start + if(sensor_flag == 0) + { + sensor_count++; -// UART -void init_uart() -{ - uart_1.baud(115200); // 設定baudrate + if(sensor_count >= 50) + { + sensor_flag = 1; + sensor_count = 0; + } + } + else + { + read_sensor(); + Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18); + Acce_axis_data_f_old[0] = Acce_axis_data_f[0]; + Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18); + Acce_axis_data_f_old[1] = Acce_axis_data_f[1]; + Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18); + Acce_axis_data_f_old[2] = Acce_axis_data_f[2]; + + Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18); + Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0]; + Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18); + Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1]; + Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18); + Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2]; + + Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x; + Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y; + Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z; + + Gyro_scale[0] = ((Gyro_axis_data_f[0]-Gyro_axis_zero[0]))*Gyro_gain_x; + Gyro_scale[1] = ((Gyro_axis_data_f[1]-Gyro_axis_zero[1]))*Gyro_gain_y; + Gyro_scale[2] = ((Gyro_axis_data_f[2]-Gyro_axis_zero[2]))*Gyro_gain_z; + switch (mode) { + case 'a': + deg = zero*4096.0f/360.0f; + break; + case 'b': + deg = (zero + 90)*4096.0f/360.0f; + break; + case 'c': + deg = (zero-A*sin(2*pi*f*tt*0.001*Ts))*4096.0f/360.0f; // 在正負A度搖 + tt = tt + 1; + if(tt >= 1/(f*0.001*Ts)) // 2*pi*f*t*0.001*Ts = 2*pi -> t = 1/(f*0.001*T) + tt = 0; + break; + } + tim2 = 1; + dynamixelClass.servo(SERVO_ID,deg,0x400); +// Pos = dynamixelClass.readPosition(SERVO_ID); + tim2 = 0; + uart_send(); + tim1 = 0; + } + } + } // while(1) end } int i = 0; void uart_send(void) { // uart send data - display[0] = Gyro_axis_data[0]; - display[1] = Gyro_axis_data[1]; - display[2] = Gyro_axis_data[2]; - display[3] = Acce_axis_data[0]; - display[4] = Acce_axis_data[1]; - display[5] = Acce_axis_data[2]; + display[0] = Acce_scale[0]*10; + display[1] = Acce_scale[1]*10; + display[2] = Acce_scale[2]*10; + display[3] = Gyro_scale[0]*10; + display[4] = Gyro_scale[1]*10; + display[5] = Gyro_scale[2]*10; + separate(); int j = 0; int k = 1; while(k) { - if(uart_1.writeable()) + if(USB.writeable()) { - uart_1.putc(num[i]); + USB.putc(num[i]); i++; j++; } @@ -136,10 +212,31 @@ } } - if(i>15) + if(i>13) i = 0; } +void read_sensor(void) +{ + // sensor raw data + Gyro_axis_data[0] = sensor.readRawGyroX(); + Gyro_axis_data[1] = sensor.readRawGyroY(); + Gyro_axis_data[2] = sensor.readRawGyroZ(); + + Acce_axis_data[0] = sensor.readRawAccelX(); + Acce_axis_data[1] = sensor.readRawAccelY(); + Acce_axis_data[2] = sensor.readRawAccelZ(); + + Magn_axis_data[0] = sensor.readRawMagX(); + Magn_axis_data[1] = sensor.readRawMagY(); + Magn_axis_data[2] = sensor.readRawMagZ(); +} + +void init_uart() +{ + USB.baud(57600); // 設定baudrate +} + void separate(void) { num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列 @@ -156,96 +253,16 @@ num[13] = display[5] & 0x00ff; } -// sensor void init_sensor(void) { sensor.begin(); -} - -void read_sensor(void) -{ - // sensor raw data - Gyro_axis_data[0] = sensor.readRawGyroX(); - Gyro_axis_data[1] = sensor.readRawGyroY(); - Gyro_axis_data[2] = sensor.readRawGyroZ(); - - Acce_axis_data[0] = sensor.readRawAccelX(); - Acce_axis_data[1] = sensor.readRawAccelY(); - Acce_axis_data[2] = sensor.readRawAccelZ(); -} - -void sensor_filter(void) -{ - // gyro filter - Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18); - Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0]; - Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18); - Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1]; - Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18); - Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2]; - - // acce filter - Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18); - Acce_axis_data_f_old[0] = Acce_axis_data_f[0]; - Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18); - Acce_axis_data_f_old[1] = Acce_axis_data_f[1]; - Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18); - Acce_axis_data_f_old[2] = Acce_axis_data_f[2]; - - Gyro_scale[0] = (Gyro_axis_data_f[0]-Gyro_axis_zero[0])*Gyro_gain_x; - Gyro_scale[1] = (Gyro_axis_data_f[1]-Gyro_axis_zero[1])*Gyro_gain_y; - Gyro_scale[2] = (Gyro_axis_data_f[2]-Gyro_axis_zero[2])*Gyro_gain_z; - - Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x; - Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y; - Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z; -} - -// Timer -void init_TIMER(void) -{ - timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz) -} - -void timer1_interrupt(void) -{ - // sensor initial start - if(sensor_flag == 0) { - sensor_count++; - - if(sensor_count >= 50) { - sensor_flag = 1; - sensor_count = 0; - } - } else { - read_sensor(); - sensor_filter(); - uart_send(); - } - - switch (mode) { - case 'a': - deg = zero*4096.0f/360.0f; - break; - case 'b': - deg = (zero + 90)*4096.0f/360.0f; - break; - case 'c': - deg = (zero-A*sin(2*pi*f*t*0.001*Ts))*4096.0f/360.0f; // 在正負A度搖 - t = t + 1; - if(t >= 1/(f*0.001*Ts)) // 2*pi*f*t*0.001*Ts = 2*pi -> t = 1/(f*0.001*T) - t = 0; - break; - - } - - dynamixelClass.servo(SERVO_ID,deg,0x400); - Pos = dynamixelClass.readPosition(SERVO_ID); -} - -void change_mode() -{ - mode = uart_1.getc(); + // sensor.begin() setting : + // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS, + // accel_scale aScl = A_SCALE_8G, + // mag_scale mScl = M_SCALE_8GS, + // gyro_odr gODR = G_ODR_760_BW_100, + // accel_odr aODR = A_ODR_800, + // mag_odr mODR = M_ODR_100); } float lpf(float input, float output_old, float frequency) @@ -256,3 +273,8 @@ return output; } + +void change_mode() +{ + mode = USB.getc(); +} \ No newline at end of file