![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Sensor correction
Fork of MX28_Sensor_Correction by
main.cpp
- Committer:
- cpul5338
- Date:
- 2017-02-13
- Revision:
- 2:bd98f7a4e231
- Parent:
- 0:6dc8ac1c7c00
File content as of revision 2:bd98f7a4e231:
#include "mbed.h" #include "LSM9DS0.h" #include "Mx28.h" #include <math.h> // Dynamixel #define SERVO_ID 0x01 // ID of which we will set Dynamixel too #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. #define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps) #define TxPin A0 #define RxPin A1 #define CW_LIMIT_ANGLE 0x001 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode #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 1 // sampling time #define A 20 // angle #define f 0.5 // Hz unsigned long tt = 0; double deg = 0; int Pos = 0; int zero = 190; char mode = 'c'; DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin); DigitalIn mybutton(USER_BUTTON); // // Interrupt //Ticker timer1; // timer 1 #define LOOPTIME 1000 // 1 ms unsigned long lastMilli = 0; // sampling time float T = 0.001; Timer t; DigitalOut tim1(PC_1); DigitalOut tim2(PC_0); Serial USB(USBTX, USBRX); // TX : D10 RX : D2 // serial 1 void init_uart(void); void separate(void); void uart_send(void); //void timer1_interrupt(void); void change_mode(); float lpf(float input, float output_old, float frequency); // uart send data int display[6] = {0,0,0,0,0,0}; char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0}; // char num[0] , num[1] : 2 start byte void init_sensor(void); void read_sensor(void); LSM9DS0 sensor(SPI_MODE, D9, D6); // SPI_CS1 : D7 , SPI_CS2 : D6 int sensor_flag = 0; // sensor initial flag int sensor_count = 0; // 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.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] = {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] = {-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() { 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(); while(1) { // 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++; 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] = 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(USB.writeable()) { USB.putc(num[i]); i++; j++; } if(j>1) // send 2 bytes { k = 0; j = 0; } } 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)資料存入陣列 num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列 num[4] = display[1] >> 8; num[5] = display[1] & 0x00ff; num[6] = display[2] >> 8; num[7] = display[2] & 0x00ff; num[8] = display[3] >> 8; num[9] = display[3] & 0x00ff; num[10] = display[4] >> 8; num[11] = display[4] & 0x00ff; num[12] = display[5] >> 8; num[13] = display[5] & 0x00ff; } void init_sensor(void) { sensor.begin(); // 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) { float output = 0; output = (output_old + frequency*T*input) / (1 + frequency*T); return output; } void change_mode() { mode = USB.getc(); }