2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- mpu????????
- Revision:
- 73:05feda5b0f98
- Parent:
- 72:aef1ec8c66c7
- Child:
- 74:8ccd04302a7f
diff -r aef1ec8c66c7 -r 05feda5b0f98 main.cpp --- a/main.cpp Fri Oct 27 09:51:52 2017 +0000 +++ b/main.cpp Sat Dec 16 12:06:41 2017 +0000 @@ -1,4 +1,20 @@ //翼端can program +/***************************************** +* ----- +* | 1 | Is R pin +* | 2 | In set mode pin +* | 3 | on:Elevon, off:Drug +* | 4 | on:Trim, off:Max deg +* ----- +* +****************************************** +**magic character of debug** +* +*(0)s:sending datas: mpu, servoV +*(1)g:getting datas: eruronfloat, drugInput, servoOff +*(2)c:servo config:eruronTrim,drugTrim,eMD,dMD +****************************************** +*/ #include "mbed.h" #include "MPU6050.h" #include "INA226.hpp" @@ -10,7 +26,7 @@ #define INIT_SERVO_PERIOD_MS 20 #define WAIT_LOOP_TIME 0.02 #define CONTROL_VALUES_NUM 7 -#define TO_SEND_CAN_ID 100 +#define TO_SEND_CAN_ID 0x100 //0x0>>0x7ff #define SEND_DATAS_LOOP_TIME 0.1 #define RECEIVE_DATAS_LOOP_TIME 0.1 @@ -27,11 +43,16 @@ #define ERURON_TRIM_INI_L 0.41567 // 値を大きくすると頭上げ #define DRUG_TRIM_INI_L 0.73//値を小さくすると開く側 - -#define debug pc.printf -#define debugMPU pc.printf +#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 const char *configfilename = "/local/CONFIG.csv"; +struct flaglist{ + char key; + bool flag; +}; /*ドラッグラダー 初期値 0.65 @@ -76,6 +97,12 @@ 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; @@ -93,6 +120,18 @@ Ticker gTimer; + + +void receiveFromPc(){ + for(;pc.readable();){ + char c = pc.getc(); + for(int i = 0; debugflag[i].key != '0'; i++){ + if(debugflag[i].key == c) + debugflag[i].flag = !(debugflag[i].flag); + } + } +} + bool servoInit() { drugServo.period_ms(INIT_SERVO_PERIOD_MS); @@ -174,7 +213,7 @@ gyro_c[3]=(char)(int)((roll - (int)roll)*100); gyro_c[4]=(char)((int)yaw); gyro_c[5]=(char)(int)((yaw - (int)yaw)*100); - debugMPU("p:%f,r:%f,y:%f\n\r",pitch,roll,yaw); + Thread::wait(1); }//while(1) } @@ -248,7 +287,7 @@ sscanf(s, "%f,%f,%f,%f\n", &eruronTrim, &drugTrim, &eruronMoveDeg, &drugMoveDeg); } fclose(fp); - debug("eruronTrim:%f,drugtrim:%f,eMD:%f,dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg); + //debug("eruronTrim:%f,drugtrim:%f,eMD:%f,dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg); return 0; } @@ -281,18 +320,29 @@ int tmp = VCmonitor.getVoltage(&V); tmp = VCmonitor.getCurrent(&C); } - - for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) { - toSendDatas[i] = gyro_c[i]; - } -// toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100); - toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)V; - pc.printf("servoV:%f\n\r",V); + uint8_t r[4]; + uint8_t y[4]; + 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); + r[3] = (r[2]>>7 == 0)?0:0xff;y[3] = (y[2]>>7 == 0)?0:0xff;//r[3]can make from r[2] + int i = 0; + for(;i<3;i++) + toSendDatas[i] = r[i]; + for(;i<6;i++) + toSendDatas[i] = y[i-3]; + toSendDatas[i] = (char)V; + print2pc(SENDING_DATA_DEBUG_FLAG,"p:%12f,r:%12f,y:%12f,servoV%12f r[]:%d,y[]:%d\n\r" + ,pitch,roll,yaw,V,*(int*)r,*(int*)y); } void receiveDatas() { - if(can.read(recmsg)) { //ここの中でpc.printfすると重すぎて固まるので注意 + if(can.read(recmsg)) { //ここの中でpc.printfすると固まるので注意 for(int i = 0; i < CONTROL_VALUES_NUM; i++) { floatValues[i] = recmsg.data[i]; } @@ -317,7 +367,7 @@ // pc.printf("\n\r"); drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput)); eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat)); - pc.printf("ef:%10f %5f\n\r",eruronfloat,drugInput); + print2pc(GETTING_DATA_DEBUG_FLAG,"ef:%10f di:%5f so:%d\n\r",eruronfloat,drugInput,servoOff); } void setTrim() @@ -332,7 +382,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,eruronfloat); + pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); } void setMaxDeg() @@ -348,7 +398,7 @@ drugServo.pulsewidth(calcPulse(drugTemp)); } pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); - pc.printf("eMD:%f dMD:%f ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); + pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); wait_us(10); } @@ -374,9 +424,9 @@ led2 = 0; receiveDatas(); sendDatas(); - pc.printf("V:%f\n\r",V); WriteServo(); updateDatas(); + receiveFromPc(); led3 = !led3; wait(WAIT_LOOP_TIME); }