Diff: main.cpp

Revision:

32:60af346703d3

Parent:

31:3a6be197e98f

Child:

33:d54dacb7a0aa

diff -r 3a6be197e98f -r 60af346703d3 main.cpp
--- a/main.cpp	Thu Mar 23 08:50:19 2017 +0000
+++ b/main.cpp	Thu Mar 23 15:59:46 2017 +0000
@@ -3,7 +3,7 @@
 #include "mbed.h"
 #define WAIT_LOOP_TIME 0.001
 #define YOKUTAN_DATAS_NUM 7
-#define INPUT_DATAS_NUM 7
+#define INPUT_DATAS_NUM 7 //ここは1倍とまでしかCANでは一度に送れないため、8以下。そして、翼端コードと数字を合わせる必要あり。
 #define SEND_DATAS_CAN_ID 100
 #define SEND_DATAS_TIME 0.5
 #define THRESHOLD_OH_VALUE 0.14
@@ -109,7 +109,7 @@
     if(it == enumRoll) {
         if(Upper > Lower) {
             if(value < neutral) {
-                value =  neutral - ((neutral - value) * (Upper / Lower));
+                value =  neutral + ((value - neutral) * (Upper / Lower));
                 rollLowerDiff  =  -rollUpperDiff; 
             }
         } else  {
@@ -122,7 +122,7 @@
     } else if(it == enumPitch) {
         if(Upper > Lower) {
             if(value < neutral) {
-                value =  neutral - ((neutral - value) * (Upper / Lower));
+                value =  neutral + ((value -neutral) * (Upper / Lower));
                 pitchLowerDiff =  -pitchUpperDiff;
             }
         } else  {
@@ -175,10 +175,10 @@
     setMaxAndMin(enumPitch, pitchPin.read());
 
 //   pc.printf("rollN:%f     rollMax:%f     rollMin:%f   pitchN:%f   pitchMax:%f   pitchUpper:%f  pitchMin:%f   pitchLo:%f  raw:%f\r\n",rollNeutral,rollNeutral+rollUpperDiff,rollNeutral+rollLowerDiff,pitchNeutral,pitchNeutral+pitchUpperDiff,pitchUpperDiff,pitchNeutral+pitchLowerDiff,pitchLowerDiff,pitchPin.read());
-    float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollLowerDiff + rollLowerDiff,rollNeutral,rollPin.read() + neutralDiff);
+    float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollNeutral + rollLowerDiff,rollNeutral,rollPin.read() + neutralDiff);
     float MatchedPitch = MatchUpperAndLower(enumPitch, pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchPin.read());
-
-    float FormatedRoll_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll ),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
+//pc.printf("Rollmax:%f   value:%f    Min:%f" ,);
+    float FormatedRoll_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
     *(int *)inputDatas_R =PhaseFloat(FormatedRoll_R,SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
 
     float FormatedRoll_L = Format2Range(SetRollPitchRacio(MatchedPitch, - MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,-rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral  +pitchLowerDiff, - rollNeutral - rollUpperDiff));
@@ -194,15 +194,13 @@
     else if(*(int *)inputDatas_L > PHASE_NUM)
         *(int *)inputDatas_L =PHASE_NUM;
 
-   // pc.printf("DR :%d   DL:%d     input_R:%d   input_L:%d\n\r",drug_R.read(),drug_L.read(),*(int *)inputDatas_R,*(int *)inputDatas_L);
+    pc.printf("DR :%d   DL:%d     input_R:%d   input_L:%d\n\r",drug_R.read(),drug_L.read(),*(int *)inputDatas_R,*(int *)inputDatas_L);
     inputDatas_R[sizeof(int)+ 2] = (char)drug_R;
     led4 =! led4;
     // pc.printf("%c",*(char *)inputDatas_R[4]);
     //pc.printf("%c",(char)drug_R);
     inputDatas_L[sizeof(int)+2] = (char)drug_L;
   //  pc.printf("eruron:%f    drug:%f\n\r,",);
-  pc.printf(" DL:%d     input_R:%d\n\r",inputDatas_L[sizeof(int)+2],*(int *)inputDatas_L);
-    // pc.printf("");
 }
 
 void SendDatas()