2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
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();