Diff: main.cpp

Branch:

mpu????????

Revision:

73:05feda5b0f98

Parent:

72:aef1ec8c66c7

Child:

74:8ccd04302a7f

--- 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);
     }