2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- mpu????????
- Revision:
- 44:624a4469ae21
- Parent:
- 43:9a57cec43257
- Child:
- 45:e3df49d7b912
diff -r 9a57cec43257 -r 624a4469ae21 main.cpp --- a/main.cpp Sun Mar 19 15:18:39 2017 +0000 +++ b/main.cpp Tue Mar 21 23:24:04 2017 +0000 @@ -8,7 +8,7 @@ #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 CONTROL_VALUES_NUM sizeof(int) + 2 #define TO_SEND_CAN_ID 100 #define SEND_DATAS_LOOP_TIME 0.1 #define RECEIVE_DATAS_LOOP_TIME 0.1 @@ -18,17 +18,17 @@ #define ERURON_MOVE_DEG_INI_R 1.0 #define DRUG_MOVE_DEG_INI_R 0.32 -#define ERURON_TRIM_INI_R 0 +#define ERURON_TRIM_INI_R 0.35 #define DRUG_TRIM_INI_R 0.62 #define ERURON_MOVE_DEG_INI_L 0.8 -#define DRUG_MOVE_DEG_INI_L 0.32 -#define ERURON_TRIM_INI_L 0.3 +#define DRUG_MOVE_DEG_INI_L -0.32 +#define ERURON_TRIM_INI_L 0.4 #define DRUG_TRIM_INI_L 0.62 #define ERURON_MOVE_RANGE 0.24 -#define PHASE_NUM 6.0 //偶数にしてください。そしてメインコードと必ず同じ値にしてください +#define PHASE_NUM 13.0 //偶数にしてください。そしてメインコードと必ず同じ値にしてください /*ドラッグラダー 初期値 0.65 @@ -49,7 +49,7 @@ DigitalIn setTrimPin(p12); DigitalIn EDstatePin(p14); DigitalIn setMaxDegPin(p15); -DigitalOut debugLED(LED2); +DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); Ticker sendDatasTicker; @@ -68,7 +68,7 @@ float drugTrim; float eruronMoveDeg; float drugMoveDeg; -int eruronint; +int eruronint = 1; unsigned short ina_val; double V,C; bool SERVO_FLAG; @@ -229,6 +229,7 @@ controlValues[i] = recmsg.data[i]; if(i<sizeof(int)) intvalues[i] = controlValues[i]; } + // intvalues[0] = controlValues[0]; eruronint = *(const int *)intvalues; led1 = !led1; } @@ -248,16 +249,16 @@ void WriteServo() { - drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)] / 2.0)); + drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)])); eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * ((1.0/PHASE_NUM) * eruronint) * ERURON_MOVE_RANGE)); - pc.printf("WriteNum:%f ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint)); + pc.printf("WriteNum:%f ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint* ERURON_MOVE_RANGE)); pc.printf("drValue::%f ef::%d\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)],eruronint); // pc.printf("raw:%f sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0)); } void setTrim() { - debugLED = 1; + led2 = 1; if(EDstatePin) { eruronTrim = eruronAna.read(); } else { @@ -309,7 +310,7 @@ //// pc.printf("eMD:%f dMD:%f ",eruronMoveDeg,drugMoveDeg); // pc.printf("pitch:%d roll:%d yaw:%d\n\r",pitch,roll,yaw); led4 = 0; - debugLED = 0; + led2 = 0; //receiveDatas(); // sendDatas(); WriteServo();