File content as of revision 10:439e73c9a207:

//翼端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 ERURON_MOVE_DEG_INI 10
#define DRUG_MOVE_DEG_INI -92
#define ERURON_TRIM_INI 96
#define DRUG_TRIM_INI 109
#define start 510
#define end 2390

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 setTrimPin(p15);
DigitalIn debugServoPin(p16);
DigitalIn checkMaxDegPin(p17);
DigitalOut debugLED(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
Ticker sendDatasTicker;
Ticker toStringTicker;

char toSendDatas[TO_SEND_DATAS_NUM];
char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug

float eruronTrim = ERURON_TRIM_INI;
float drugTrim = DRUG_TRIM_INI;
float eruronMoveDeg = ERURON_MOVE_DEG_INI;
float drugMoveDeg = DRUG_MOVE_DEG_INI;
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();

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

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

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;
    eruronTrim = eruronAna.read()*180;
    drugTrim = drugAna.read()*180;
    eruronServo.pulsewidth(calcPulse(eruronTrim));
    drugServo.pulsewidth(calcPulse(drugTrim));
    pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
}

void checkMaxDeg(){
    led4 = 1;
    float eruronTemp = eruronAna.read()*180;
    float drugTemp = drugAna.read()*180;
    eruronServo.pulsewidth(calcPulse(eruronTemp));
    drugServo.pulsewidth(calcPulse(drugTemp));
    eruronMoveDeg = eruronTemp-eruronTrim;
    drugMoveDeg = drugTemp-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();
        }
        led4 = 0;
        debugLED = 0;
        receiveDatas();
        WriteServo();
        updateDatas();
        wait(WAIT_LOOP_TIME);
    }
}