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

--- 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