Chris LU
Sensor_Correction
Fork of
MX28_Sensor_Correction
by 
LDSC_Robotics_TAs
Diff: main.cpp
- Revision:
- 2:bd98f7a4e231
- Parent:
- 0:6dc8ac1c7c00
--- 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