![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
+3度のやつ
Dependencies: ADXL345_I2C Control_Yokutan_CANver1 mbed
Fork of Souda_Yokutan_ver4 by
main.cpp
- Committer:
- YusukeWakuta
- Date:
- 2016-03-21
- Revision:
- 17:d91ce54dd09a
- Parent:
- 16:82310bf7c326
- Child:
- 18:253f0526b8b3
File content as of revision 17:d91ce54dd09a:
//翼端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 12 #define DRUG_MOVE_DEG_INI_R 96 #define ERURON_TRIM_INI_R 31 #define DRUG_TRIM_INI_R 177 #define ERURON_MOVE_DEG_INI_L -12 #define DRUG_MOVE_DEG_INI_L -80 #define ERURON_TRIM_INI_L 111 #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); } }