Chris LU / Mbed 2 deprecated MX28_Sensor_Correction

Sensor_Correction

Dependencies:   LSM9DS0 mbed

Fork of MX28_Sensor_Correction by LDSC_Robotics_TAs

main.cpp@2:bd98f7a4e231, 2017-02-13 (annotated)

Committer:
cpul5338
Date:
Mon Feb 13 14:35:01 2017 +0000
Revision:
2:bd98f7a4e231
Parent:
0:6dc8ac1c7c00 
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alan82914 0:6dc8ac1c7c00 1 #include "mbed.h"
alan82914 0:6dc8ac1c7c00 2 #include "LSM9DS0.h"
alan82914 0:6dc8ac1c7c00 3 #include "Mx28.h"
alan82914 0:6dc8ac1c7c00 4 #include <math.h>
alan82914 0:6dc8ac1c7c00 5
alan82914 0:6dc8ac1c7c00 6 // Dynamixel
alan82914 0:6dc8ac1c7c00 7 #define SERVO_ID 0x01 // ID of which we will set Dynamixel too
alan82914 0:6dc8ac1c7c00 8 #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.
alan82914 0:6dc8ac1c7c00 9 #define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps)
alan82914 0:6dc8ac1c7c00 10 #define TxPin A0
alan82914 0:6dc8ac1c7c00 11 #define RxPin A1
alan82914 0:6dc8ac1c7c00 12 #define CW_LIMIT_ANGLE 0x001 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
alan82914 0:6dc8ac1c7c00 13 #define CCW_LIMIT_ANGLE 0xFFF // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
alan82914 0:6dc8ac1c7c00 14
alan82914 0:6dc8ac1c7c00 15 #define pi 3.14
cpul5338 2:bd98f7a4e231 16 #define Ts 1 // sampling time
alan82914 0:6dc8ac1c7c00 17 #define A 20 // angle
cpul5338 2:bd98f7a4e231 18 #define f 0.5 // Hz
cpul5338 2:bd98f7a4e231 19 unsigned long tt = 0;
alan82914 0:6dc8ac1c7c00 20 double deg = 0;
alan82914 0:6dc8ac1c7c00 21 int Pos = 0;
alan82914 0:6dc8ac1c7c00 22 int zero = 190;
cpul5338 2:bd98f7a4e231 23 char mode = 'c';
alan82914 0:6dc8ac1c7c00 24 DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
alan82914 0:6dc8ac1c7c00 25 DigitalIn mybutton(USER_BUTTON);
alan82914 0:6dc8ac1c7c00 26
cpul5338 2:bd98f7a4e231 27 //
cpul5338 2:bd98f7a4e231 28 // Interrupt
cpul5338 2:bd98f7a4e231 29 //Ticker timer1;
cpul5338 2:bd98f7a4e231 30 // timer 1
cpul5338 2:bd98f7a4e231 31 #define LOOPTIME 1000 // 1 ms
cpul5338 2:bd98f7a4e231 32 unsigned long lastMilli = 0;
cpul5338 2:bd98f7a4e231 33 // sampling time
alan82914 0:6dc8ac1c7c00 34 float T = 0.001;
alan82914 0:6dc8ac1c7c00 35
cpul5338 2:bd98f7a4e231 36 Timer t;
cpul5338 2:bd98f7a4e231 37 DigitalOut tim1(PC_1);
cpul5338 2:bd98f7a4e231 38 DigitalOut tim2(PC_0);
cpul5338 2:bd98f7a4e231 39 Serial USB(USBTX, USBRX); // TX : D10 RX : D2 // serial 1
alan82914 0:6dc8ac1c7c00 40
alan82914 0:6dc8ac1c7c00 41 void init_uart(void);
alan82914 0:6dc8ac1c7c00 42 void separate(void);
alan82914 0:6dc8ac1c7c00 43 void uart_send(void);
cpul5338 2:bd98f7a4e231 44 //void timer1_interrupt(void);
alan82914 0:6dc8ac1c7c00 45 void change_mode();
alan82914 0:6dc8ac1c7c00 46 float lpf(float input, float output_old, float frequency);
alan82914 0:6dc8ac1c7c00 47
alan82914 0:6dc8ac1c7c00 48 // uart send data
alan82914 0:6dc8ac1c7c00 49 int display[6] = {0,0,0,0,0,0};
alan82914 0:6dc8ac1c7c00 50 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
alan82914 0:6dc8ac1c7c00 51
alan82914 0:6dc8ac1c7c00 52 void init_sensor(void);
alan82914 0:6dc8ac1c7c00 53 void read_sensor(void);
alan82914 0:6dc8ac1c7c00 54
alan82914 0:6dc8ac1c7c00 55 LSM9DS0 sensor(SPI_MODE, D9, D6); // SPI_CS1 : D7 , SPI_CS2 : D6
alan82914 0:6dc8ac1c7c00 56
alan82914 0:6dc8ac1c7c00 57 int sensor_flag = 0; // sensor initial flag
alan82914 0:6dc8ac1c7c00 58 int sensor_count = 0;
alan82914 0:6dc8ac1c7c00 59
alan82914 0:6dc8ac1c7c00 60 // sensor data
alan82914 0:6dc8ac1c7c00 61 int16_t Gyro_axis_data[3] = {0}; // X, Y, Z axis
alan82914 0:6dc8ac1c7c00 62 int16_t Acce_axis_data[3] = {0}; // X, Y, Z axis
cpul5338 2:bd98f7a4e231 63 int16_t Magn_axis_data[3] = {0}; // X, Y, Z axis
alan82914 0:6dc8ac1c7c00 64
alan82914 0:6dc8ac1c7c00 65 // sensor gain
cpul5338 2:bd98f7a4e231 66 #define Gyro_gain_x 0.002422966362920f
cpul5338 2:bd98f7a4e231 67 #define Gyro_gain_y 0.002422966362920f
cpul5338 2:bd98f7a4e231 68 #define Gyro_gain_z 0.002422966362920f // datasheet : 70 mdeg/digit
cpul5338 2:bd98f7a4e231 69 #define Acce_gain_x -0.002404245422390f // -9.81 / ( 3317.81 - (-764.05) )
cpul5338 2:bd98f7a4e231 70 #define Acce_gain_y -0.002344293490135f // -9.81 / ( 3476.34 - (-676.806))
cpul5338 2:bd98f7a4e231 71 #define Acce_gain_z -0.002307390759185f // -9.81 / ( 4423.63 - (285.406) )
cpul5338 2:bd98f7a4e231 72 #define Magn_gain 0
alan82914 0:6dc8ac1c7c00 73
alan82914 0:6dc8ac1c7c00 74 // sensor filter correction data
alan82914 0:6dc8ac1c7c00 75 float Gyro_axis_data_f[3];
alan82914 0:6dc8ac1c7c00 76 float Gyro_axis_data_f_old[3];
alan82914 0:6dc8ac1c7c00 77 float Gyro_scale[3]; // scale = (data - zero) * gain
cpul5338 2:bd98f7a4e231 78 float Gyro_axis_zero[3] = {50.095,56.249,1.862};
cpul5338 2:bd98f7a4e231 79 //Gyro offset
cpul5338 2:bd98f7a4e231 80 //**********************************************
cpul5338 2:bd98f7a4e231 81 //51.33333333 57.76969697 7.508080808
cpul5338 2:bd98f7a4e231 82 //49.26363636 55.94646465 -4.662626263
cpul5338 2:bd98f7a4e231 83 //49.68848849 55.03183183 2.74044044
cpul5338 2:bd98f7a4e231 84
cpul5338 2:bd98f7a4e231 85 //**********************************************
cpul5338 2:bd98f7a4e231 86
alan82914 0:6dc8ac1c7c00 87
alan82914 0:6dc8ac1c7c00 88 float Acce_axis_data_f[3];
alan82914 0:6dc8ac1c7c00 89 float Acce_axis_data_f_old[3];
alan82914 0:6dc8ac1c7c00 90 float Acce_scale[3]; // scale = (data - zero) * gain
cpul5338 2:bd98f7a4e231 91 float Acce_axis_zero[3] = {-651.6106,-697.9139,218.8919};
cpul5338 2:bd98f7a4e231 92 //Acce offset
cpul5338 2:bd98f7a4e231 93 //**********************************************
cpul5338 2:bd98f7a4e231 94 //-670.5135135 -669.5905906 4470.447447
cpul5338 2:bd98f7a4e231 95 //-632.7077077 3486.715716 186.8898899
cpul5338 2:bd98f7a4e231 96 //3428.671672 -726.2372372 250.8938939
cpul5338 2:bd98f7a4e231 97
cpul5338 2:bd98f7a4e231 98 //**********************************************
cpul5338 2:bd98f7a4e231 99
cpul5338 2:bd98f7a4e231 100
cpul5338 2:bd98f7a4e231 101 float Magn_axis_data_f[3];
cpul5338 2:bd98f7a4e231 102 float Magn_axis_data_f_old[3];
cpul5338 2:bd98f7a4e231 103 float Magn_scale[3]; // scale = (data - zero) * gain
cpul5338 2:bd98f7a4e231 104 float Magn_axis_zero[3] = {0,0,0};
alan82914 0:6dc8ac1c7c00 105
alan82914 0:6dc8ac1c7c00 106 int main()
alan82914 0:6dc8ac1c7c00 107 {
alan82914 0:6dc8ac1c7c00 108 dynamixelClass.setMode(SERVO_ID, SERVO, CW_LIMIT_ANGLE, CCW_LIMIT_ANGLE); // set mode to SERVO and set angle limits
cpul5338 2:bd98f7a4e231 109 // timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
cpul5338 2:bd98f7a4e231 110 USB.attach(&change_mode);
cpul5338 2:bd98f7a4e231 111 t.start();
alan82914 0:6dc8ac1c7c00 112
alan82914 0:6dc8ac1c7c00 113 init_uart();
alan82914 0:6dc8ac1c7c00 114 init_sensor();
cpul5338 2:bd98f7a4e231 115
alan82914 0:6dc8ac1c7c00 116 while(1)
alan82914 0:6dc8ac1c7c00 117 {
cpul5338 2:bd98f7a4e231 118 // timer 1
cpul5338 2:bd98f7a4e231 119 if((t.read_us() - lastMilli) >= LOOPTIME) // 2000 us = 2 ms
cpul5338 2:bd98f7a4e231 120 {
cpul5338 2:bd98f7a4e231 121 tim1 = 1;
cpul5338 2:bd98f7a4e231 122 lastMilli = t.read_us();
cpul5338 2:bd98f7a4e231 123
cpul5338 2:bd98f7a4e231 124 // sensor initial start
cpul5338 2:bd98f7a4e231 125 if(sensor_flag == 0)
cpul5338 2:bd98f7a4e231 126 {
cpul5338 2:bd98f7a4e231 127 sensor_count++;
alan82914 0:6dc8ac1c7c00 128
cpul5338 2:bd98f7a4e231 129 if(sensor_count >= 50)
cpul5338 2:bd98f7a4e231 130 {
cpul5338 2:bd98f7a4e231 131 sensor_flag = 1;
cpul5338 2:bd98f7a4e231 132 sensor_count = 0;
cpul5338 2:bd98f7a4e231 133 }
cpul5338 2:bd98f7a4e231 134 }
cpul5338 2:bd98f7a4e231 135 else
cpul5338 2:bd98f7a4e231 136 {
cpul5338 2:bd98f7a4e231 137 read_sensor();
cpul5338 2:bd98f7a4e231 138 Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18);
cpul5338 2:bd98f7a4e231 139 Acce_axis_data_f_old[0] = Acce_axis_data_f[0];
cpul5338 2:bd98f7a4e231 140 Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18);
cpul5338 2:bd98f7a4e231 141 Acce_axis_data_f_old[1] = Acce_axis_data_f[1];
cpul5338 2:bd98f7a4e231 142 Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18);
cpul5338 2:bd98f7a4e231 143 Acce_axis_data_f_old[2] = Acce_axis_data_f[2];
cpul5338 2:bd98f7a4e231 144
cpul5338 2:bd98f7a4e231 145 Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18);
cpul5338 2:bd98f7a4e231 146 Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0];
cpul5338 2:bd98f7a4e231 147 Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18);
cpul5338 2:bd98f7a4e231 148 Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1];
cpul5338 2:bd98f7a4e231 149 Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18);
cpul5338 2:bd98f7a4e231 150 Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
cpul5338 2:bd98f7a4e231 151
cpul5338 2:bd98f7a4e231 152 Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x;
cpul5338 2:bd98f7a4e231 153 Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y;
cpul5338 2:bd98f7a4e231 154 Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z;
cpul5338 2:bd98f7a4e231 155
cpul5338 2:bd98f7a4e231 156 Gyro_scale[0] = ((Gyro_axis_data_f[0]-Gyro_axis_zero[0]))*Gyro_gain_x;
cpul5338 2:bd98f7a4e231 157 Gyro_scale[1] = ((Gyro_axis_data_f[1]-Gyro_axis_zero[1]))*Gyro_gain_y;
cpul5338 2:bd98f7a4e231 158 Gyro_scale[2] = ((Gyro_axis_data_f[2]-Gyro_axis_zero[2]))*Gyro_gain_z;
cpul5338 2:bd98f7a4e231 159 switch (mode) {
cpul5338 2:bd98f7a4e231 160 case 'a':
cpul5338 2:bd98f7a4e231 161 deg = zero*4096.0f/360.0f;
cpul5338 2:bd98f7a4e231 162 break;
cpul5338 2:bd98f7a4e231 163 case 'b':
cpul5338 2:bd98f7a4e231 164 deg = (zero + 90)*4096.0f/360.0f;
cpul5338 2:bd98f7a4e231 165 break;
cpul5338 2:bd98f7a4e231 166 case 'c':
cpul5338 2:bd98f7a4e231 167 deg = (zero-A*sin(2*pi*f*tt*0.001*Ts))*4096.0f/360.0f; // 在正負A度搖
cpul5338 2:bd98f7a4e231 168 tt = tt + 1;
cpul5338 2:bd98f7a4e231 169 if(tt >= 1/(f*0.001*Ts)) // 2*pi*f*t*0.001*Ts = 2*pi -> t = 1/(f*0.001*T)
cpul5338 2:bd98f7a4e231 170 tt = 0;
cpul5338 2:bd98f7a4e231 171 break;
cpul5338 2:bd98f7a4e231 172 }
cpul5338 2:bd98f7a4e231 173 tim2 = 1;
cpul5338 2:bd98f7a4e231 174 dynamixelClass.servo(SERVO_ID,deg,0x400);
cpul5338 2:bd98f7a4e231 175 // Pos = dynamixelClass.readPosition(SERVO_ID);
cpul5338 2:bd98f7a4e231 176 tim2 = 0;
cpul5338 2:bd98f7a4e231 177 uart_send();
cpul5338 2:bd98f7a4e231 178 tim1 = 0;
cpul5338 2:bd98f7a4e231 179 }
cpul5338 2:bd98f7a4e231 180 }
cpul5338 2:bd98f7a4e231 181 } // while(1) end
alan82914 0:6dc8ac1c7c00 182 }
alan82914 0:6dc8ac1c7c00 183
alan82914 0:6dc8ac1c7c00 184 int i = 0;
alan82914 0:6dc8ac1c7c00 185 void uart_send(void)
alan82914 0:6dc8ac1c7c00 186 {
alan82914 0:6dc8ac1c7c00 187 // uart send data
cpul5338 2:bd98f7a4e231 188 display[0] = Acce_scale[0]*10;
cpul5338 2:bd98f7a4e231 189 display[1] = Acce_scale[1]*10;
cpul5338 2:bd98f7a4e231 190 display[2] = Acce_scale[2]*10;
cpul5338 2:bd98f7a4e231 191 display[3] = Gyro_scale[0]*10;
cpul5338 2:bd98f7a4e231 192 display[4] = Gyro_scale[1]*10;
cpul5338 2:bd98f7a4e231 193 display[5] = Gyro_scale[2]*10;
cpul5338 2:bd98f7a4e231 194
alan82914 0:6dc8ac1c7c00 195 separate();
alan82914 0:6dc8ac1c7c00 196
alan82914 0:6dc8ac1c7c00 197 int j = 0;
alan82914 0:6dc8ac1c7c00 198 int k = 1;
alan82914 0:6dc8ac1c7c00 199 while(k)
alan82914 0:6dc8ac1c7c00 200 {
cpul5338 2:bd98f7a4e231 201 if(USB.writeable())
alan82914 0:6dc8ac1c7c00 202 {
cpul5338 2:bd98f7a4e231 203 USB.putc(num[i]);
alan82914 0:6dc8ac1c7c00 204 i++;
alan82914 0:6dc8ac1c7c00 205 j++;
alan82914 0:6dc8ac1c7c00 206 }
alan82914 0:6dc8ac1c7c00 207
alan82914 0:6dc8ac1c7c00 208 if(j>1) // send 2 bytes
alan82914 0:6dc8ac1c7c00 209 {
alan82914 0:6dc8ac1c7c00 210 k = 0;
alan82914 0:6dc8ac1c7c00 211 j = 0;
alan82914 0:6dc8ac1c7c00 212 }
alan82914 0:6dc8ac1c7c00 213 }
alan82914 0:6dc8ac1c7c00 214
cpul5338 2:bd98f7a4e231 215 if(i>13)
alan82914 0:6dc8ac1c7c00 216 i = 0;
alan82914 0:6dc8ac1c7c00 217 }
alan82914 0:6dc8ac1c7c00 218
cpul5338 2:bd98f7a4e231 219 void read_sensor(void)
cpul5338 2:bd98f7a4e231 220 {
cpul5338 2:bd98f7a4e231 221 // sensor raw data
cpul5338 2:bd98f7a4e231 222 Gyro_axis_data[0] = sensor.readRawGyroX();
cpul5338 2:bd98f7a4e231 223 Gyro_axis_data[1] = sensor.readRawGyroY();
cpul5338 2:bd98f7a4e231 224 Gyro_axis_data[2] = sensor.readRawGyroZ();
cpul5338 2:bd98f7a4e231 225
cpul5338 2:bd98f7a4e231 226 Acce_axis_data[0] = sensor.readRawAccelX();
cpul5338 2:bd98f7a4e231 227 Acce_axis_data[1] = sensor.readRawAccelY();
cpul5338 2:bd98f7a4e231 228 Acce_axis_data[2] = sensor.readRawAccelZ();
cpul5338 2:bd98f7a4e231 229
cpul5338 2:bd98f7a4e231 230 Magn_axis_data[0] = sensor.readRawMagX();
cpul5338 2:bd98f7a4e231 231 Magn_axis_data[1] = sensor.readRawMagY();
cpul5338 2:bd98f7a4e231 232 Magn_axis_data[2] = sensor.readRawMagZ();
cpul5338 2:bd98f7a4e231 233 }
cpul5338 2:bd98f7a4e231 234
cpul5338 2:bd98f7a4e231 235 void init_uart()
cpul5338 2:bd98f7a4e231 236 {
cpul5338 2:bd98f7a4e231 237 USB.baud(57600); // 設定baudrate
cpul5338 2:bd98f7a4e231 238 }
cpul5338 2:bd98f7a4e231 239
alan82914 0:6dc8ac1c7c00 240 void separate(void)
alan82914 0:6dc8ac1c7c00 241 {
alan82914 0:6dc8ac1c7c00 242 num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列
alan82914 0:6dc8ac1c7c00 243 num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列
alan82914 0:6dc8ac1c7c00 244 num[4] = display[1] >> 8;
alan82914 0:6dc8ac1c7c00 245 num[5] = display[1] & 0x00ff;
alan82914 0:6dc8ac1c7c00 246 num[6] = display[2] >> 8;
alan82914 0:6dc8ac1c7c00 247 num[7] = display[2] & 0x00ff;
alan82914 0:6dc8ac1c7c00 248 num[8] = display[3] >> 8;
alan82914 0:6dc8ac1c7c00 249 num[9] = display[3] & 0x00ff;
alan82914 0:6dc8ac1c7c00 250 num[10] = display[4] >> 8;
alan82914 0:6dc8ac1c7c00 251 num[11] = display[4] & 0x00ff;
alan82914 0:6dc8ac1c7c00 252 num[12] = display[5] >> 8;
alan82914 0:6dc8ac1c7c00 253 num[13] = display[5] & 0x00ff;
alan82914 0:6dc8ac1c7c00 254 }
alan82914 0:6dc8ac1c7c00 255
alan82914 0:6dc8ac1c7c00 256 void init_sensor(void)
alan82914 0:6dc8ac1c7c00 257 {
alan82914 0:6dc8ac1c7c00 258 sensor.begin();
cpul5338 2:bd98f7a4e231 259 // sensor.begin() setting :
cpul5338 2:bd98f7a4e231 260 // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS,
cpul5338 2:bd98f7a4e231 261 // accel_scale aScl = A_SCALE_8G,
cpul5338 2:bd98f7a4e231 262 // mag_scale mScl = M_SCALE_8GS,
cpul5338 2:bd98f7a4e231 263 // gyro_odr gODR = G_ODR_760_BW_100,
cpul5338 2:bd98f7a4e231 264 // accel_odr aODR = A_ODR_800,
cpul5338 2:bd98f7a4e231 265 // mag_odr mODR = M_ODR_100);
alan82914 0:6dc8ac1c7c00 266 }
alan82914 0:6dc8ac1c7c00 267
alan82914 0:6dc8ac1c7c00 268 float lpf(float input, float output_old, float frequency)
alan82914 0:6dc8ac1c7c00 269 {
alan82914 0:6dc8ac1c7c00 270 float output = 0;
alan82914 0:6dc8ac1c7c00 271
alan82914 0:6dc8ac1c7c00 272 output = (output_old + frequency*T*input) / (1 + frequency*T);
alan82914 0:6dc8ac1c7c00 273
alan82914 0:6dc8ac1c7c00 274 return output;
alan82914 0:6dc8ac1c7c00 275 }
cpul5338 2:bd98f7a4e231 276
cpul5338 2:bd98f7a4e231 277 void change_mode()
cpul5338 2:bd98f7a4e231 278 {
cpul5338 2:bd98f7a4e231 279 mode = USB.getc();
cpul5338 2:bd98f7a4e231 280 }