File content as of revision 20:a5a85df4de13:

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

#define TO_SEND_DATAS_NUM 4
#define INIT_SERVO_PERIOD_MS 20
#define WAIT_LOOP_TIME 0.02
#define CONTROL_VALUES_NUM 2
#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 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 -7
#define DRUG_MOVE_DEG_INI_L -80
#define ERURON_TRIM_INI_L 113   //元々95
#define DRUG_TRIM_INI_L 110

CAN can(p30,p29);
CANMessage recmsg;
Serial pc(USBTX,USBRX);
ADXL345_I2C accelerometer(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:eruruon,1:drug

float eruronTrim;
float drugTrim;
float eruronMoveDeg;
float drugMoveDeg;
unsigned short ina_val;
double V,C;
bool SERVO_FLAG;
bool ADXL_FLAG;
bool INA_FLAG;

int acc[3] = {0,0,0};
char acc_mean[3][ADXL_MEAN_NUM];
int adxl_mean_counter = 0;

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

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

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

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();
    ADXL_FLAG = adxlInit();
    INA_FLAG = inaInit();
    sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
    // toStringTicker.attach(&toString,0.5);
    receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
}

void updateDatas(){
    if(ADXL_FLAG){
        accelerometer.getOutput(acc);
    }
    if(INA_FLAG){
        int tmp = VCmonitor.getVoltage(&V);
        tmp = VCmonitor.getCurrent(&C);
    }
    for(int i = 0; i < 3; i++){
        toSendDatas[i] = acc[i];
    }
    toSendDatas[3] = (char)(V/100);
}

void receiveDatas(){
    if(can.read(recmsg)){
        for(int i = 0; i < CONTROL_VALUES_NUM; i++){
            controlValues[i] = recmsg.data[i];
        }
        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[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\n\r",eruronMoveDeg,drugMoveDeg);
}

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\n\r",eruronMoveDeg,drugMoveDeg);
    wait_us(10);
}

int main(){
    init();
    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\n\r",eruronMoveDeg,drugMoveDeg);
        led4 = 0;
        
        debugLED = 0;
        //receiveDatas();
        WriteServo();
        updateDatas();
        wait(WAIT_LOOP_TIME);
    }
}