File content as of revision 25:e8bfb629e1b1:

//翼端can program
#include "mbed.h"
//#include "ADXL345_I2C.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 ADXL_MEAN_NUM 10
#define SEND_DATAS_LOOP_TIME 0.1
#define RECEIVE_DATAS_LOOP_TIME 0.05

#define ERURON_MOVE_DEG_INI_R 30 // もともと10
#define DRUG_MOVE_DEG_INI_R 76
#define ERURON_TRIM_INI_R 97  //元々94　
#define DRUG_TRIM_INI_R 33

#define ERURON_MOVE_DEG_INI_L -30 //もともと-7
#define DRUG_MOVE_DEG_INI_L -80
#define ERURON_TRIM_INI_L 113   //元々95
#define DRUG_TRIM_INI_L 110

#define kXBusTx             p9
#define kMaxServoNum        1       // 1 - 50
#define kMaxServoPause     (sizeof(motionData) / sizeof(pauseRec))
#define kMotionInterval     10      // flame / sec
#define kMotionMinMark      0x1249
#define kMotionEndMark      0xED86

CAN can(p30,p29);
CANMessage recmsg;
Serial pc(USBTX,USBRX);
//ADXL345_I2C accelerometer(p9, p10);
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(p14);
DigitalIn setTrimPin(p15);
DigitalIn EDstatePin(p16);
DigitalIn checkMaxDegPin(p17);
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: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 ADXL_FLAG;
bool INA_FLAG;
bool MPU_FLAG;

//int16_t acc[3] = {0,0,0};
char gyro_c[6] = {0,0,0,0,0,0};
//char acc_mean[3][ADXL_MEAN_NUM];
//int adxl_mean_counter = 0;

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

static const uint8_t        servoChannel = 0x01;

XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum);
Ticker gTimer;

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

//=============================================================
// XbusIntervalHandler()
// play motion !
//=============================================================
void XbusIntervalHandler()
{
    uint16_t value;
    uint16_t diff  = kMotionEndMark - kMotionMinMark;
    value = (uint16_t)(diff * analog.read()) + kMotionMinMark;
    gXBus.setServo(servoChannel, value);
    gXBus.sendChannelDataPacket();
}

//bool adxlInit(){
//    accelerometer.setPowerControl(0x00);
//    accelerometer.setDataFormatControl(0x0B);
//    accelerometer.setDataRate(ADXL345_3200HZ);
//    accelerometer.setPowerControl(0x08);
//    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;
}

XBusError init(){
    
    XBusError result;
    
    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();
//    ADXL_FLAG = adxlInit();
    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);
    
    //XBus
     // initialize XBus
    result = gXBus.start();
    if (result != kXBusError_NoError) {
        gXBus.stop();
        return result;
    }
    // initialize XBus Servos
    result = gXBus.addServo(servoChannel, kXbusServoNeutral);
    if (result != kXBusError_NoError) {
        gXBus.stop();
        return result;
    }

    return kXBusError_NoError;
}

void updateDatas(){
//    if(ADXL_FLAG){
////        accelerometer.getOutput(acc);
//    }
    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] = acc[i];
        toSendDatas[i] = gyro_c[i];
    }
//    toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
    toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77;
}

void receiveDatas(){
    if(can.read(recmsg)){
        for(int i = 0; i < CONTROL_VALUES_NUM; i++){
            controlValues[i] = recmsg.data[i];
            if(i<CONTROL_VALUES_NUM-1) floatvalues[i] = controlValues[i];
        }
        eruronfloat = *(const float *)floatvalues;
        led1 = !led1;
        //WriteServo();
    }
}

void toString(){
    for(int i = 0; i <CONTROL_VALUES_NUM; i++){
        pc.printf("%d, ",controlValues[i]);
    }
    pc.printf("\n\r");
}

double calcPulse(int deg){
    return (0.0006+(deg/180.0)*(0.00235-0.00045));

}

void WriteServo(){
    //if(debugServoPin){
//        led3 = 1;
//        float a = eruronAna.read()*180;
//        float b = drugAna.read()*180;
//        eruronServo.pulsewidth(calcPulse(eruronAna.read()*180));
//        drugServo.pulsewidth(calcPulse(drugAna.read()*180));
//        pc.printf("eruron:%f    drug:%f\n\r",a,b);
//    }
//    else{
      //  led3 = 0;
//        eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (controlValues[0]-1) ));
        drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
        eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronfloat-1) ));
    //}
}

void setTrim(){
    debugLED =  1;
    if(EDstatePin){
    eruronTrim = eruronAna.read()*180;
    eruronServo.pulsewidth(calcPulse(eruronTrim));
    }
    else{
    drugTrim = drugAna.read()*180;
    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 checkMaxDeg(){
    led4 = 1;
    float eruronTemp = eruronAna.read()*180;
    float drugTemp = drugAna.read()*180;
    if(EDstatePin){
    eruronServo.pulsewidth(calcPulse(eruronTemp));
    eruronMoveDeg = eruronTemp-eruronTrim;
    }
    else{
    drugServo.pulsewidth(calcPulse(drugTemp));
    drugMoveDeg = drugTemp-drugTrim;
    }
   // pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
    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();
    XBusError result;
    
    // start motion
    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
    
    while(1){
        while(setTrimPin){
            setTrim();
        }
        while(checkMaxDegPin){
            checkMaxDeg();
        }
      //  pc.printf("eT:%f\n\r",eruronTrim);
      pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
      pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
        led4 = 0;
        
        debugLED = 0;
        //receiveDatas();
//        sendDatas();
        WriteServo();
        updateDatas();
//      pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]);
        led3 = !led3;
        wait(WAIT_LOOP_TIME);
    }
}