Diff: main.cpp

Branch:

mpu????????

Revision:

74:8ccd04302a7f

Parent:

73:05feda5b0f98

Child:

75:4b6f1b976bec

Child:

77:ca4ab599ba2b

--- 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) {