Diff: setup.cpp

Revision:

65:ea184054e659

Parent:

61:c05353850017

Child:

68:b9f6938fab9d

--- a/setup.cpp	Tue Jun 22 02:04:32 2021 +0000
+++ b/setup.cpp	Tue Jun 22 02:08:31 2021 +0000
@@ -2,7 +2,6 @@
 
 void setup()
 {
-    pc.baud(57600);
     pitchPID.setSetPoint(0.0);
     pitchratePID.setSetPoint(0.0); 
     pitchPID.setBias(0.0);
@@ -12,8 +11,10 @@
     pitchPID.setInputLimits(-M_PI, M_PI);
     pitchratePID.setInputLimits(-M_PI, M_PI);
     
-    servo.period_us(15000.0);
-    servo.pulsewidth_us(1500.0);
+    elevServo.period_us(15000.0);
+    elevServo.pulsewidth_us(1500.0);
+    rudServo.period_us(15000.0);
+    rudServo.pulsewidth_us(1500.0);
     
     accelgyro.initialize();
     //加速度計のフルスケールレンジを設定
@@ -28,9 +29,9 @@
 
 void calibrate()
 {
-    pc.printf("\r\nEnter to Calibration Mode\r\n");
+    pc.serial.printf("\r\nEnter to Calibration Mode\r\n");
     wait(5);
-    pc.printf("Acc and Gyro Calibration Start\r\n");
+    pc.serial.printf("Acc and Gyro Calibration Start\r\n");
     
     int gxs = 0;
     int gys = 0;
@@ -51,41 +52,13 @@
     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("Press userbutton without initial Magbias value from EEPROM\r\n");
-    int magBiasFromEEPROM = 1; 
-    for(int i = 0;i<50 ;i++){
-        wait(0.1);
-        if(userButton.read() == 1)
-        {
-            magBiasFromEEPROM  = 0;
-            break;
-        }
-    };
-    wait(1);
-    if(magBiasFromEEPROM==1){
-        pc.printf("Read Initial Magbias from EEPROM\r\n");
-        U read_bias;
-        readEEPROM(eeprom_address, eeprom_pointeraddress, read_bias.c, N_EEPROM*4);
-        wait(3);
-        for (int i = 0 ; i < N_EEPROM; i ++)
-        {
-            magbiasMin[0] = float(read_bias.i[1])/1000.0f;
-            magbiasMin[1] = float(read_bias.i[2])/1000.0f;
-            magbiasMin[2] = float(read_bias.i[3])/1000.0f;
-            magbiasMax[0] = float(read_bias.i[4])/1000.0f;
-            magbiasMax[1] = float(read_bias.i[5])/1000.0f;
-            magbiasMax[2] = float(read_bias.i[6])/1000.0f;
-        }
-        magCalibrator.setExtremes(magbiasMin,magbiasMax);
-    }else{
-        pc.printf("Preset Initial Magbias \r\n");
-    }
-    magCalibrator.getExtremes(magbiasMin,magbiasMax);
-    pc.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
-    pc.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
+    pc.serial.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
+    
     
-    pc.printf("Mag Calibration Start\r\n");
+    pc.serial.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
+    pc.serial.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
+    
+    pc.serial.printf("Mag Calibration Start\r\n");
     
     float inputMag[3];
     float outputMag[3];
@@ -103,58 +76,11 @@
         wait(0.001);
     }
     magCalibrator.getExtremes(magbiasMin,magbiasMax);
-    pc.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
-    pc.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
+    pc.serial.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
+    pc.serial.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
     magCalibrator.getExtremes(magbiasMin,magbiasMax);
-    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;
-    }
-    pc.printf("Calculating pitch/roll Offset \r\n");  
+    pc.serial.printf("Calculating pitch/roll Offset \r\n");  
     //姿勢オフセットを計算
     rpy_align.y = 0.0f*M_PI/180.0f;
     rpy_align.x = 0.0f*M_PI/180.0f;
@@ -167,10 +93,10 @@
         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);
+        //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;
@@ -181,71 +107,7 @@
         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 = 0; i < 3; i++)
-    {
-        if (!isnan(magbiasMax[i]))
-            transfer_data.i[i+1] = int(magbiasMin[i]*1000);  // intに丸めた値を送る。
-        else
-        {
-            pc.printf("Mag bias is NOT transferred\n");
-            transfer_data.i[i+1] = 100;
-        }
-    }
-    for (int i = 0; i < 3; i++)
-    {
-        if (!isnan(magbiasMax[i]))
-            transfer_data.i[i+4] = int(magbiasMax[i]*1000);  // intに丸めた値を送る。
-        else
-        {
-            pc.printf("Mag bias is NOT transferred\n");
-            transfer_data.i[i+4] = 100;
-        }
-    }
-    // gxs,gys,gzsを送る
-    int gxyzs[3] = {gxs, gys, gzs};
-    for (int i = 0; i < 3; i++)
-    {
-        if (!isnan(gxyzs[i]))
-            transfer_data.i[i+7] = gxyzs[i];
-        else
-        {
-            pc.printf("gxyzs is NOT transferred\n");
-            transfer_data.i[i+7] = 0;
-        }
-    }
-    
-    // ave_pitch,ave_rollを送る
-    int ave_pr[2] = {ave_pitch*100, ave_roll*100};
-    for (int i = 0; i < 2; i++)
-    {
-        if (!isnan(ave_pr[i]))
-            transfer_data.i[i+10] = ave_pr[i];
-        else
-        {
-          pc.printf("alignment data is NOT transferred\n");
-          transfer_data.i[i+10] = 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]); 
-    }
+    pc.serial.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);
     
     while(1)
     {