Diff: main.cpp

Branch:

?????

Revision:

42:6cc53727acc2

Parent:

41:d2d123d7a6fa

Child:

43:2d8bffac06cd

diff -r d2d123d7a6fa -r 6cc53727acc2 main.cpp
--- a/main.cpp	Sun Apr 23 15:12:59 2017 +0000
+++ b/main.cpp	Wed Apr 26 11:57:00 2017 +0000
@@ -1,6 +1,7 @@
 //中央
 
 #include "mbed.h"
+#include "ValueProcessor.h"
 //#include "rtos.h"
 #define WAIT_LOOP_TIME 0.001
 #define YOKUTAN_DATAS_NUM 7
@@ -10,9 +11,6 @@
 #define THRESHOLD_OH_VALUE 0.14
 #define SAMPLE_INTERVAL 0.05
 
-#define PHASE_NUM 15 //奇数にしてください
-#define SUM_UP_NUM 10.0
-#define PITCHPERROLL 1.5
 /*
 roll入力とピッチ入力だとピッチの方が1.5倍効くように
 */
@@ -43,6 +41,7 @@
 DigitalOut led2(LED2);
 DigitalOut led4(LED4);
 DigitalOut led3(LED3);
+ValueProcess VP;
 //DigitalOut fusokuControl(p25);
 //Ticker sendDatasTicker;
 
@@ -51,15 +50,6 @@
 char inputDatas_R[INPUT_DATAS_NUM];
 char inputDatas_L[INPUT_DATAS_NUM];
 
-float rollNeutral = 0.739;
-float rollUpperDiff = 0;
-float rollLowerDiff = 0;
-
-float pitchNeutral = 0.468 ;//1って書いた方
-float pitchUpperDiff = 0;
-float pitchLowerDiff = 0;
-float neutralDiff;
-
 CANMessage recmsg_R;
 CANMessage recmsg_L;
 
@@ -72,44 +62,12 @@
 //    }
 //}
 
-
-
-
-
-
-
 void InputControlValues()
 {
-    setMaxAndMin(enumRoll, rollPin.read());
-    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,rollNeutral + rollLowerDiff,rollNeutral,rollPin.read() + neutralDiff);
-    float MatchedPitch = MatchUpperAndLower(enumPitch, pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchPin.read());
-//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));
-    //pc.printf("matched:%f    max:%f  min:%f\n\r",(MatchedPitch - MatchedRoll) / 2.0,(pitchNeutral  +pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0,(pitchNeutral + pitchUpperDiff-rollNeutral - rollLowerDiff) / 2.0);
-    *(int *)inputDatas_L = PhaseFloat(FormatedRoll_L,SetRollPitchRacio(pitchNeutral + pitchUpperDiff ,- rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff ,- rollNeutral - rollUpperDiff));
-    //pc.printf("Format:%f    max:%f  min:%f\n\r",FormatedRoll_L,(pitchNeutral + pitchUpperDiff - rollNeutral - rollLowerDiff) / 2.0,(pitchNeutral + pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0);
-    if(*(int *)inputDatas_R < 1)
-        *(int *)inputDatas_R = 1;
-    else if(*(int *)inputDatas_R > PHASE_NUM)
-        *(int *)inputDatas_R   = PHASE_NUM;
-    if(*(int *)inputDatas_L < 1)
-        *(int *)inputDatas_L = 1 ;
-    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);
+    VP.Processing(rollPin.read(),pitchPin.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,",);
 }
 
 void SendDatas()
@@ -150,7 +108,6 @@
     if(can_R.read(recmsg_R)) {
         for(int i = 0; i < recmsg_R.len; i++) {
             yokutanDatas_R[i] = recmsg_R.data[i];
-            //   pc.printf("%c",yokutanDatas_R[i]);
         }
         led1 = !led1;
     }
@@ -158,7 +115,6 @@
         for(int i = 0; i < recmsg_L.len; i++) {
             yokutanDatas_L[i] = recmsg_L.data[i];
             led3 = !led3;
-            //  pc.printf("%c",yokutanDatas_L[i]);
         }
         led2 = !led2;
     }
@@ -168,7 +124,7 @@
 {
 //    Thread tickFusokuThread(&tickFusoku);
     init();
-    setNeutral();
+    VP.setNeutral(rollPin.read(),pitchPin.read());
     while(1) {
         InputControlValues();
         wait_us(5);