2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- mpu????????
- Revision:
- 74:8ccd04302a7f
- Parent:
- 73:05feda5b0f98
- Child:
- 75:4b6f1b976bec
- Child:
- 77:ca4ab599ba2b
diff -r 05feda5b0f98 -r 8ccd04302a7f main.cpp --- a/main.cpp Sat Dec 16 12:06:41 2017 +0000 +++ b/main.cpp Sat Dec 23 05:30:55 2017 +0000 @@ -13,6 +13,7 @@ *(0)s:sending datas: mpu, servoV *(1)g:getting datas: eruronfloat, drugInput, servoOff *(2)c:servo config:eruronTrim,drugTrim,eMD,dMD +*(3)j:data to debug what you want(joker) ****************************************** */ #include "mbed.h" @@ -22,11 +23,12 @@ #include "XBusServo.h" #include "math.h" -#define TO_SEND_DATAS_NUM 7 #define INIT_SERVO_PERIOD_MS 20 #define WAIT_LOOP_TIME 0.02 -#define CONTROL_VALUES_NUM 7 -#define TO_SEND_CAN_ID 0x100 //0x0>>0x7ff +#define YOKUTAN_DATAS_NUM 7 +#define INPUT_DATAS_NUM 4 //ここは8バイトまでしかCANでは一度に送れないため、8以下。そして、操舵コードと数字を合わせる必要あり。 +#define ERURON_DATAS_NUM 3 //送られてくるエルロンインプットの文字数 +#define TO_SEND_CAN_ID 0x701 //0x0>>0x7ff #define SEND_DATAS_LOOP_TIME 0.1 #define RECEIVE_DATAS_LOOP_TIME 0.1 @@ -43,21 +45,30 @@ #define ERURON_TRIM_INI_L 0.41567 // 値を大きくすると頭上げ #define DRUG_TRIM_INI_L 0.73//値を小さくすると開く側 +/*ドラッグラダー +初期値 0.65 +最大角0.99 +*/ + #define print2pc(flag,fmt,...) do{if(flag){pc.printf(fmt,__VA_ARGS__);}}while(0) #define SENDING_DATA_DEBUG_FLAG debugflag[0].flag #define GETTING_DATA_DEBUG_FLAG debugflag[1].flag #define SERVO_DONFIG_DEBUG_FLAG debugflag[2].flag +#define DEBUG_FLAG debugflag[3].flag -const char *configfilename = "/local/CONFIG.csv"; struct flaglist{ char key; bool flag; }; +struct flaglist debugflag[]={ + {'s',0}, + {'g',0}, + {'c',0}, + {'j',0}, + {'0',0} +}; -/*ドラッグラダー -初期値 0.65 -最大角0.99 -*/ +const char *configfilename = "/local/CONFIG.csv"; CAN can(p30,p29); CANMessage recmsg; @@ -85,8 +96,8 @@ LocalFileSystem local("local"); -char toSendDatas[TO_SEND_DATAS_NUM]; -char floatValues[10]; +char toSendDatas[YOKUTAN_DATAS_NUM]; +char intValues[ERURON_DATAS_NUM]; float eruronTrim; float drugTrim; float eruronMoveDeg; @@ -97,17 +108,13 @@ bool SERVO_FLAG; bool INA_FLAG; bool MPU_FLAG; -struct flaglist debugflag[]={ - {'s',0}, - {'g',0}, - {'c',0}, - {'0',0} -}; + int gyroX; int gyroY; int gyroZ; float sum = 0; -float drugInput = 0.0; +int drugInput = 0; +int servoOffVer = 0; uint32_t sumCount = 0; char gyro_c[6] = {0,0,0,0,0,0}; @@ -123,7 +130,7 @@ void receiveFromPc(){ - for(;pc.readable();){ + while(pc.readable()){ char c = pc.getc(); for(int i = 0; debugflag[i].key != '0'; i++){ if(debugflag[i].key == c) @@ -140,7 +147,7 @@ void sendDatas() { - if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) { + if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, YOKUTAN_DATAS_NUM))) { } } @@ -325,7 +332,6 @@ int rl,yw; rl = (int)(roll*10000); yw = (int)(yaw*10000); -// r[3] = (rl&0xff000000)>>24; y[3] = (yw&0xff000000)>>24; r[2] = (rl&0x00ff0000)>>16; y[2] = (yw&0x00ff0000)>>16; r[1] = (rl&0x0000ff00)>>8; y[1] = (yw&0x0000ff00)>>8; r[0] = (rl&0x000000ff); y[0] = (yw&0x000000ff); @@ -343,14 +349,15 @@ void receiveDatas() { if(can.read(recmsg)) { //ここの中でpc.printfすると固まるので注意 - for(int i = 0; i < CONTROL_VALUES_NUM; i++) { - floatValues[i] = recmsg.data[i]; + for(int i = 0; i < ERURON_DATAS_NUM; i++) { + intValues[i] = recmsg.data[i]; } - drugInput = recmsg.data[5]- '0'; - servoOff = recmsg.data[6]-'0'; - eruronfloat = atof(floatValues); + drugInput = (recmsg.data[3]-'0')/2; + servoOffVer = ((recmsg.data[3]-'0')%2); + eruronfloat = atoi(intValues)/100.0; led1 = !led1; } + servoOff = servoOffVer; } double calcPulse(float analog) @@ -367,7 +374,8 @@ // pc.printf("\n\r"); drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput)); eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat)); - print2pc(GETTING_DATA_DEBUG_FLAG,"ef:%10f di:%5f so:%d\n\r",eruronfloat,drugInput,servoOff); + print2pc(GETTING_DATA_DEBUG_FLAG,"ef:%5.2f di:%d so:%d\n\r",eruronfloat,drugInput,int(servoOff)); + print2pc(DEBUG_FLAG,"servoOffVer:%d\n\r",servoOffVer); } void setTrim() @@ -407,7 +415,7 @@ init(); Thread mpu_thread(&mpuProcessing); - // start motion +// start motion // gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); while(1) {