Diff: main.cpp

Revision:

49:73fe59148dd4

Parent:

48:1065506191f2

Child:

52:abec61ea3cd3

--- a/main.cpp	Wed May 19 05:39:14 2021 +0000
+++ b/main.cpp	Fri May 21 01:35:05 2021 +0000
@@ -24,14 +24,16 @@
 #define servoPwmMin  1200.0
 #define motorPwmMax  2000.0
 #define motorPwmMin  1100.0
-#define pitch_align 8.0*3.141562/180.0
-#define roll_align -2.8*3.141562/180.0
+
+#define N_EEPROM 8
 
 MPU6050 accelgyro;
 MAG3110 mag(PB_9,PB_8);
 SBUS sbus(PD_5, PD_6);
 Serial pc(USBTX, USBRX);
 DigitalIn userButton(USER_BUTTON);
+DigitalIn locdef1(PG_2);
+DigitalIn locdef2(PG_3);
 FastPWM servo(PE_9);
 PID pitchPID(5.0, 0,0,PID_dt); //rad
 PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
@@ -92,25 +94,70 @@
 float accref[3] = {0, 0, -0.98};
 float magref[3] = {0.65, 0, 0.75};
  
-int agoffset[6] = {0, 0, 0, -117, -563, 2 };
-float magbias[4] = {-140.868240, 121.863251, -162.735092, 37.112610};
+int calibrationFlag = 0;
+int pos_tail = 1;//1:left 2:center 3:right
+int agoffset[6] = {0, 0, 0, 386, -450, 48};
+float magbias[4] = {-215.833374, 207.740616, -167.444565, 36.688690};
+float pitch_align = 0.0*3.141562/180.0
+float roll_align  = 0.0*3.141562/180.0
+// eepromのread writeのためのunionを定義
+int address = 0xAE; // EEPROMの3つの入力が全て+より
+int pointeraddress = 0;
+
+I2C i2c(PB_9,PB_8);  // sda, scl
+union U{
+ int i[N_EEPROM]; // 0: flag(>1で正常に書き込まれた状態) 1 ~ 3: 磁場の中心座標, 4: 磁場の半径、 5~7; mag, 8~10: acc bias
+ char c[N_EEPROM*4]; // floatはcharの4倍
+};
 
 void writeSdcard()
 {
     //magcal.getExtremes(&magmin[0],&magmax[0]);
     //pc.printf("%f %f %f %f %f %f\r\n",magmin[0],magmin[1],magmin[2],magmax[0],magmax[1],magmax[2]);
     //pc.printf("%f %f %f %f %f %f %f %f %f %f %f %f \r\n",acc_x,acc_y,acc_z,mag_x,mag_y,mag_z,accref[0],accref[1],accref[2],magref[0],magref[1],magref[2]);
-    pc.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
+    //pc.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
     //pc.printf("%f %f %f %f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
     //pc.printf("%f %f %f %f : %f %f %f\r\n",float(obs_count)/100.0,th_mg,dynaccnorm,accnormerr,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
-    //pc.printf("%d \r\n",userButton.read());
-    //pc.printf("%f %f %f %f %f %f\r\n",gyro_x*180.0f/pi,gyro_y*180.0f/pi,gyro_z*180.0f/pi,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
-    //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
+    //pc.printf("%f %f \r\n",rc[1],servoOut[0]);
+    pc.printf("%f %f %f %f %f %f\r\n",gyro_x*180.0f/pi,gyro_y*180.0f/pi,gyro_z*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
+    //pc.printf("%f %f\r\n",roll_g*180.0f/pi,pitch_g*180.0f/pi);
     //pc.printf("%f %f %f %f %f %f\r\n",dynacc_x,dynacc_y,dynacc_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
     //pc.printf("%f %f %f : %f %f %f\r\n",magref[0],magref[1],magref[2],mag_x,mag_y,mag_z);
     //pc.printf("%f %f %f %f %f : %f %f %f %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],magval[0],magval[1],magval[2],mdata.x,mdata.y,mdata.z);
 }
 
+// eeprom書き込み・読み込みに必要な関数
+void writeEEPROM(int address, unsigned int eeaddress, char *data, int size)
+{
+    char i2cBuffer[size + 2];
+    i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
+    i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
+ 
+    for (int i = 0; i < size; i++) {
+        i2cBuffer[i + 2] = data[i];
+    }
+ 
+    int result = i2c.write(address, i2cBuffer, size + 2, false);
+    //sleep_ms(500);
+}
+
+// this function has no read limit
+void readEEPROM(int address, unsigned int eeaddress, char *data, int size)
+{
+    char i2cBuffer[2];
+    i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
+    i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
+ 
+    // Reset eeprom pointer address
+    int result = i2c.write(address, i2cBuffer, 2, false);
+    
+    //sleep_ms(500);
+ 
+    // Read eeprom
+    i2c.read(address, data, size);
+    //sleep_ms(500);
+}
+
 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
 {
     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
@@ -377,82 +424,6 @@
      dynacc_z = LPacc_z-(D.getNumber( 3, 1 )*accref[0]+D.getNumber( 3, 2 )*accref[1]+D.getNumber( 3, 3 )*accref[2]);
 }
 
-void execCalibration(){
-    pc.printf("\r\nEnter to Calibration Mode\r\n");
-    wait(5);
-    pc.printf("\r\n Acc and Gyro Calibration Start\r\n");
-    int axs = 0;
-    int ays = 0;
-    int azs = 0;
-    int gxs = 0;
-    int gys = 0;
-    int gzs = 0;
-    int iter_n = 5000;
-    for(int i = 0;i<iter_n ;i++){
-        accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
-        axs += ax;
-        ays += ay;
-        azs -= az;
-        gxs += gx;
-        gys += gy;
-        gzs -= gz;
-        wait(0.001);
-    }
-    axs = axs /iter_n;
-    ays = ays /iter_n;
-    azs = azs /iter_n; 
-    gxs = gxs /iter_n; 
-    gys = gys /iter_n; 
-    gzs = gzs /iter_n; 
-    pc.printf("\r\nagoffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
-    
-    pc.printf("\r\n Mag Calibration Start\r\n");
-    float f = 0;
-    while(1){
-        mag.getAxis(mdata); // flush the magnetmeter
-        magval[0] = (mdata.x - magbias[0]);
-        magval[1] = (mdata.y - magbias[1]);
-        magval[2] = (mdata.z - magbias[2]);
-        float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2];
-        float lr = 0.00001f;
-        f = mag_r - magbias[3]*magbias[3];
-        magbias[0] = magbias[0] + 4 * lr * f * magval[0];
-        magbias[1] = magbias[1] + 4 * lr * f * magval[1];
-        magbias[2] = magbias[2] + 4 * lr * f * magval[2];
-        magbias[3] = magbias[3] + 4 * lr * f * magbias[3];
-        
-        if(userButton.read() == 1){
-            break;
-        }
-        wait(0.001);
-    }
-    pc.printf("magbias : %f, %f, %f, %f\r\n",magbias[0],magbias[1],magbias[2],magbias[3]);
-    pc.printf("\r\n Refvec Calibration waiting\r\n");
-    wait(5);
-    pc.printf("\r\n Calibration Start\r\n");
-    float arefs[3] = {0.0,0.0,0.0};
-    float mrefs[3] = {0.0,0.0,0.0};
-    for(int i = 0;i<iter_n ;i++){
-        getIMUval();
-        arefs[0] += acc_x;
-        arefs[1] += acc_y; 
-        arefs[2] += acc_z;
-        mrefs[0] += mag_x;
-        mrefs[1] += mag_y; 
-        mrefs[2] += mag_z;
-        wait(0.001);
-    }
-    arefs[0] /= iter_n;
-    arefs[1] /= iter_n;
-    arefs[2] /= iter_n;
-    mrefs[0] /= iter_n;
-    mrefs[1] /= iter_n;
-    mrefs[2] /= iter_n;
-    pc.printf("\r\naccreg : %f, %f, %f\r\n",arefs[0],arefs[1],arefs[2]);
-    pc.printf("\r\nmagreg : %f, %f, %f\r\n",mrefs[0],mrefs[1],mrefs[2]);
-    while(1) {wait(1000);}
-}
-
 // 割り込まれた時点での出力(computeの結果）を返す関数
 void calcServoOut(){
     if(sbus.failSafe == false) {
@@ -536,31 +507,62 @@
     accelgyro.setDLPFMode(MPU6050_LPF);
     //地磁気
     mag.enable();
+
+    qhat << 1 << 0 << 0 << 0;
     
-    if(userButton.read() == 0){
-        qhat << 1 << 0 << 0 << 0;
-        
-        D.add(1,1,1.0);
-        D.add(2,2,1.0);
-        D.add(3,3,1.0);
+    D.add(1,1,1.0);
+    D.add(2,2,1.0);
+    D.add(3,3,1.0);
+
+    Phat.add(1,1,0.1);
+    Phat.add(2,2,0.1);
+    Phat.add(3,3,0.1);
+    Phat.add(4,4,0.1);
     
-        Phat << 0.01 << 0 <<0 <<0
-             << 0 << 0.01 <<0 <<0
-             << 0 << 0 <<0.001 <<0
-             << 0 << 0 << 0 << 0.001;
+    Qgyro.add(1,1,0.0224);
+    Qgyro.add(2,2,0.0224); 
+    Qgyro.add(3,3,0.0224);  
+    
+    Racc.add(1,1,0.0330*100);
+    Racc.add(2,2,0.0330*100);
+    Racc.add(3,3,0.0330*100);
+    
+    Rmag.add(1,1,1.0);
+    Rmag.add(2,2,1.0);
+    Rmag.add(3,3,1.0); 
+    
+    calibrationFlag = userButton.read();
+    if(calibrationFlag == 0){
         
-        Qgyro << 0.0224<< 0     <<0
-              << 0     <<0.0224<<0
-              << 0     << 0     <<0.0224;  
-        
-        Racc.add(1,1,0.0330*100);
-        Racc.add(2,2,0.0330*100);
-        Racc.add(3,3,0.0330*100);
-        
-        Rmag.add(1,1,1.0);
-        Rmag.add(2,2,1.0);
-        Rmag.add(3,3,1.0); 
-  
+        pc.printf("reading calibration value\r\n");
+        //キャリブレーション値を取得
+        U read_calib;
+        readEEPROM(address, pointeraddress, read_calib.c, 32);
+        wait(3);
+        for (int i = 0 ; i < N_EEPROM; i ++){
+        pos_tail = read_calib.i[0];
+        agoffset[3] = float(read_calib.i[5]);
+        agoffset[4] = float(read_calib.i[6]);
+        agoffset[5] = float(read_calib.i[7]);
+        magbias[0] = float(read_calib.i[1])/1000.0;
+        magbias[1] = float(read_calib.i[2])/1000.0;
+        magbias[2] = float(read_calib.i[3])/1000.0;
+        magbias[3] = float(read_calib.i[4])/1000.0;
+        }
+        switch(pos_tail){
+            case 1:
+                pc.printf("This MBED is Located at Left \r\n");
+                break;
+            case 2:
+                pc.printf("This MBED is Located at Center \r\n");
+                break;
+            case 3:
+                pc.printf("This MBED is Located at Right \r\n");
+                break;
+            default:   // error situation
+                pc.printf("error\r\n");
+                break;
+        }
         getIMUval();
         triad(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm, accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,mag_x/magnorm,mag_y/magnorm,mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm);
         float sumLPaccnorm = 0;
@@ -588,6 +590,144 @@
             att_dt = (tend-tstart);
         }
     }else{
-        execCalibration();
+        pc.printf("\r\nEnter to Calibration Mode\r\n");
+        wait(5);
+        pc.printf("Acc and Gyro Calibration Start\r\n");
+
+        int gxs = 0;
+        int gys = 0;
+        int gzs = 0;
+        int iter_n = 10000;
+        for(int i = 0;i<iter_n ;i++){
+            accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
+            gxs += gx;
+            gys += gy;
+            gzs -= gz;
+            //wait(0.01);
+        }
+        gxs = gxs /iter_n; 
+        gys = gys /iter_n; 
+        gzs = gzs /iter_n; 
+        agoffset[3] = gxs;
+        agoffset[4] = gys;
+        agoffset[5] = gzs;
+        pc.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
+        
+        pc.printf("Mag Calibration Start\r\n");
+        float f = 0;
+        while(1){
+            mag.getAxis(mdata); // flush the magnetmeter
+            magval[0] = (mdata.x - magbias[0]);
+            magval[1] = (mdata.y - magbias[1]);
+            magval[2] = (mdata.z - magbias[2]);
+            float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2];
+            float lr = 0.00001f;
+            f = mag_r - magbias[3]*magbias[3];
+            magbias[0] = magbias[0] + 4 * lr * f * magval[0];
+            magbias[1] = magbias[1] + 4 * lr * f * magval[1];
+            magbias[2] = magbias[2] + 4 * lr * f * magval[2];
+            magbias[3] = magbias[3] + 4 * lr * f * magbias[3];
+            
+            if(userButton.read() == 1){
+                break;
+            }
+            wait(0.001);
+        }
+        pc.printf("Magbias : %f, %f, %f, %f\r\n",magbias[0],magbias[1],magbias[2],magbias[3]);
+        
+        pc.printf("Determine Position of MBED\r\n");
+        wait(1);
+        pc.printf("Press the user button\r\n");
+        
+        while(userButton.read() == 0){wait(0.01);};
+        while(1){
+            pc.printf("Left\r\n");
+            wait(2);
+            if(userButton.read() == 0){
+                pos_tail=1;
+                break;
+            };
+            pc.printf("Center\r\n");
+            wait(2);
+            if(userButton.read() == 0){
+                pos_tail=2;
+                break;
+            };
+            pc.printf("Right\r\n");
+            wait(2);
+            if(userButton.read() == 0){
+                pos_tail=3;
+                break;
+            };
+        };
+        switch(pos_tail){
+            case 1:
+                pc.printf("This MBED is Located at Left \r\n");
+                break;
+            case 2:
+                pc.printf("This MBED is Located at Center \r\n");
+                break;
+            case 3:
+                pc.printf("This MBED is Located at Right \r\n");
+                break;
+            default:   // error situation
+                pc.printf("error\r\n");
+                break;
+        }
+            
+        
+        
+        pc.printf("Writing to EEPROM...\r\n");
+        U transfer_data;
+        transfer_data.i[0] = pos_tail;
+        for (int i = 1; i < 5; i++) {
+        if (!isnan(magbias[i - 1]))
+          transfer_data.i[i] = int(magbias[i - 1]*1000);  // intに丸めた値を送る。
+        else {
+          pc.printf("Mag bias is NOT transferred\n");
+          transfer_data.i[i] = 100;
+        }
+        }
+        // gxs,gys,gzsを送る
+        int gxyzs[3] = {gxs, gys, gzs};
+        for (int i = 5; i < 8; i++) {
+        if (!isnan(gxyzs[i - 5]))
+          transfer_data.i[i] = gxyzs[i - 5];
+        else {
+          pc.printf("gxyzs is NOT transferred\n");
+          transfer_data.i[i] = 0;
+        }
+        }
+        
+        for (int i = 0; i < N_EEPROM; i++) {
+        pc.printf("transfer_data[%d]: %\d\r\n", i, transfer_data.i[i]);
+        }
+        writeEEPROM(address, pointeraddress, transfer_data.c, N_EEPROM*4);
+        wait(3);
+        
+        U read_test;
+        readEEPROM(address, pointeraddress, read_test.c, N_EEPROM*4);
+        wait(3);
+        for (int i = 0 ; i < N_EEPROM; i ++){
+          pc.printf("transfer_data[%d]: %d\r\n",i, read_test.i[i]); 
+        }
+        
+        float pitch_align = 0.0*3.141562/180.0
+        float roll_align  = 0.0*3.141562/180.0
+        
+        for (int i =1 ; i<1000; i++){
+            float tstart = t.read();
+            //姿勢角を更新
+            getIMUval();
+            updateBetweenMeasures();
+            updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag);
+            updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc);
+            computeAngles();
+            
+            float tend = t.read();
+            att_dt = (tend-tstart);
+        }
+        
+        while(1) {wait(1000);}
     }
 }
\ No newline at end of file