2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
main.cpp
- Committer:
- YusukeWakuta
- Date:
- 2017-03-11
- Branch:
- XBus???
- Revision:
- 40:ad98da5da7bf
- Parent:
- 39:c6036315831a
- Child:
- 41:efebfdd955ac
- Child:
- 42:bf98a29e81ac
File content as of revision 40:ad98da5da7bf:
//翼端can program #include "mbed.h" #include "MPU6050.h" #include "INA226.hpp" #include "XBusServo.h" #define TO_SEND_DATAS_NUM 7 #define INIT_SERVO_PERIOD_MS 20 #define WAIT_LOOP_TIME 0.02 #define CONTROL_VALUES_NUM sizeof(float) + 1 #define TO_SEND_CAN_ID 100 #define SEND_DATAS_LOOP_TIME 0.1 #define RECEIVE_DATAS_LOOP_TIME 0.1 #define ERURON_MOVE_DEG_INI_R 1.0 #define DRUG_MOVE_DEG_INI_R 0.32 #define ERURON_TRIM_INI_R 0 #define DRUG_TRIM_INI_R 0.62 #define ERURON_MOVE_DEG_INI_L 1.0 #define DRUG_MOVE_DEG_INI_L 0.32 #define ERURON_TRIM_INI_L 0 #define DRUG_TRIM_INI_L 0.62 /*ドラッグラダー 初期値 0.65 最大角0.99 */ CAN can(p30,p29); CANMessage recmsg; Serial pc(USBTX,USBRX); MPU6050 mpu(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(p11); DigitalIn setTrimPin(p12); DigitalIn EDstatePin(p14); DigitalIn setMaxDegPin(p15); 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~3:eruruon,4( sizeof(float)で指定してください ):drug char floatvalues[sizeof(float)]; float eruronTrim; float drugTrim; float eruronMoveDeg; float drugMoveDeg; float eruronfloat; unsigned short ina_val; double V,C; bool SERVO_FLAG; bool INA_FLAG; bool MPU_FLAG; uint16_t XbusValue; char gyro_c[6] = {0,0,0,0,0,0}; void toString(); void receiveDatas(); void WriteServo(); Ticker gTimer; bool servoInit() { drugServo.period_ms(INIT_SERVO_PERIOD_MS); 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(); MPU_FLAG = mpu.testConnection(); INA_FLAG = inaInit(); sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME); // toStringTicker.attach(&toString,0.5); receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME); // initXBus(); } void updateDatas() { if(INA_FLAG) { int tmp = VCmonitor.getVoltage(&V); tmp = VCmonitor.getCurrent(&C); } if(MPU_FLAG) { mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6); } for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) { toSendDatas[i] = gyro_c[i]; } // toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100); toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)V; } void receiveDatas() { if(can.read(recmsg)) { for(int i = 0; i < CONTROL_VALUES_NUM; i++) { controlValues[i] = recmsg.data[i]; if(i<sizeof(float)) floatvalues[i] = controlValues[i]; } eruronfloat = *(const float *)floatvalues; led1 = !led1; } } double calcPulse(float deg) { return (0.0006+(deg)*(0.00220-0.00045)); /* int start=510, end=2390; while(1) { // pc.printf("%f\n\r",(start + (double)(end - start) * analogIn.read())); pc.printf("%f\n\r",analogIn.read()); pwm.pulsewidth_us(start + (double)(end - start) * analogIn.read()); */ } float SampleFloat(float f) //小数点以下第二位を切り捨て { int temp = ((f + 0.025) * 100.0) / 5; float result = temp / 20.0; return result; } void WriteServo() { drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)] / 2.0)); eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * SampleFloat((eruronfloat)))); pc.printf("drValue::%f ef::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)],SampleFloat((eruronfloat))); // pc.printf("raw:%f sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0)); } void setTrim() { debugLED = 1; if(EDstatePin) { eruronTrim = eruronAna.read(); } else { drugTrim = drugAna.read(); 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 ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); } void setMaxDeg() { led4 = 1; float eruronTemp = eruronAna.read(); float drugTemp = drugAna.read(); if(EDstatePin) { eruronMoveDeg = eruronTemp-eruronTrim; } else { drugServo.pulsewidth(calcPulse(drugTemp)); drugMoveDeg = drugTemp-drugTrim; } pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); wait_us(10); } int main() { init(); setTrimPin.mode(PullDown); setMaxDegPin.mode(PullDown); EDstatePin.mode(PullDown); LRstatePin.mode(PullDown); // start motion // gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); while(1) { while(setTrimPin) { setTrim(); } while (setMaxDegPin) { setMaxDeg(); } pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); pc.printf("eMD:%f dMD:%f ",eruronMoveDeg,drugMoveDeg); led4 = 0; debugLED = 0; //receiveDatas(); // sendDatas(); WriteServo(); updateDatas(); led3 = !led3; wait(WAIT_LOOP_TIME); } }