Diff: main.cpp

Branch:

mpu????????

Revision:

42:bf98a29e81ac

Parent:

40:ad98da5da7bf

Child:

43:9a57cec43257

--- a/main.cpp	Sat Mar 11 22:42:45 2017 +0000
+++ b/main.cpp	Sun Mar 19 14:22:58 2017 +0000
@@ -2,6 +2,7 @@
 #include "mbed.h"
 #include "MPU6050.h"
 #include "INA226.hpp"
+#include "rtos.h"
 #include "XBusServo.h"
 
 #define TO_SEND_DATAS_NUM 7
@@ -12,6 +13,9 @@
 #define SEND_DATAS_LOOP_TIME 0.1
 #define RECEIVE_DATAS_LOOP_TIME 0.1
 
+#define MPU_LOOP_TIME 0.01
+#define MPU_DELT_MIN 250
+
 #define ERURON_MOVE_DEG_INI_R 1.0
 #define DRUG_MOVE_DEG_INI_R 0.32
 #define ERURON_TRIM_INI_R 0
@@ -22,6 +26,8 @@
 #define ERURON_TRIM_INI_L 0
 #define DRUG_TRIM_INI_L 0.62
 
+#define PHASE_NUM 6.0 //偶数にしてください。そしてメインコードと必ず同じ値にしてください
+
 /*ドラッグラダー
 初期値 0.65
 最大角0.99
@@ -30,7 +36,6 @@
 CAN can(p30,p29);
 CANMessage recmsg;
 Serial pc(USBTX,USBRX);
-MPU6050 mpu(p9,p10);
 I2C ina226_i2c(p28,p27);
 INA226 VCmonitor(ina226_i2c);
 PwmOut drugServo(p22);
@@ -48,27 +53,39 @@
 Ticker sendDatasTicker;
 //Ticker toStringTicker;
 Ticker receiveDatasTicker;
+MPU6050 mpu6050;
+Timer t;
+
+//Set up I2C, (SDA,SCL)
+I2C i2c_mpu(p9,p10);
 
 char toSendDatas[TO_SEND_DATAS_NUM];
 char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4( sizeof(float)で指定してください ):drug
-char floatvalues[sizeof(float)];
+char intvalues[sizeof(int)];
 float eruronTrim;
 float drugTrim;
 float eruronMoveDeg;
 float drugMoveDeg;
-float eruronfloat;
+int eruronint;
 unsigned short ina_val;
 double V,C;
 bool SERVO_FLAG;
 bool INA_FLAG;
 bool MPU_FLAG;
 uint16_t XbusValue;
+int gyroX;
+int gyroY;
+int gyroZ;
+float sum = 0;
+uint32_t sumCount = 0;
 
 char gyro_c[6] = {0,0,0,0,0,0};
 
 void toString();
 void receiveDatas();
 void WriteServo();
+void MpuInit();
+void mpuProcessing(void const *arg);
 
 Ticker gTimer;
 
@@ -84,6 +101,74 @@
     }
 }
 
+void MpuInit()
+{
+    i2c_mpu.frequency(400000);  // use fast (400 kHz) I2C
+    t.start();
+    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
+    if (whoami == 0x68) { // WHO_AM_I should always be 0x68
+        Thread::wait(100);
+        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+        Thread::wait(100);
+        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
+            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
+            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+            mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+            Thread::wait(200);
+        } else {
+        }
+    } else {
+        //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
+    }
+}
+
+
+void mpuProcessing(void const *arg)
+{
+    MpuInit();
+    while(1) {
+        if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
+            mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
+            mpu6050.getAres();
+            ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+            ay = (float)accelCount[1]*aRes - accelBias[1];
+            az = (float)accelCount[2]*aRes - accelBias[2];
+            mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
+            mpu6050.getGres();
+            gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
+            gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
+            gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
+            tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
+            temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
+        }
+        Now = t.read_us();
+        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+        lastUpdate = Now;
+        sum += deltat;
+        sumCount++;
+        if(lastUpdate - firstUpdate > 10000000.0f) {
+            beta = 0.04;  // decrease filter gain after stabilized
+            zeta = 0.015; // increasey bias drift gain after stabilized
+        }
+        mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+        delt_t = t.read_ms() - count;
+        if (delt_t > MPU_DELT_MIN) {
+            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+            pitch *= 180.0f / PI;
+            yaw   *= 180.0f / PI;
+            roll  *= 180.0f / PI;
+            myled= !myled;
+            count = t.read_ms();
+            sum = 0;
+            sumCount = 0;
+        }
+        Thread::wait(1);
+    }//while(1)
+}
+
+
 bool inaInit()
 {
     if(!VCmonitor.isExist()) {
@@ -113,7 +198,6 @@
         drugMoveDeg =DRUG_MOVE_DEG_INI_R;
     }
     SERVO_FLAG = servoInit();
-    MPU_FLAG = mpu.testConnection();
     INA_FLAG = inaInit();
     sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
     // toStringTicker.attach(&toString,0.5);
@@ -128,9 +212,7 @@
         int tmp = VCmonitor.getVoltage(&V);
         tmp = VCmonitor.getCurrent(&C);
     }
-    if(MPU_FLAG) {
-        mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6);
-    }
+
     for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) {
         toSendDatas[i] = gyro_c[i];
     }
@@ -143,16 +225,16 @@
     if(can.read(recmsg)) {
         for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
             controlValues[i] = recmsg.data[i];
-            if(i<sizeof(float)) floatvalues[i] = controlValues[i];
+            if(i<sizeof(int)) intvalues[i] = controlValues[i];
         }
-        eruronfloat = *(const float *)floatvalues;
+        eruronint = *(const int *)intvalues;
         led1 = !led1;
     }
 }
 
-double calcPulse(float deg)
+double calcPulse(float analog)
 {
-    return (0.0006+(deg)*(0.00220-0.00045));
+    return (0.0006+(analog)*(0.00240-0.00060));
     /*
         int start=510, end=2390;
     while(1) {
@@ -162,18 +244,12 @@
     */
 }
 
