2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- XBus???
- Revision:
- 37:1f71ca1e5dd1
- Parent:
- 36:ddf4aa818e88
- Child:
- 38:b492990e2b56
diff -r ddf4aa818e88 -r 1f71ca1e5dd1 main.cpp --- a/main.cpp Sat Mar 11 11:06:20 2017 +0000 +++ b/main.cpp Sat Mar 11 16:22:16 2017 +0000 @@ -13,14 +13,14 @@ #define RECEIVE_DATAS_LOOP_TIME 0.1 #define ERURON_MOVE_DEG_INI_R 1.0 -#define DRUG_MOVE_DEG_INI_R 1.0 +#define DRUG_MOVE_DEG_INI_R 0.3 #define ERURON_TRIM_INI_R 0 -#define DRUG_TRIM_INI_R 0 +#define DRUG_TRIM_INI_R 0.5 #define ERURON_MOVE_DEG_INI_L 1.0 -#define DRUG_MOVE_DEG_INI_L 1.0 +#define DRUG_MOVE_DEG_INI_L 0.3 #define ERURON_TRIM_INI_L 0 -#define DRUG_TRIM_INI_L 0 +#define DRUG_TRIM_INI_L 0.5 /* #define kMaxServoNum 1 // 1 - 50 @@ -28,6 +28,9 @@ #define kMotionInterval 10 // flame / sec #define kMotionMinMark 0x1249 #define kMotionEndMark 0xED86 + +初期値 0.65 +最大角0.99 */ CAN can(p30,p29); @@ -175,6 +178,13 @@ double calcPulse(float deg) { return (0.0006+(deg)*(0.00235-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) //小数点以下第二位を切り捨て @@ -186,10 +196,10 @@ void WriteServo() { - drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * float)controlValues[sizeof(float)] / 2.0)); - eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg *SampleFloat( (eruronfloat / 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)); + drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)])); + eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * SampleFloat((eruronfloat / 2.0)))); + pc.printf("drValue::%f ef::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)] / 2.0,SampleFloat((eruronfloat / 2.0))); + // pc.printf("raw:%f sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0)); } void setTrim() @@ -198,7 +208,7 @@ if(EDstatePin) { eruronTrim = eruronAna.read(); } else { - drugTrim = drugAna.read()*180; + drugTrim = drugAna.read(); drugServo.pulsewidth(calcPulse(drugTrim)); } //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim); @@ -210,7 +220,7 @@ { led4 = 1; float eruronTemp = eruronAna.read(); - float drugTemp = drugAna.read()*180; + float drugTemp = drugAna.read(); if(EDstatePin) { eruronMoveDeg = eruronTemp-eruronTrim; } else { @@ -241,6 +251,8 @@ while (setMaxDegPin) { setMaxDeg(); } + pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); + pc.printf("eMD:%f dMD:%f ",eruronMoveDeg,drugMoveDeg); led4 = 0; debugLED = 0; //receiveDatas();