2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- XBus???
- Revision:
- 33:d075918d4846
- Parent:
- 32:b03557a08efa
- Child:
- 35:707119a675dc
--- a/main.cpp Fri Mar 10 12:59:41 2017 +0000 +++ b/main.cpp Sat Mar 11 05:28:36 2017 +0000 @@ -10,7 +10,7 @@ #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.05 +#define RECEIVE_DATAS_LOOP_TIME 0.1 #define ERURON_MOVE_DEG_INI_R 0 #define DRUG_MOVE_DEG_INI_R 0 @@ -18,9 +18,9 @@ #define DRUG_TRIM_INI_R 0 #define ERURON_MOVE_DEG_INI_L 0 -#define DRUG_MOVE_DEG_INI_L 0 +#define DRUG_MOVE_DEG_INI_L 30 #define ERURON_TRIM_INI_L 0 -#define DRUG_TRIM_INI_L 0 +#define DRUG_TRIM_INI_L 90 #define kMaxServoNum 1 // 1 - 50 #define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec)) @@ -35,6 +35,7 @@ I2C ina226_i2c(p28,p27); INA226 VCmonitor(ina226_i2c); PwmOut drugServo(p22); +PwmOut eruronServo(p23); DigitalOut led1(LED1); AnalogIn drugAna(p20); AnalogIn eruronAna(p19); @@ -50,7 +51,7 @@ Ticker receiveDatasTicker; char toSendDatas[TO_SEND_DATAS_NUM]; -char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4:drug +char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4( sizeof(float)で指定してください ):drug char floatvalues[sizeof(float)]; float eruronTrim; float drugTrim; @@ -162,7 +163,7 @@ if(can.read(recmsg)) { for(int i = 0; i < CONTROL_VALUES_NUM; i++) { controlValues[i] = recmsg.data[i]; - if(i<CONTROL_VALUES_NUM-1) floatvalues[i] = controlValues[i]; + if(i<sizeof(float)) floatvalues[i] = controlValues[i]; } eruronfloat = *(const float *)floatvalues; led1 = !led1; @@ -185,7 +186,8 @@ void WriteServo() { - drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4])); + drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[sizeof(float)])); + eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat )); } void setTrim() @@ -230,7 +232,7 @@ LRstatePin.mode(PullDown); // start motion - gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); +// gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); while(1) { while(setTrimPin) { @@ -240,9 +242,9 @@ setMaxDeg(); } // pc.printf("eT:%f\n\r",eruronTrim); - pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); - pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); - // pc.printf("%c,%c,%c,DG:%c\n\r",controlValues[0],controlValues[1],controlValues[2],controlValues[3]); +// pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); + pc.printf("eMD:%f dMD:%f ef:%f\n",eruronMoveDeg,drugMoveDeg,eruronfloat); + pc.printf("EV:%f,DG:%d\n\r",eruronfloat,controlValues[sizeof(float)]); led4 = 0; debugLED = 0;