-float SampleFloat(float f)  //小数点以下第二位を切り捨て
-{
-    int temp = ((f + 0.025) * 100.0) / 5;
-    float result = temp / 20.0;
-    return result;
-}
-
 void WriteServo()
 {
     drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)] / 2.0));
-    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * SampleFloat((eruronfloat))));
-     pc.printf("drValue::%f   ef::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)],SampleFloat((eruronfloat)));
+    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint));
+    pc.printf("WriteNum:%f      ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint));
+    pc.printf("drValue::%f   ef::%d\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)],eruronint);
     //  pc.printf("raw:%f    sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0));
 }
 
@@ -188,7 +264,7 @@
     }
     //pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
     pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-    pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
+    pc.printf("eMD:%f   dMD:%f    ei:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint);
 }
 
 void setMaxDeg()
@@ -203,7 +279,7 @@
         drugMoveDeg = drugTemp-drugTrim;
     }
     pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-    pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
+    pc.printf("eMD:%f   dMD:%f    ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint);
     wait_us(10);
 }
 
@@ -215,6 +291,7 @@
     setMaxDegPin.mode(PullDown);
     EDstatePin.mode(PullDown);
     LRstatePin.mode(PullDown);
+    Thread mpu_thread(&mpuProcessing);
 
     // start motion
 //    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
@@ -226,8 +303,9 @@
         while (setMaxDegPin) {
             setMaxDeg();
         }
-        pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-        pc.printf("eMD:%f   dMD:%f    ",eruronMoveDeg,drugMoveDeg);
+        //  pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
+        ////  pc.printf("eMD:%f   dMD:%f    ",eruronMoveDeg,drugMoveDeg);
+        //  pc.printf("pitch:%d      roll:%d     yaw:%d\n\r",pitch,roll,yaw);
         led4 = 0;
         debugLED = 0;
         //receiveDatas();