File content as of revision 41:efebfdd955ac:

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

#define TO_SEND_DATAS_NUM 7
#define INIT_SERVO_PERIOD_MS 20
#define WAIT_LOOP_TIME 0.02
#define CONTROL_VALUES_NUM sizeof(float) + 1
#define TO_SEND_CAN_ID 100
#define SEND_DATAS_LOOP_TIME 0.1
#define RECEIVE_DATAS_LOOP_TIME 0.1

#define ERURON_MOVE_DEG_INI_R 1.0
#define DRUG_MOVE_DEG_INI_R 0.32
#define ERURON_TRIM_INI_R 0
#define DRUG_TRIM_INI_R 0.62

#define ERURON_MOVE_DEG_INI_L 1.0
#define DRUG_MOVE_DEG_INI_L 0.32
#define ERURON_TRIM_INI_L 0
#define DRUG_TRIM_INI_L 0.62

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

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);
PwmOut eruronServo(p23);
DigitalOut led1(LED1);
AnalogIn drugAna(p20);
AnalogIn eruronAna(p19);
DigitalIn LRstatePin(p11);
DigitalIn setTrimPin(p12);
DigitalIn EDstatePin(p14);
DigitalIn setMaxDegPin(p15);
DigitalOut debugLED(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
Ticker sendDatasTicker;
//Ticker toStringTicker;
Ticker receiveDatasTicker;

char toSendDatas[TO_SEND_DATAS_NUM];
char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4( sizeof(float)で指定してください ):drug
char floatvalues[sizeof(float)];
float eruronTrim;
float drugTrim;
float eruronMoveDeg;
float drugMoveDeg;
float eruronfloat;
unsigned short ina_val;
double V,C;
bool SERVO_FLAG;
bool INA_FLAG;
bool MPU_FLAG;
uint16_t XbusValue;

char gyro_c[6] = {0,0,0,0,0,0};

void toString();
void receiveDatas();
void WriteServo();

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))) {
    }
}

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;
}

void init()
{
    if(!LRstatePin) {
        eruronTrim = ERURON_TRIM_INI_L;
        drugTrim = DRUG_TRIM_INI_L;
        eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
        drugMoveDeg = DRUG_MOVE_DEG_INI_L;
    } else {
        eruronTrim = ERURON_TRIM_INI_R;
        drugTrim = DRUG_TRIM_INI_R;
        eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
        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);
    receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);

    //  initXBus();
}

void updateDatas()
{
    if(INA_FLAG) {
        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];
    }
//    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(float)) floatvalues[i] = controlValues[i];
        }
        eruronfloat = *(const float *)floatvalues;
        led1 = !led1;
    }
}

double calcPulse(float deg)
{
    return (0.0006+(deg)*(0.00220-0.00045));
    /*
        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());
    */
}

float SampleFloat(float f)  //小数点以下第二位を切り捨て
{
    int temp = ((f + 0.05) * 100.0) / 10;
    float result = temp /10.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)));
    //  pc.printf("raw:%f    sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0));
}

void setTrim()
{
    debugLED =  1;
    if(EDstatePin) {
        eruronTrim = eruronAna.read();
    } 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    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
}

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

int main()
{
    init();

    setTrimPin.mode(PullDown);
    setMaxDegPin.mode(PullDown);
    EDstatePin.mode(PullDown);
    LRstatePin.mode(PullDown);

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

    while(1) {
        while(setTrimPin) {
            setTrim();
        }
        while (setMaxDegPin) {
            setMaxDeg();
        }
        pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
        pc.printf("eMD:%f   dMD:%f    ",eruronMoveDeg,drugMoveDeg);
        led4 = 0;
        debugLED = 0;
        //receiveDatas();
//        sendDatas();
        WriteServo();
        updateDatas();
        led3 = !led3;
        wait(WAIT_LOOP_TIME);
    }
}