2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- XBus???
- Revision:
- 35:707119a675dc
- Parent:
- 33:d075918d4846
- Child:
- 36:ddf4aa818e88
diff -r d075918d4846 -r 707119a675dc main.cpp --- a/main.cpp Sat Mar 11 05:28:36 2017 +0000 +++ b/main.cpp Sat Mar 11 07:46:44 2017 +0000 @@ -12,13 +12,13 @@ #define SEND_DATAS_LOOP_TIME 0.1 #define RECEIVE_DATAS_LOOP_TIME 0.1 -#define ERURON_MOVE_DEG_INI_R 0 -#define DRUG_MOVE_DEG_INI_R 0 +#define ERURON_MOVE_DEG_INI_R 1.0 +#define DRUG_MOVE_DEG_INI_R 1.0 #define ERURON_TRIM_INI_R 0 #define DRUG_TRIM_INI_R 0 -#define ERURON_MOVE_DEG_INI_L 0 -#define DRUG_MOVE_DEG_INI_L 30 +#define ERURON_MOVE_DEG_INI_L 1.0 +#define DRUG_MOVE_DEG_INI_L 1.0 #define ERURON_TRIM_INI_L 0 #define DRUG_TRIM_INI_L 90 @@ -170,24 +170,24 @@ } } -double calcPulse(int deg) +double calcPulse(float deg) { - return (0.0006+(deg/180.0)*(0.00235-0.00045)); + return (0.0006+(deg)*(0.00235-0.00045)); } -void XbusIntervalHandler() +float SampleFloat(float f) //小数点以下第二位を切り捨て { - uint16_t diff = kMotionEndMark - kMotionMinMark; - XbusValue = (uint16_t)(diff * (eruronTrim + eruronMoveDeg * eruronfloat / 2.0) + kMotionMinMark); - //pc.printf("%f",value); - gXBus.setServo(servoChannel, XbusValue); - gXBus.sendChannelDataPacket(); + int temp = ((f + 0.05) * 100.0) / 5; + float result = temp / 20.0; + return result; } void WriteServo() { - drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[sizeof(float)])); - eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat )); + drugServo.pulsewidth(calcPulse(/* drugTrim + drugMoveDeg * */(float)controlValues[sizeof(float)] / 2.0)); + eruronServo.pulsewidth(calcPulse(/* eruronTrim + eruronMoveDeg **/SampleFloat( (eruronfloat - 1.5) * 2.0))); + // pc.printf("dr::%f ef::%f\n\r",(float)controlValues[sizeof(float)],eruronfloat - 1.5); + pc.printf("raw:%f sampled:%f\n\r",(eruronfloat - 1.5) * 2.0,SampleFloat( (eruronfloat - 1.5) * 2.0)); } void setTrim() @@ -215,7 +215,6 @@ 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 ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); wait_us(10); @@ -241,10 +240,6 @@ while (setMaxDegPin) { 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",eruronMoveDeg,drugMoveDeg,eruronfloat); - pc.printf("EV:%f,DG:%d\n\r",eruronfloat,controlValues[sizeof(float)]); led4 = 0; debugLED = 0; @@ -252,8 +247,6 @@ // sendDatas(); WriteServo(); updateDatas(); -// pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]); - // pc.printf("%f\n\r",XbusValue); led3 = !led3; wait(WAIT_LOOP_TIME); }