2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- mpu????????
- Revision:
- 61:988e3f4280ac
- Parent:
- 60:a45dc19a6001
- Child:
- 62:1db967d29809
--- a/main.cpp Wed Jun 07 14:13:53 2017 +0000 +++ b/main.cpp Wed Jun 07 15:14:10 2017 +0000 @@ -59,7 +59,6 @@ I2C i2c_mpu(p9,p10); char toSendDatas[TO_SEND_DATAS_NUM]; -char controlValues[sizeof(float) +3];//0~3:eruruon,4( sizeof(float)で指定してください ):drug char floatValues[10]; float eruronTrim; float drugTrim; @@ -71,11 +70,11 @@ bool SERVO_FLAG; bool INA_FLAG; bool MPU_FLAG; -uint16_t XbusValue; int gyroX; int gyroY; int gyroZ; float sum = 0; +float drugInput = 0.0; uint32_t sumCount = 0; char gyro_c[6] = {0,0,0,0,0,0}; @@ -186,8 +185,7 @@ //動かしたいエレボンの角度から、動かしたいサーボホーンの角度を得る。 double ConvertDeg(double servo) { - double result = 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105; - return result; + return 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105; } //ホーンを動かしたい角度から、変化させるアナログ値の幅を得る。3度変化 @@ -199,8 +197,7 @@ double GetFloatByErebon(double erebonDeg) { double servoDeg = ConvertDeg(erebonDeg); - double FirstMoveDeg = GetValueByHorn(servoDeg); - return FirstMoveDeg; + return GetValueByHorn(servoDeg); } void init() @@ -221,8 +218,6 @@ sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME); // toStringTicker.attach(&toString,0.5); receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME); - - // initXBus(); } void updateDatas() @@ -241,12 +236,11 @@ void receiveDatas() { - if(can.read(recmsg)) { - for(int i = 0; i < 6; i++) { + if(can.read(recmsg)) { //ここの中でpc.printfすると重すぎて固まるので注意 + for(int i = 0; i < 5; i++) { floatValues[i] = recmsg.data[i]; - // controlValues[i] = recmsg.data[i]; - // if(i<sizeof(float)) floatValues[i] = controlValues[i]; } + drugInput = recmsg.data[5]- '0'; eruronfloat = atof(floatValues); led1 = !led1; } @@ -255,34 +249,18 @@ double calcPulse(float analog) { return (0.0006 + (analog)*(0.00240-0.00060) ); -// double min = 0.0006; -// double max = 0.00240; -// if(analog >= max) -// analog = max; -// else if(analog <= min) -// analog = min; -// -// return (min+(analog)*(max-min)); - /* - 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()); - */ } void WriteServo() { - for(int i = 0; i< 10; i++) { - pc.printf("%c",floatValues[i]); - } - pc.printf(" : %f",eruronfloat); - pc.printf("\n\r"); - drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(float) + 2])); + // for(int i = 0; i< 10; i++) { +// pc.printf("%c",floatValues[i]); +// } +// pc.printf(" : %f",eruronfloat); +// pc.printf("\n\r"); + drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput)); eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat)); - //pc.printf("WriteNum:%10f ef:%10d \n\r",calcPulse( eruronTrim + eruronMoveDeg * eruronfloat),eruronfloat); - // pc.printf("drValue::%f ei::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float) + 2]); + pc.printf("ef:%10f %5f\n\r",eruronfloat,drugInput); } void setTrim()