albatross
今までのジョイスティックの入力を段階分けしていた方式から、アナログ値をできる限りそのまま使用する方式に変更しました。
Fork of
ControlMain2017
by 
albatross
Diff: main.cpp
- Revision:
- 25:12408563540d
- Parent:
- 24:72f9cb6e9440
- Child:
- 26:745735b0479d
diff -r 72f9cb6e9440 -r 12408563540d main.cpp
--- a/main.cpp Mon Mar 20 16:10:41 2017 +0000
+++ b/main.cpp Mon Mar 20 23:33:54 2017 +0000
@@ -38,8 +38,8 @@
AnalogIn pitchPin(p20);
DigitalIn drug_R(p14);
DigitalIn drug_L(p19);
-DigitalOut myled1(LED1);
-DigitalOut myled2(LED2);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
DigitalOut led4(LED4);
DigitalOut led3(LED3);
//Ticker sendDatasTicker;
@@ -48,98 +48,94 @@
char yokutanDatas_L[YOKUTAN_DATAS_NUM];
char inputDatas_R[INPUT_DATAS_NUM];
char inputDatas_L[INPUT_DATAS_NUM];
-bool stateP = true;
-bool stateQ = true;
-bool stateP_old = true;
-bool stateQ_old = true;
-float changedRollneutral = 0.739;
-float rollUpperDiff = 0.224;
-float rollLowerDiff = -0.184;
+float rollNeutral = 0.739;
+float rollUpperDiff = 0;
+float rollLowerDiff = 0;
-float changedPitchNeutral = 0.468 ;//1って書いた方
-float pitchUpperDiff = 0.416;
-float pitchLowerDiff = -0.358;
+float pitchNeutral = 0.468 ;//1って書いた方
+float pitchUpperDiff = 0;
+float pitchLowerDiff = 0;
float neutralDiff;
CANMessage recmsg_R;
CANMessage recmsg_L;
+enum InputType {
+ roll,
+ pitch
+};
+
void setNeutral()
{
float rollSum;
float pitchSum;
-
+
for(int i = 0; i < SUM_UP_NUM; i++) {
rollSum += rollPin.read();
pitchSum += pitchPin.read();
}
- changedRollneutral = rollSum / SUM_UP_NUM;
- changedPitchNeutral = pitchSum / SUM_UP_NUM;
+ rollNeutral = rollSum / SUM_UP_NUM;
+ pitchNeutral = pitchSum / SUM_UP_NUM;
+}
+
+void setMaxAndMin(InputType it,float value)
+{
+ if(it = InputType.pitch) {
+ if(value >pitchNeutral + pitchUpperDiff)
+ pitchUpperDiff = value - pitchNeutral;
+ else if(value < rollNeutral - pitchLowerDiff)
+ pitchLowerDiff = pitchNeutral - value;
+ } else if(it = InputType.roll) {
+ if(value >rollNeutral + rollUpperDiff)
+ rollUpperDiff = value - rollNeutral;
+ else if(value < rollNeutral - rollLowerDiff)
+ rollLowerDiff = rollNeutral - value;
+ }
}
-void setRollMaxAndMin(float value)
-{
- if(value >changedRollneutral + rollUpperDiff)
- rollUpperDiff = value - changedRollneutral;
- else if(value < changedRollneutral - rollLowerDiff)
- rollLowerDiff = changedRollneutral - value;
-}
-
-void setPitchMaxAndMin(float value)
+void calibrateRoll2Pitch()
{
- if(value >changedPitchNeutral + pitchUpperDiff)
- pitchUpperDiff = value - changedPitchNeutral;
- else if(value < changedRollneutral - pitchLowerDiff)
- pitchLowerDiff = changedPitchNeutral - value;
-}
-
-
-void calibrateNeutral()
-{
- neutralDiff = changedPitchNeutral - changedRollneutral; //ピッチの初期値の方がい小さいと仮定
- changedRollneutral = changedRollneutral + neutralDiff;
- changedRollneutral = rollUpperDiff + neutralDiff;
- changedRollneutral = rollLowerDiff + neutralDiff;
+ neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定
+ rollNeutral += neutralDiff;
+ rollUpperDiff += neutralDiff;
+ rollLowerDiff += neutralDiff;
}
//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
-float MatchRollUpperAndLower(float max,float min,float neutral,float value)
+float MatchRollUpperAndLower(InputType it, float max,float min,float neutral,float value)
{
float Upper = max- neutral;
float Lower = neutral - min;
- if(Upper > Lower) {
- if(value < neutral) {
- value = neutral - ((neutral - value) * (Upper / Lower));
- rollLowerDiff = (rollLowerDiff)* (Upper / Lower);
+
+ if(it = InputType.roll) {
+ if(Upper > Lower) {
+ if(value < neutral) {
+ value = neutral - ((neutral - value) * (Upper / Lower));
+ rollLowerDiff = (rollLowerDiff)* (Upper / Lower);
+ }
+ } else {
+ if(value > neutral) {
+ value = neutral + ((value - neutral) * (Lower / Upper));
+ rollUpperDiff = rollUpperDiff* (Upper / Lower);
+ }
}
- } else {
- if(value > neutral) {
- value = neutral + ((value - neutral) * (Lower / Upper));
- rollUpperDiff = rollUpperDiff* (Upper / Lower);
+ return value;
+ } else if(it = InputType.pitch) {
+ if(Upper > Lower) {
+ if(value < neutral) {
+ value = neutral - ((neutral - value) * (Upper / Lower));
+ pitchLowerDiff = (pitchLowerDiff)* (Upper / Lower);
+ }
+ } else {
+ if(value > neutral) {
+ value = neutral + ((value - neutral) * (Lower / Upper));
+ pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower);
+ }
}
+ return value;
}
- return value;
-}
-
-//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
-float MatchPitchUpperAndLower(float max,float min,float neutral,float value)
-{
- float Upper = max- neutral;
- float Lower = neutral - min;
- if(Upper > Lower) {
- if(value < neutral) {
- value = neutral - ((neutral - value) * (Upper / Lower));
- pitchLowerDiff = (pitchLowerDiff)* (Upper / Lower);
- }
- } else {
- if(value > neutral) {
- value = neutral + ((value - neutral) * (Lower / Upper));
- pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower);
- }
- }
- return value;
}
//範囲外に値がない場合にエラーが発生するので範囲内に収める
@@ -173,31 +169,31 @@
return 0;
else if(value>=max)
return PHASE_NUM;
-
}
void InputControlValues()
{
- setRollMaxAndMin(rollPin.read());
- setPitchMaxAndMin(pitchPin.read());
- calibrateNeutral();
- float MatchedRoll = MatchRollUpperAndLower(changedRollneutral + rollUpperDiff,rollLowerDiff + rollLowerDiff,changedRollneutral,rollPin.read() - neutralDiff);
- float MatchedPitch = MatchPitchUpperAndLower(changedPitchNeutral + pitchUpperDiff,changedPitchNeutral + pitchLowerDiff,changedPitchNeutral,pitchPin.read());
+ setRollMaxAndMin(InputType.roll, rollPin.read());
+ setPitchMaxAndMin(InputType.pitch, pitchPin.read());
+ calibrateRoll2Pitch();
+ float MatchedRoll = MatchUpperAndLower(InputType.roll, rollNeutral + rollUpperDiff,rollLowerDiff + rollLowerDiff,rollNeutral,rollPin.read() - neutralDiff);
+ float MatchedPitch = MatchUpperAndLower(InputType.pitch, pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchPin.read());
- float FormatedRollR = Format2Range((MatchedPitch +MatchedRoll ) / 2.0,(changedRollneutral + rollUpperDiff+ changedPitchNeutral + pitchUpperDiff) / 2.0,(changedRollneutral + rollLowerDiff+ changedPitchNeutral + pitchLowerDiff) / 2.0);
- *(int *)inputDatas_R =PhaseFloat(FormatedRollR,(changedRollneutral + rollUpperDiff+ changedPitchNeutral + pitchUpperDiff) / 2.0,(changedRollneutral + rollLowerDiff+ changedPitchNeutral + pitchLowerDiff) / 2.0);
+ float FormatedRoll_R = Format2Range((MatchedPitch +MatchedRoll ) / 2.0,(rollNeutral + rollUpperDiff+ pitchNeutral + pitchUpperDiff) / 2.0,(rollNeutral + rollLowerDiff+ pitchNeutral + pitchLowerDiff) / 2.0);
+ *(int *)inputDatas_R =PhaseFloat(FormatedRoll_R,(rollNeutral + rollUpperDiff+ pitchNeutral + pitchUpperDiff) / 2.0,(rollNeutral + rollLowerDiff+ pitchNeutral + pitchLowerDiff) / 2.0);
- float FormatedRollL = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(changedRollneutral + rollUpperDiff- changedPitchNeutral -pitchLowerDiff) / 2.0,(changedRollneutral + rollLowerDiff- changedPitchNeutral - pitchUpperDiff) / 2.0);
- *(int *)inputDatas_L = PhaseFloat(FormatedRollR,(changedRollneutral + rollUpperDiff- changedPitchNeutral - pitchLowerDiff) / 2.0,(changedRollneutral + rollLowerDiff- changedPitchNeutral + pitchUpperDiff) / 2.0);
+ float FormatedRoll_L = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(rollNeutral + rollUpperDiff- pitchNeutral -pitchLowerDiff) / 2.0,(rollNeutral + rollLowerDiff- pitchNeutral - pitchUpperDiff) / 2.0);
+ *(int *)inputDatas_L = PhaseFloat(FormatedRoll_R,(rollNeutral + rollUpperDiff- pitchNeutral - pitchLowerDiff) / 2.0,(rollNeutral + rollLowerDiff- pitchNeutral + pitchUpperDiff) / 2.0);
- if(*(int *)inputDatas_R < 0)
- *(int *)inputDatas_R = 0;
- else if(*(int *)inputDatas_R > PHASE_NUM)
- *(int *)inputDatas_R = PHASE_NUM;
- if(*(int *)inputDatas_L < 0)
- *(int *)inputDatas_L = 0 ;
- else if(*(int *)inputDatas_L > PHASE_NUM)
- *(int *)inputDatas_L =PHASE_NUM;
+
+ if(*(int *)inputDatas_R < 0)
+ *(int *)inputDatas_R = 0;
+ else if(*(int *)inputDatas_R > PHASE_NUM)
+ *(int *)inputDatas_R = PHASE_NUM;
+ if(*(int *)inputDatas_L < 0)
+ *(int *)inputDatas_L = 0 ;
+ else if(*(int *)inputDatas_L > PHASE_NUM)
+ *(int *)inputDatas_L =PHASE_NUM;
pc.printf("input_R:%d input_L:%d\n\r",*(int *)inputDatas_R,*(int *)inputDatas_L);
inputDatas_R[4] = (char)drug_R;
@@ -208,25 +204,6 @@
// pc.printf("");
}
-
-
-void toString_R()
-{
- pc.printf("R:");
- for(int i = 0; i < INPUT_DATAS_NUM; i++) {
- pc.printf("%d:%i ",i,inputDatas_R[i]);
- }
-}
-
-void toString_L()
-{
- pc.printf("L:");
- for(int i = 0; i < INPUT_DATAS_NUM; i++) {
- pc.printf("%d:%i ",i,inputDatas_L[i]);
- }
- pc.printf("\n\r");
-}
-
void SendDatas()
{
can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM));
@@ -262,7 +239,7 @@
yokutanDatas_R[i] = recmsg_R.data[i];
// pc.printf("%c",yokutanDatas_R[i]);
}
- myled1 = !myled1;
+ led1 = !led1;
}
if(can_L.read(recmsg_L)) {
for(int i = 0; i < recmsg_L.len; i++) {
@@ -270,7 +247,7 @@
led3 = !led3;
// pc.printf("%c",yokutanDatas_L[i]);
}
- myled2 = !myled2;
+ led2 = !led2;
}
}