Diff: setup.cpp

Revision:

56:888379912f81

Child:

58:a7947322db87

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/setup.cpp	Mon May 31 18:59:36 2021 +0000
@@ -0,0 +1,213 @@
+#include "global.hpp"
+
+void setup()
+{
+    pitchPID.setSetPoint(0.0);
+    pitchratePID.setSetPoint(0.0); 
+    pitchPID.setBias(0.0);
+    pitchratePID.setBias(0.0);  
+    pitchPID.setOutputLimits(-1.0,1.0);
+    pitchratePID.setOutputLimits(-1.0,1.0);
+    pitchPID.setInputLimits(-M_PI, M_PI);
+    pitchratePID.setInputLimits(-M_PI, M_PI);
+    
+    servo.period_us(15000.0);
+    servo.pulsewidth_us(1500.0);
+    
+    accelgyro.initialize();
+    //加速度計のフルスケールレンジを設定
+    accelgyro.setFullScaleAccelRange(ACCEL_FSR);
+    //角速度計のフルスケールレンジを設定
+    accelgyro.setFullScaleGyroRange(GYRO_FSR);
+    //MPU6050のLPFを設定
+    accelgyro.setDLPFMode(MPU6050_LPF);
+    //地磁気
+    mag_sensor.enable();
+}
+
+void calibrate()
+{
+    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_sensor.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.000001f;
+        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 = 0;
+            break;
+        };
+        pc.printf("Center\r\n");
+        wait(2);
+        if(userButton.read() == 0)
+        {
+            pos_tail = 1;
+            break;
+        };
+        pc.printf("Right\r\n");
+        wait(2);
+        if(userButton.read() == 0)
+        {
+            pos_tail = 2;
+            break;
+        };
+    };
+    pc.printf("tail_address : %d\r\n", tail_address[pos_tail]);
+    switch(pos_tail)
+    {
+    case 0:
+        pc.printf("This MBED is Located at Left \r\n");
+        break;
+    case 1:
+        pc.printf("This MBED is Located at Center \r\n");
+        break;
+    case 2:
+        pc.printf("This MBED is Located at Right \r\n");
+        break;
+    default:   // error situation
+        pc.printf("error\r\n");
+        break;
+    }
+        
+    //姿勢オフセットを計算
+    rpy_align.y = 0.0f*M_PI/180.0f;
+    rpy_align.x = 0.0f*M_PI/180.0f;
+    float ave_pitch = 0.0f;
+    float ave_roll = 0.0f;
+    Timer _t;
+    _t.start();
+    for (int i = 0 ; i < 2200; i++)
+    {
+        float tstart = _t.read();
+        //姿勢角を更新
+        getIMUval();
+        ekf.updateBetweenMeasures(gyro, att_dt);
+        ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag);
+        ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc);
+        ekf.computeAngles(rpy, rpy_g, rpy_align);
+        if(i>199)
+        {
+            ave_pitch += rpy.y;
+            ave_roll += rpy.x;
+        }
+        wait(0.001);
+        float tend = _t.read();
+        att_dt = (tend-tstart);
+    }
+    
+    pc.printf("aliginment data : %f(pitch deg) %f(roll deg)\r\n",ave_pitch/2000.0f*180.0f/M_PI,ave_roll/2000.0f*180.0f/M_PI);
+    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;
+        }
+    }
+    
+    // ave_pitch,ave_rollを送る
+    int ave_pr[2] = {ave_pitch*100, ave_roll*100};
+    for (int i = 8; i < 10; i++)
+    {
+        if (!isnan(ave_pr[i - 8]))
+            transfer_data.i[i] = ave_pr[i - 8];
+        else
+        {
+          pc.printf("alignment data is NOT transferred\n");
+          transfer_data.i[i] = 0;
+        }
+    }
+//    transfer_data.i[10] = tail_address[pos_tail];
+    for (int i = 0; i < N_EEPROM; i++)
+    {
+        pc.printf("transfer_data[%d]: %d\r\n", i, transfer_data.i[i]);
+    }
+    writeEEPROM(eeprom_address, eeprom_pointeraddress, transfer_data.c, N_EEPROM*4);
+    wait(3);
+    
+    U read_test;
+    readEEPROM(eeprom_address, eeprom_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]); 
+    }
+    
+    while(1)
+    {
+        wait(1000);
+    }
+}
\ No newline at end of file