Chris LU / Mbed 2 deprecated MX28_Sensor_Correction

Sensor_Correction

Dependencies:   LSM9DS0 mbed

Fork of MX28_Sensor_Correction by LDSC_Robotics_TAs

main.cpp@0:6dc8ac1c7c00, 2017-02-13 (annotated)

Committer:
alan82914
Date:
Mon Feb 13 04:52:05 2017 +0000
Revision:
0:6dc8ac1c7c00
Child:
2:bd98f7a4e231
SMD

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
alan82914 0:6dc8ac1c7c00 16 #define Ts 5 // sampling time
alan82914 0:6dc8ac1c7c00 17 #define A 20 // angle
alan82914 0:6dc8ac1c7c00 18 #define f 0.2 // Hz
alan82914 0:6dc8ac1c7c00 19
alan82914 0:6dc8ac1c7c00 20 unsigned long t = 0;
alan82914 0:6dc8ac1c7c00 21 double deg = 0;
alan82914 0:6dc8ac1c7c00 22 int Pos = 0;
alan82914 0:6dc8ac1c7c00 23 int zero = 190;
alan82914 0:6dc8ac1c7c00 24 char mode = 'a';
alan82914 0:6dc8ac1c7c00 25
alan82914 0:6dc8ac1c7c00 26 Serial pc(SERIAL_TX, SERIAL_RX);
alan82914 0:6dc8ac1c7c00 27
alan82914 0:6dc8ac1c7c00 28 DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
alan82914 0:6dc8ac1c7c00 29 //
alan82914 0:6dc8ac1c7c00 30 DigitalIn mybutton(USER_BUTTON);
alan82914 0:6dc8ac1c7c00 31
alan82914 0:6dc8ac1c7c00 32 // sampling time Lowpass Filter
alan82914 0:6dc8ac1c7c00 33 float T = 0.001;
alan82914 0:6dc8ac1c7c00 34
alan82914 0:6dc8ac1c7c00 35 // Interrupt
alan82914 0:6dc8ac1c7c00 36 Ticker timer1;
alan82914 0:6dc8ac1c7c00 37
alan82914 0:6dc8ac1c7c00 38 // UART
alan82914 0:6dc8ac1c7c00 39 Serial uart_1(D10,D2); // TX : D10 RX : D2 // serial 1
alan82914 0:6dc8ac1c7c00 40
alan82914 0:6dc8ac1c7c00 41 //
alan82914 0:6dc8ac1c7c00 42 void init_uart(void);
alan82914 0:6dc8ac1c7c00 43 void separate(void);
alan82914 0:6dc8ac1c7c00 44 void uart_send(void);
alan82914 0:6dc8ac1c7c00 45 void init_TIMER(void);
alan82914 0:6dc8ac1c7c00 46 void timer1_interrupt(void);
alan82914 0:6dc8ac1c7c00 47 void change_mode();
alan82914 0:6dc8ac1c7c00 48
alan82914 0:6dc8ac1c7c00 49 float lpf(float input, float output_old, float frequency);
alan82914 0:6dc8ac1c7c00 50
alan82914 0:6dc8ac1c7c00 51 // uart send data
alan82914 0:6dc8ac1c7c00 52 int display[6] = {0,0,0,0,0,0};
alan82914 0:6dc8ac1c7c00 53 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 54
alan82914 0:6dc8ac1c7c00 55 void init_sensor(void);
alan82914 0:6dc8ac1c7c00 56 void read_sensor(void);
alan82914 0:6dc8ac1c7c00 57 void sensor_filter(void);
alan82914 0:6dc8ac1c7c00 58
alan82914 0:6dc8ac1c7c00 59 LSM9DS0 sensor(SPI_MODE, D9, D6); // SPI_CS1 : D7 , SPI_CS2 : D6
alan82914 0:6dc8ac1c7c00 60
alan82914 0:6dc8ac1c7c00 61 int sensor_flag = 0; // sensor initial flag
alan82914 0:6dc8ac1c7c00 62 int sensor_count = 0;
alan82914 0:6dc8ac1c7c00 63
alan82914 0:6dc8ac1c7c00 64 // sensor data
alan82914 0:6dc8ac1c7c00 65 int16_t Gyro_axis_data[3] = {0}; // X, Y, Z axis
alan82914 0:6dc8ac1c7c00 66 int16_t Acce_axis_data[3] = {0}; // X, Y, Z axis
alan82914 0:6dc8ac1c7c00 67
alan82914 0:6dc8ac1c7c00 68 // sensor gain
alan82914 0:6dc8ac1c7c00 69 #define Gyro_gain_x 0
alan82914 0:6dc8ac1c7c00 70 #define Gyro_gain_y 0
alan82914 0:6dc8ac1c7c00 71 #define Gyro_gain_z 0 // datasheet : 70 mdeg/digit
alan82914 0:6dc8ac1c7c00 72 #define Acce_gain_x 0
alan82914 0:6dc8ac1c7c00 73 #define Acce_gain_y 0
alan82914 0:6dc8ac1c7c00 74 #define Acce_gain_z 0
alan82914 0:6dc8ac1c7c00 75
alan82914 0:6dc8ac1c7c00 76 // sensor filter correction data
alan82914 0:6dc8ac1c7c00 77 float Gyro_axis_data_f[3];
alan82914 0:6dc8ac1c7c00 78 float Gyro_axis_data_f_old[3];
alan82914 0:6dc8ac1c7c00 79 float Gyro_scale[3]; // scale = (data - zero) * gain
alan82914 0:6dc8ac1c7c00 80 float Gyro_axis_zero[3] = {0,0,0};
alan82914 0:6dc8ac1c7c00 81
alan82914 0:6dc8ac1c7c00 82 float Acce_axis_data_f[3];
alan82914 0:6dc8ac1c7c00 83 float Acce_axis_data_f_old[3];
alan82914 0:6dc8ac1c7c00 84 float Acce_scale[3]; // scale = (data - zero) * gain
alan82914 0:6dc8ac1c7c00 85 float Acce_axis_zero[3] = {0,0,0};
alan82914 0:6dc8ac1c7c00 86
alan82914 0:6dc8ac1c7c00 87 int main()
alan82914 0:6dc8ac1c7c00 88 {
alan82914 0:6dc8ac1c7c00 89 pc.baud(115200);
alan82914 0:6dc8ac1c7c00 90 dynamixelClass.setMode(SERVO_ID, SERVO, CW_LIMIT_ANGLE, CCW_LIMIT_ANGLE); // set mode to SERVO and set angle limits
alan82914 0:6dc8ac1c7c00 91
alan82914 0:6dc8ac1c7c00 92 init_uart();
alan82914 0:6dc8ac1c7c00 93 init_sensor();
alan82914 0:6dc8ac1c7c00 94 init_TIMER();
alan82914 0:6dc8ac1c7c00 95 uart_1.attach(&change_mode);
alan82914 0:6dc8ac1c7c00 96
alan82914 0:6dc8ac1c7c00 97 while(1)
alan82914 0:6dc8ac1c7c00 98 {
alan82914 0:6dc8ac1c7c00 99 pc.printf("%d\n",Pos);
alan82914 0:6dc8ac1c7c00 100 } // while(1) end
alan82914 0:6dc8ac1c7c00 101 }
alan82914 0:6dc8ac1c7c00 102
alan82914 0:6dc8ac1c7c00 103 // UART
alan82914 0:6dc8ac1c7c00 104 void init_uart()
alan82914 0:6dc8ac1c7c00 105 {
alan82914 0:6dc8ac1c7c00 106 uart_1.baud(115200); // 設定baudrate
alan82914 0:6dc8ac1c7c00 107 }
alan82914 0:6dc8ac1c7c00 108
alan82914 0:6dc8ac1c7c00 109 int i = 0;
alan82914 0:6dc8ac1c7c00 110 void uart_send(void)
alan82914 0:6dc8ac1c7c00 111 {
alan82914 0:6dc8ac1c7c00 112 // uart send data
alan82914 0:6dc8ac1c7c00 113 display[0] = Gyro_axis_data[0];
alan82914 0:6dc8ac1c7c00 114 display[1] = Gyro_axis_data[1];
alan82914 0:6dc8ac1c7c00 115 display[2] = Gyro_axis_data[2];
alan82914 0:6dc8ac1c7c00 116 display[3] = Acce_axis_data[0];
alan82914 0:6dc8ac1c7c00 117 display[4] = Acce_axis_data[1];
alan82914 0:6dc8ac1c7c00 118 display[5] = Acce_axis_data[2];
alan82914 0:6dc8ac1c7c00 119 separate();
alan82914 0:6dc8ac1c7c00 120
alan82914 0:6dc8ac1c7c00 121 int j = 0;
alan82914 0:6dc8ac1c7c00 122 int k = 1;
alan82914 0:6dc8ac1c7c00 123 while(k)
alan82914 0:6dc8ac1c7c00 124 {
alan82914 0:6dc8ac1c7c00 125 if(uart_1.writeable())
alan82914 0:6dc8ac1c7c00 126 {
alan82914 0:6dc8ac1c7c00 127 uart_1.putc(num[i]);
alan82914 0:6dc8ac1c7c00 128 i++;
alan82914 0:6dc8ac1c7c00 129 j++;
alan82914 0:6dc8ac1c7c00 130 }
alan82914 0:6dc8ac1c7c00 131
alan82914 0:6dc8ac1c7c00 132 if(j>1) // send 2 bytes
alan82914 0:6dc8ac1c7c00 133 {
alan82914 0:6dc8ac1c7c00 134 k = 0;
alan82914 0:6dc8ac1c7c00 135 j = 0;
alan82914 0:6dc8ac1c7c00 136 }
alan82914 0:6dc8ac1c7c00 137 }
alan82914 0:6dc8ac1c7c00 138
alan82914 0:6dc8ac1c7c00 139 if(i>15)
alan82914 0:6dc8ac1c7c00 140 i = 0;
alan82914 0:6dc8ac1c7c00 141 }
alan82914 0:6dc8ac1c7c00 142
alan82914 0:6dc8ac1c7c00 143 void separate(void)
alan82914 0:6dc8ac1c7c00 144 {
alan82914 0:6dc8ac1c7c00 145 num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列
alan82914 0:6dc8ac1c7c00 146 num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列
alan82914 0:6dc8ac1c7c00 147 num[4] = display[1] >> 8;
alan82914 0:6dc8ac1c7c00 148 num[5] = display[1] & 0x00ff;
alan82914 0:6dc8ac1c7c00 149 num[6] = display[2] >> 8;
alan82914 0:6dc8ac1c7c00 150 num[7] = display[2] & 0x00ff;
alan82914 0:6dc8ac1c7c00 151 num[8] = display[3] >> 8;
alan82914 0:6dc8ac1c7c00 152 num[9] = display[3] & 0x00ff;
alan82914 0:6dc8ac1c7c00 153 num[10] = display[4] >> 8;
alan82914 0:6dc8ac1c7c00 154 num[11] = display[4] & 0x00ff;
alan82914 0:6dc8ac1c7c00 155 num[12] = display[5] >> 8;
alan82914 0:6dc8ac1c7c00 156 num[13] = display[5] & 0x00ff;
alan82914 0:6dc8ac1c7c00 157 }
alan82914 0:6dc8ac1c7c00 158
alan82914 0:6dc8ac1c7c00 159 // sensor
alan82914 0:6dc8ac1c7c00 160 void init_sensor(void)
alan82914 0:6dc8ac1c7c00 161 {
alan82914 0:6dc8ac1c7c00 162 sensor.begin();
alan82914 0:6dc8ac1c7c00 163 }
alan82914 0:6dc8ac1c7c00 164
alan82914 0:6dc8ac1c7c00 165 void read_sensor(void)
alan82914 0:6dc8ac1c7c00 166 {
alan82914 0:6dc8ac1c7c00 167 // sensor raw data
alan82914 0:6dc8ac1c7c00 168 Gyro_axis_data[0] = sensor.readRawGyroX();
alan82914 0:6dc8ac1c7c00 169 Gyro_axis_data[1] = sensor.readRawGyroY();
alan82914 0:6dc8ac1c7c00 170 Gyro_axis_data[2] = sensor.readRawGyroZ();
alan82914 0:6dc8ac1c7c00 171
alan82914 0:6dc8ac1c7c00 172 Acce_axis_data[0] = sensor.readRawAccelX();
alan82914 0:6dc8ac1c7c00 173 Acce_axis_data[1] = sensor.readRawAccelY();
alan82914 0:6dc8ac1c7c00 174 Acce_axis_data[2] = sensor.readRawAccelZ();
alan82914 0:6dc8ac1c7c00 175 }
alan82914 0:6dc8ac1c7c00 176
alan82914 0:6dc8ac1c7c00 177 void sensor_filter(void)
alan82914 0:6dc8ac1c7c00 178 {
alan82914 0:6dc8ac1c7c00 179 // gyro filter
alan82914 0:6dc8ac1c7c00 180 Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18);
alan82914 0:6dc8ac1c7c00 181 Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0];
alan82914 0:6dc8ac1c7c00 182 Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18);
alan82914 0:6dc8ac1c7c00 183 Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1];
alan82914 0:6dc8ac1c7c00 184 Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18);
alan82914 0:6dc8ac1c7c00 185 Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
alan82914 0:6dc8ac1c7c00 186
alan82914 0:6dc8ac1c7c00 187 // acce filter
alan82914 0:6dc8ac1c7c00 188 Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18);
alan82914 0:6dc8ac1c7c00 189 Acce_axis_data_f_old[0] = Acce_axis_data_f[0];
alan82914 0:6dc8ac1c7c00 190 Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18);
alan82914 0:6dc8ac1c7c00 191 Acce_axis_data_f_old[1] = Acce_axis_data_f[1];
alan82914 0:6dc8ac1c7c00 192 Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18);
alan82914 0:6dc8ac1c7c00 193 Acce_axis_data_f_old[2] = Acce_axis_data_f[2];
alan82914 0:6dc8ac1c7c00 194
alan82914 0:6dc8ac1c7c00 195 Gyro_scale[0] = (Gyro_axis_data_f[0]-Gyro_axis_zero[0])*Gyro_gain_x;
alan82914 0:6dc8ac1c7c00 196 Gyro_scale[1] = (Gyro_axis_data_f[1]-Gyro_axis_zero[1])*Gyro_gain_y;
alan82914 0:6dc8ac1c7c00 197 Gyro_scale[2] = (Gyro_axis_data_f[2]-Gyro_axis_zero[2])*Gyro_gain_z;
alan82914 0:6dc8ac1c7c00 198
alan82914 0:6dc8ac1c7c00 199 Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x;
alan82914 0:6dc8ac1c7c00 200 Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y;
alan82914 0:6dc8ac1c7c00 201 Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z;
alan82914 0:6dc8ac1c7c00 202 }
alan82914 0:6dc8ac1c7c00 203
alan82914 0:6dc8ac1c7c00 204 // Timer
alan82914 0:6dc8ac1c7c00 205 void init_TIMER(void)
alan82914 0:6dc8ac1c7c00 206 {
alan82914 0:6dc8ac1c7c00 207 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
alan82914 0:6dc8ac1c7c00 208 }
alan82914 0:6dc8ac1c7c00 209
alan82914 0:6dc8ac1c7c00 210 void timer1_interrupt(void)
alan82914 0:6dc8ac1c7c00 211 {
alan82914 0:6dc8ac1c7c00 212 // sensor initial start
alan82914 0:6dc8ac1c7c00 213 if(sensor_flag == 0) {
alan82914 0:6dc8ac1c7c00 214 sensor_count++;
alan82914 0:6dc8ac1c7c00 215
alan82914 0:6dc8ac1c7c00 216 if(sensor_count >= 50) {
alan82914 0:6dc8ac1c7c00 217 sensor_flag = 1;
alan82914 0:6dc8ac1c7c00 218 sensor_count = 0;
alan82914 0:6dc8ac1c7c00 219 }
alan82914 0:6dc8ac1c7c00 220 } else {
alan82914 0:6dc8ac1c7c00 221 read_sensor();
alan82914 0:6dc8ac1c7c00 222 sensor_filter();
alan82914 0:6dc8ac1c7c00 223 uart_send();
alan82914 0:6dc8ac1c7c00 224 }
alan82914 0:6dc8ac1c7c00 225
alan82914 0:6dc8ac1c7c00 226 switch (mode) {
alan82914 0:6dc8ac1c7c00 227 case 'a':
alan82914 0:6dc8ac1c7c00 228 deg = zero*4096.0f/360.0f;
alan82914 0:6dc8ac1c7c00 229 break;
alan82914 0:6dc8ac1c7c00 230 case 'b':
alan82914 0:6dc8ac1c7c00 231 deg = (zero + 90)*4096.0f/360.0f;
alan82914 0:6dc8ac1c7c00 232 break;
alan82914 0:6dc8ac1c7c00 233 case 'c':
alan82914 0:6dc8ac1c7c00 234 deg = (zero-A*sin(2*pi*f*t*0.001*Ts))*4096.0f/360.0f; // 在正負A度搖
alan82914 0:6dc8ac1c7c00 235 t = t + 1;
alan82914 0:6dc8ac1c7c00 236 if(t >= 1/(f*0.001*Ts)) // 2*pi*f*t*0.001*Ts = 2*pi -> t = 1/(f*0.001*T)
alan82914 0:6dc8ac1c7c00 237 t = 0;
alan82914 0:6dc8ac1c7c00 238 break;
alan82914 0:6dc8ac1c7c00 239
alan82914 0:6dc8ac1c7c00 240 }
alan82914 0:6dc8ac1c7c00 241
alan82914 0:6dc8ac1c7c00 242 dynamixelClass.servo(SERVO_ID,deg,0x400);
alan82914 0:6dc8ac1c7c00 243 Pos = dynamixelClass.readPosition(SERVO_ID);
alan82914 0:6dc8ac1c7c00 244 }
alan82914 0:6dc8ac1c7c00 245
alan82914 0:6dc8ac1c7c00 246 void change_mode()
alan82914 0:6dc8ac1c7c00 247 {
alan82914 0:6dc8ac1c7c00 248 mode = uart_1.getc();
alan82914 0:6dc8ac1c7c00 249 }
alan82914 0:6dc8ac1c7c00 250
alan82914 0:6dc8ac1c7c00 251 float lpf(float input, float output_old, float frequency)
alan82914 0:6dc8ac1c7c00 252 {
alan82914 0:6dc8ac1c7c00 253 float output = 0;
alan82914 0:6dc8ac1c7c00 254
alan82914 0:6dc8ac1c7c00 255 output = (output_old + frequency*T*input) / (1 + frequency*T);
alan82914 0:6dc8ac1c7c00 256
alan82914 0:6dc8ac1c7c00 257 return output;
alan82914 0:6dc8ac1c7c00 258 }