3/18 翼端操舵

Dependencies:   ADXL345_I2C Control_Yokutan_CANver1 mbed

Fork of ControlYokutan02 by albatross

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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