Diff: run.cpp

Revision:

56:888379912f81

Child:

61:c05353850017

diff -r b11c9fef094a -r 888379912f81 run.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/run.cpp	Mon May 31 18:59:36 2021 +0000
@@ -0,0 +1,79 @@
+#include "global.hpp"
+
+void run()
+{
+    pc.printf("reading calibration value\r\n");
+    //キャリブレーション値を取得
+    U read_calib;
+    readEEPROM(eeprom_address, eeprom_pointeraddress, read_calib.c, N_EEPROM*4);
+    wait(3);
+    pos_tail = (int)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.0f;
+    magbias[1] = float(read_calib.i[2])/1000.0f;
+    magbias[2] = float(read_calib.i[3])/1000.0f;
+    magbias[3] = float(read_calib.i[4])/1000.0f;
+    rpy_align.y = float(read_calib.i[8])/200000.0f;
+    rpy_align.x = float(read_calib.i[9])/200000.0f;
+//    tail_address[pos_tail] = (int)read_calib.i[10];
+    
+    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;
+    }
+    pc.printf("tail_address : %d\r\n", tail_address[pos_tail]);
+    pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",rpy_align.y*180.0f/M_PI,rpy_align.x*180.0f/M_PI);
+    getIMUval();
+    ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
+    float sumLPaccnorm = 0;
+    for(int i = 0; i < 1000; i++){
+        getIMUval();
+        val_thmg += acos((mag % acc)/mag.Norm()/acc.Norm());
+        sumLPaccnorm += LPacc.Norm();
+    }
+    accref.z = -sumLPaccnorm / 1000;
+    val_thmg /= 1000;
+    
+    for (int i = 0; i < 3; i++)
+    {
+        if (i == pos_tail)
+        {
+            break;
+        }
+        else
+        {
+            tail.Subscribe(tail_address[i], &(posValues[i]));
+        }
+    }
+    
+    LoopTicker PIDtick;
+    PIDtick.attach(calcServoOut,PID_dt);
+    
+    Timer _t;
+    _t.start();
+    
+    while(1)
+    {
+        float tstart = _t.read();
+        //姿勢角を更新
+        getIMUval();
+        ekf.updateBetweenMeasures(gyro, att_dt);
+        ekf.computeAngles(rpy, rpy_g, rpy_align);
+        PIDtick.loop(); 
+        
+        float tend = _t.read();
+        att_dt = (tend-tstart);
+    }
+}
\ No newline at end of file