File content as of revision 58:f84bd22fd586:

//翼端can program
#include "mbed.h"
#include "MPU6050.h"
#include "INA226.hpp"
#include "rtos.h"
#include "XBusServo.h"
#include "math.h"

#define TO_SEND_DATAS_NUM 7
#define INIT_SERVO_PERIOD_MS 20
#define WAIT_LOOP_TIME 0.02
#define CONTROL_VALUES_NUM 7
#define TO_SEND_CAN_ID 100
#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 18.0        //degree
#define DRUG_MOVE_DEG_INI_R 0.49
#define ERURON_TRIM_INI_R 0.38  //値lowerすると頭上げ
#define DRUG_TRIM_INI_R  0.37

#define ERURON_MOVE_DEG_INI_L -19.4     //degree
#define DRUG_MOVE_DEG_INI_L -0.44
#define ERURON_TRIM_INI_L  0.402  // 値をお大きいくすると頭上げ
#define DRUG_TRIM_INI_L    0.73//値を小さくすると開く側

#define PHASE_NUM 7 //奇数にしてください。そしてメインコードと必ず同じ値にしてください
#define NEUTRAL_PHASE (PHASE_NUM + 1) / 2.0

/*ドラッグラダー
初期値 0.65
最大角0.99
*/

CAN can(p30,p29);
CANMessage recmsg;
Serial pc(USBTX,USBRX);
I2C ina226_i2c(p28,p27);
INA226 VCmonitor(ina226_i2c);
PwmOut drugServo(p22);
PwmOut eruronServo(p23);
DigitalOut led1(LED1);
AnalogIn drugAna(p20);
AnalogIn eruronAna(p19);
DigitalIn IsRPin(p11);
DigitalIn setTrimPin(p12);
DigitalIn EDstatePin(p14);
DigitalIn setMaxDegPin(p15);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
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[sizeof(int) +3];//0~3:eruruon,4( sizeof(float)で指定してください ):drug
char intvalues[sizeof(int)];
float eruronTrim;
float drugTrim;
float eruronMoveDeg;
float drugMoveDeg;
int eruronint = NEUTRAL_PHASE;
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;

bool servoInit()
{
    drugServo.period_ms(INIT_SERVO_PERIOD_MS);
    return true;
}

void sendDatas()
{
    if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) {
    }
}

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()) {
        pc.printf("VCmonitor NOT FOUND\n");
        return false;
    }
    ina_val = 0;
    if(VCmonitor.rawRead(0x00,&ina_val) != 0) {
        pc.printf("VCmonitor READ ERROR\n");
        return false;
    }
    VCmonitor.setCurrentCalibration();
    return true;
}

//動かしたいエレボンの角度から、動かしたいサーボホーンの角度を得る。
double ConvertDeg(double servo)
{
    double result = 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105;
    return result;
}

//ホーンを動かしたい角度から、変化させるアナログ値の幅を得る。3度変化
double GetValueByHorn(double deg)
{
    return deg*(0.10/25.7);
}

double GetFloatByErebon(double erebonDeg)
{
    double servoDeg = ConvertDeg(erebonDeg);
    double FirstMoveDeg = GetValueByHorn(servoDeg);
    return FirstMoveDeg;
}

void init()
{
    if(IsRPin) {
        eruronTrim = ERURON_TRIM_INI_R;
        drugTrim = DRUG_TRIM_INI_R;
        eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R);
        drugMoveDeg =DRUG_MOVE_DEG_INI_R;
    } else {
        eruronTrim = ERURON_TRIM_INI_L;
        drugTrim = DRUG_TRIM_INI_L;
        eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L);
        drugMoveDeg = DRUG_MOVE_DEG_INI_L;
    }
    SERVO_FLAG = servoInit();
    INA_FLAG = inaInit();
    sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
    // toStringTicker.attach(&toString,0.5);
    receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);

    //  initXBus();
}

void updateDatas()
{
    if(INA_FLAG) {
        int tmp = VCmonitor.getVoltage(&V);
        tmp = VCmonitor.getCurrent(&C);
    }

    for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) {
        toSendDatas[i] = gyro_c[i];
    }
//    toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
    toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)V;
}

void receiveDatas()
{
    if(can.read(recmsg)) {
        for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
            controlValues[i] = recmsg.data[i];
            if(i<sizeof(int)) intvalues[i] = controlValues[i];
        }
        //   intvalues[0] = controlValues[0];
        eruronint = *(const int *)intvalues;
        led1 = !led1;
    }
}

double calcPulse(float analog)
{
    return (0.0006 + (analog)*(0.00240-0.00060) );
//    double min = 0.0006;
//    double max = 0.00240;
//    if(analog >= max)
//        analog = max;
//    else if(analog <= min)
//        analog = min;
//
//    return (min+(analog)*(max-min));
    /*
        int start=510, end=2390;
    while(1) {
        //    pc.printf("%f\n\r",(start + (double)(end - start) * analogIn.read()));
        pc.printf("%f\n\r",analogIn.read());
        pwm.pulsewidth_us(start + (double)(end - start) * analogIn.read());
    */
}

void WriteServo()
{
    drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2]));
    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronint - NEUTRAL_PHASE)/(PHASE_NUM-NEUTRAL_PHASE)));
    pc.printf("WriteNum:%f    ef:%d  ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint),eruronint);
    pc.printf("drValue::%f   ei::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2],eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * (eruronint - NEUTRAL_PHASE));
    //  pc.printf("raw:%f    sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0));
}

void setTrim()
{
    led2 =  1;
    if(EDstatePin) {
        eruronTrim = eruronAna.read();
        eruronServo.pulsewidth(calcPulse(eruronTrim));
    } else {
        drugTrim = drugAna.read();
        drugServo.pulsewidth(calcPulse(drugTrim));
    }
    //pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
    pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
    pc.printf("eMD:%f   dMD:%f    ei:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint);
}

void setMaxDeg()
{
    led4 = 1;
    float eruronTemp = eruronAna.read();
    float drugTemp = drugAna.read();
    if(EDstatePin) {
        eruronMoveDeg = eruronTemp-eruronTrim;
        eruronServo.pulsewidth(calcPulse(eruronTemp));
    } else {
        drugServo.pulsewidth(calcPulse(drugTemp));
        drugMoveDeg = drugTemp-drugTrim;
    }
    pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
    pc.printf("eMD:%f   dMD:%f    ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint);
    wait_us(10);
}

int main()
{
    init();

    setTrimPin.mode(PullDown);
    setMaxDegPin.mode(PullDown);
    EDstatePin.mode(PullDown);
    IsRPin.mode(PullDown);
    Thread mpu_thread(&mpuProcessing);

    // start motion
//    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);

    while(1) {
        while(setTrimPin) {
            setTrim();
        }
        while (setMaxDegPin) {
            setMaxDeg();
        }
        led4 = 0;
        led2 = 0;
        //receiveDatas();
//        sendDatas();
        WriteServo();
        updateDatas();
        led3 = !led3;
        wait(WAIT_LOOP_TIME);
    }
}