2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- mpu????????
- Revision:
- 59:f007e543f8c9
- Parent:
- 58:f84bd22fd586
- Child:
- 60:a45dc19a6001
--- a/main.cpp Tue Jun 06 13:26:03 2017 +0000 +++ b/main.cpp Tue Jun 06 15:46:30 2017 +0000 @@ -27,9 +27,6 @@ #define ERURON_TRIM_INI_L 0.402 // 値をお大きいくすると頭上げ #define DRUG_TRIM_INI_L 0.73//値を小さくすると開く側 -#define PHASE_NUM 7 //奇数にしてください。そしてメインコードと必ず同じ値にしてください -#define NEUTRAL_PHASE (PHASE_NUM + 1) / 2.0 - /*ドラッグラダー 初期値 0.65 最大角0.99 @@ -62,13 +59,13 @@ I2C i2c_mpu(p9,p10); char toSendDatas[TO_SEND_DATAS_NUM]; -char controlValues[sizeof(int) +3];//0~3:eruruon,4( sizeof(float)で指定してください ):drug -char intvalues[sizeof(int)]; +char controlValues[sizeof(float) +3];//0~3:eruruon,4( sizeof(float)で指定してください ):drug +char intvalues[sizeof(float)]; float eruronTrim; float drugTrim; float eruronMoveDeg; float drugMoveDeg; -int eruronint = NEUTRAL_PHASE; +float eruronfloat = 0.0; unsigned short ina_val; double V,C; bool SERVO_FLAG; @@ -247,10 +244,10 @@ if(can.read(recmsg)) { for(int i = 0; i < CONTROL_VALUES_NUM; i++) { controlValues[i] = recmsg.data[i]; - if(i<sizeof(int)) intvalues[i] = controlValues[i]; + if(i<sizeof(float)) intvalues[i] = controlValues[i]; } // intvalues[0] = controlValues[0]; - eruronint = *(const int *)intvalues; + eruronfloat = *(const float *)intvalues; led1 = !led1; } } @@ -277,11 +274,10 @@ void WriteServo() { - drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2])); - eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronint - NEUTRAL_PHASE)/(PHASE_NUM-NEUTRAL_PHASE))); - pc.printf("WriteNum:%f ef:%d ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint),eruronint); - pc.printf("drValue::%f ei::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2],eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * (eruronint - NEUTRAL_PHASE)); - // pc.printf("raw:%f sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0)); + drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(float) + 2])); + eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat)); + pc.printf("WriteNum:%f ef:%d ",calcPulse( eruronTrim + eruronMoveDeg * eruronfloat),eruronfloat); + pc.printf("drValue::%f ei::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float) + 2]); } void setTrim() @@ -296,7 +292,7 @@ } //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim); pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); - pc.printf("eMD:%f dMD:%f ei:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint); + pc.printf("eMD:%f dMD:%f ei:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); } void setMaxDeg() @@ -312,7 +308,7 @@ drugMoveDeg = drugTemp-drugTrim; } pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); - pc.printf("eMD:%f dMD:%f ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint); + pc.printf("eMD:%f dMD:%f ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); wait_us(10); }