albatross
/
ControlMain2017
2017年度の製作を開始します
Fork of Control_Main_Full_20160608 by
Diff: main.cpp
- Branch:
- ?????
- Revision:
- 42:6cc53727acc2
- Parent:
- 41:d2d123d7a6fa
- Child:
- 43:2d8bffac06cd
--- 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);