main.cpp

00001 //翼端can program
00002 #include "mbed.h"
00003 #include "ADXL345_I2C.h"
00004 #include "INA226.hpp"
00005 
00006 #define TO_SEND_DATAS_NUM 4
00007 #define INIT_SERVO_PERIOD_MS 20
00008 #define WAIT_LOOP_TIME 0.02
00009 #define CONTROL_VALUES_NUM 2
00010 #define TO_SEND_CAN_ID 100
00011 #define ADXL_MEAN_NUM 10
00012 #define SEND_DATAS_LOOP_TIME 0.1
00013 
00014 #define ERURON_MOVE_DEG_INI_R 1
00015 #define DRUG_MOVE_DEG_INI_R 1
00016 #define ERURON_TRIM_INI_R 1
00017 #define DRUG_TRIM_INI_R 1
00018 
00019 #define ERURON_MOVE_DEG_INI_L 2
00020 #define DRUG_MOVE_DEG_INI_L 2
00021 #define ERURON_TRIM_INI_L 2
00022 #define DRUG_TRIM_INI_L 2
00023 
00024 CAN can(p30,p29);
00025 CANMessage recmsg;
00026 Serial pc(USBTX,USBRX);
00027 ADXL345_I2C accelerometer(p9, p10);
00028 I2C ina226_i2c(p28,p27);
00029 INA226 VCmonitor(ina226_i2c);
00030 PwmOut drugServo(p22);
00031 PwmOut eruronServo(p23);
00032 DigitalOut led1(LED1);
00033 AnalogIn drugAna(p20);
00034 AnalogIn eruronAna(p19);
00035 DigitalIn LRstatePin(p14);
00036 DigitalIn setTrimPin(p15);
00037 DigitalIn EDstatePin(p16);
00038 DigitalIn checkMaxDegPin(p17);
00039 DigitalOut debugLED(LED2);
00040 DigitalOut led3(LED3);
00041 DigitalOut led4(LED4);
00042 Ticker sendDatasTicker;
00043 Ticker toStringTicker;
00044 //
00045 char toSendDatas[TO_SEND_DATAS_NUM];
00046 char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug
00047 
00048 float eruronTrim;
00049 float drugTrim;
00050 float eruronMoveDeg;
00051 float drugMoveDeg;
00052 unsigned short ina_val;
00053 double V,C;
00054 bool SERVO_FLAG;
00055 bool ADXL_FLAG;
00056 bool INA_FLAG;
00057 
00058 int acc[3] = {0,0,0};
00059 char acc_mean[3][ADXL_MEAN_NUM];
00060 int adxl_mean_counter = 0;
00061 
00062 void toString();
00063 
00064 bool servoInit(){
00065     drugServo.period_ms(INIT_SERVO_PERIOD_MS);
00066     eruronServo.period_ms(INIT_SERVO_PERIOD_MS);
00067     return true;
00068 }
00069 
00070 bool adxlInit(){
00071     accelerometer.setPowerControl(0x00);
00072     accelerometer.setDataFormatControl(0x0B);
00073     accelerometer.setDataRate(ADXL345_3200HZ);
00074     accelerometer.setPowerControl(0x08);
00075     return true;
00076 }
00077 
00078 void sendDatas(){
00079    if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
00080     }
00081 }
00082 
00083 bool inaInit(){
00084     if(!VCmonitor.isExist()){
00085         pc.printf("VCmonitor NOT FOUND\n");
00086         return false;
00087     }
00088     ina_val = 0;
00089     if(VCmonitor.rawRead(0x00,&ina_val) != 0){
00090         pc.printf("VCmonitor READ ERROR\n");
00091         return false;
00092     }
00093     VCmonitor.setCurrentCalibration();
00094     return true;
00095 }
00096 
00097 void init(){
00098     if(LRstatePin){
00099         eruronTrim = ERURON_TRIM_INI_L;
00100         drugTrim = DRUG_TRIM_INI_L;
00101         eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
00102         drugMoveDeg = DRUG_MOVE_DEG_INI_L;
00103     }
00104     else{
00105         eruronTrim = ERURON_TRIM_INI_R;
00106         drugTrim = DRUG_TRIM_INI_R;
00107         eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
00108         drugMoveDeg =DRUG_MOVE_DEG_INI_R;
00109     }
00110     SERVO_FLAG = servoInit();
00111     ADXL_FLAG = adxlInit();
00112     INA_FLAG = inaInit();
00113     sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
00114    // toStringTicker.attach(&toString,0.5);
00115 }
00116 
00117 void updateDatas(){
00118     if(ADXL_FLAG){
00119         accelerometer.getOutput(acc);
00120     }
00121     if(INA_FLAG){
00122         int tmp = VCmonitor.getVoltage(&V);
00123         tmp = VCmonitor.getCurrent(&C);
00124     }
00125     for(int i = 0; i < 3; i++){
00126         toSendDatas[i] = acc[i];
00127     }
00128     toSendDatas[3] = (char)(V/100);
00129 }
00130 
00131 void receiveDatas(){
00132     if(can.read(recmsg)){
00133         for(int i = 0; i < CONTROL_VALUES_NUM; i++){
00134             controlValues[i] = recmsg.data[i];
00135         }
00136         led1 = !led1;
00137     }
00138 }
00139 
00140 void toString(){
00141     for(int i = 0; i <CONTROL_VALUES_NUM; i++){
00142         pc.printf("%d, ",controlValues[i]);
00143     }
00144     pc.printf("\n\r");
00145 }
00146 
00147 double calcPulse(int deg){
00148     return (0.0006+(deg/180.0)*(0.00235-0.00045));
00149 
00150 }
00151 
00152 void WriteServo(){
00153     //if(debugServoPin){
00154 //        led3 = 1;
00155 //        float a = eruronAna.read()*180;
00156 //        float b = drugAna.read()*180;
00157 //        eruronServo.pulsewidth(calcPulse(eruronAna.read()*180));
00158 //        drugServo.pulsewidth(calcPulse(drugAna.read()*180));
00159 //        pc.printf("eruron:%f    drug:%f\n\r",a,b);
00160 //    }
00161 //    else{
00162       //  led3 = 0;
00163         eruronServo.pulsewidth(calcPulse(eruronTrim+eruronMoveDeg*(controlValues[0]-1)));
00164         drugServo.pulsewidth(calcPulse(drugTrim+drugMoveDeg*controlValues[1]));
00165     //}
00166 }
00167 
00168 void setTrim(){
00169     debugLED =  1;
00170     if(EDstatePin){
00171     eruronTrim = eruronAna.read()*180;
00172     eruronServo.pulsewidth(calcPulse(eruronTrim));
00173     }
00174     else{
00175     drugTrim = drugAna.read()*180;
00176     drugServo.pulsewidth(calcPulse(drugTrim));
00177     }
00178     pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
00179 }
00180 
00181 void checkMaxDeg(){
00182     led4 = 1;
00183     float eruronTemp = eruronAna.read()*180;
00184     float drugTemp = drugAna.read()*180;
00185     if(EDstatePin){
00186     eruronServo.pulsewidth(calcPulse(eruronTemp));
00187     eruronMoveDeg = eruronTemp-eruronTrim;
00188     }
00189     else{
00190     drugServo.pulsewidth(calcPulse(drugTemp));
00191     drugMoveDeg = drugTemp-drugTrim;
00192     }
00193     pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
00194     wait_us(10);
00195 }
00196 
00197 int main(){
00198     init();
00199     while(1){
00200         while(setTrimPin){
00201             setTrim();
00202         }
00203         while(checkMaxDegPin){
00204             checkMaxDeg();
00205         }
00206         pc.printf("eT:%f\n\r",eruronTrim);
00207         led4 = 0;
00208         debugLED = 0;
00209         receiveDatas();
00210         WriteServo();
00211         updateDatas();
00212         wait(WAIT_LOOP_TIME);
00213     }
00214 }