3/18 翼端操舵
Dependencies: ADXL345_I2C Control_Yokutan_CANver1 mbed
Fork of ControlYokutan02 by
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 }
Generated on Sun Aug 7 2022 06:59:23 by 1.7.2