今までのジョイスティックの入力を段階分けしていた方式から、アナログ値をできる限りそのまま使用する方式に変更しました。

Dependencies:   mbed-rtos mbed

Fork of ControlMain2017 by albatross

Committer:
YusukeWakuta
Date:
Tue Mar 21 19:47:11 2017 +0000
Revision:
29:8bff47ab602d
Parent:
28:056287cb001c
Child:
30:eb7b7af1070a
roll?pitch???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YusukeWakuta 13:79eb74eb86c4 1 //中央
YusukeWakuta 0:b1290ca6c4a2 2
YusukeWakuta 0:b1290ca6c4a2 3 #include "mbed.h"
tsumagari 10:b7159feb11fd 4 #define WAIT_LOOP_TIME 0.001
tsumagari 9:c32999b4d2f9 5 #define YOKUTAN_DATAS_NUM 7
tsumagari 10:b7159feb11fd 6 #define INPUT_DATAS_NUM 5
YusukeWakuta 0:b1290ca6c4a2 7 #define SEND_DATAS_CAN_ID 100
tsumagari 8:dbc8c87dac78 8 #define SEND_DATAS_TIME 0.5
tsumagari 8:dbc8c87dac78 9 #define THRESHOLD_OH_VALUE 0.14
YusukeWakuta 15:c5ade23e5de4 10 #define SAMPLE_INTERVAL 0.05
YusukeWakuta 0:b1290ca6c4a2 11
YusukeWakuta 29:8bff47ab602d 12 #define PHASE_NUM 13 //奇数にしてください
YusukeWakuta 24:72f9cb6e9440 13 #define SUM_UP_NUM 10.0
YusukeWakuta 29:8bff47ab602d 14 #define PITCHPERROLL 1.5
YusukeWakuta 19:6387e3f02b37 15 /*
YusukeWakuta 19:6387e3f02b37 16 roll入力とピッチ入力だとピッチの方が1.5倍効くように
YusukeWakuta 19:6387e3f02b37 17 */
YusukeWakuta 18:31722545ecf1 18
tsumagari 9:c32999b4d2f9 19 //-----------------------------------(resetInterrupt def)
tsumagari 9:c32999b4d2f9 20 extern "C" void mbed_reset();
YusukeWakuta 13:79eb74eb86c4 21 InterruptIn resetPin(p22);
tsumagari 9:c32999b4d2f9 22 Timer resetTimeCount;
YusukeWakuta 15:c5ade23e5de4 23 void resetInterrupt()
YusukeWakuta 15:c5ade23e5de4 24 {
YusukeWakuta 15:c5ade23e5de4 25 while(resetPin) {
tsumagari 9:c32999b4d2f9 26 resetTimeCount.start();
tsumagari 9:c32999b4d2f9 27 if(resetTimeCount.read()>3) mbed_reset();
tsumagari 9:c32999b4d2f9 28 }
tsumagari 9:c32999b4d2f9 29 resetTimeCount.reset();
tsumagari 9:c32999b4d2f9 30 }
tsumagari 9:c32999b4d2f9 31 //-------------------------------------------------------
tsumagari 9:c32999b4d2f9 32
taurin 6:7484315ba88a 33 CAN can_R(p30,p29);
taurin 6:7484315ba88a 34 CAN can_L(p9,p10);
YusukeWakuta 14:ffe9460922cb 35 Serial toKeikiSerial(p28,p27);
tsumagari 7:59ddbe239835 36 Serial pc(USBTX,USBRX);
tsumagari 10:b7159feb11fd 37 AnalogIn rollPin(p15);
YusukeWakuta 18:31722545ecf1 38 AnalogIn pitchPin(p20);
YusukeWakuta 13:79eb74eb86c4 39 DigitalIn drug_R(p14);
taurin 6:7484315ba88a 40 DigitalIn drug_L(p19);
YusukeWakuta 25:12408563540d 41 DigitalOut led1(LED1);
YusukeWakuta 25:12408563540d 42 DigitalOut led2(LED2);
YusukeWakuta 15:c5ade23e5de4 43 DigitalOut led4(LED4);
YusukeWakuta 15:c5ade23e5de4 44 DigitalOut led3(LED3);
tsumagari 9:c32999b4d2f9 45 //Ticker sendDatasTicker;
YusukeWakuta 0:b1290ca6c4a2 46
taurin 4:4a13dd263a7b 47 char yokutanDatas_R[YOKUTAN_DATAS_NUM];
taurin 4:4a13dd263a7b 48 char yokutanDatas_L[YOKUTAN_DATAS_NUM];
tsumagari 10:b7159feb11fd 49 char inputDatas_R[INPUT_DATAS_NUM];
tsumagari 10:b7159feb11fd 50 char inputDatas_L[INPUT_DATAS_NUM];
YusukeWakuta 24:72f9cb6e9440 51
YusukeWakuta 25:12408563540d 52 float rollNeutral = 0.739;
YusukeWakuta 25:12408563540d 53 float rollUpperDiff = 0;
YusukeWakuta 25:12408563540d 54 float rollLowerDiff = 0;
YusukeWakuta 24:72f9cb6e9440 55
YusukeWakuta 25:12408563540d 56 float pitchNeutral = 0.468 ;//1って書いた方
YusukeWakuta 25:12408563540d 57 float pitchUpperDiff = 0;
YusukeWakuta 25:12408563540d 58 float pitchLowerDiff = 0;
YusukeWakuta 24:72f9cb6e9440 59 float neutralDiff;
YusukeWakuta 0:b1290ca6c4a2 60
YusukeWakuta 3:0e66ce2ab2fb 61 CANMessage recmsg_R;
YusukeWakuta 3:0e66ce2ab2fb 62 CANMessage recmsg_L;
YusukeWakuta 0:b1290ca6c4a2 63
YusukeWakuta 25:12408563540d 64 enum InputType {
YusukeWakuta 26:745735b0479d 65 enumRoll,
YusukeWakuta 26:745735b0479d 66 enumPitch
YusukeWakuta 25:12408563540d 67 };
YusukeWakuta 25:12408563540d 68
YusukeWakuta 24:72f9cb6e9440 69 void setNeutral()
YusukeWakuta 24:72f9cb6e9440 70 {
YusukeWakuta 24:72f9cb6e9440 71 float rollSum;
YusukeWakuta 24:72f9cb6e9440 72 float pitchSum;
YusukeWakuta 25:12408563540d 73
YusukeWakuta 24:72f9cb6e9440 74 for(int i = 0; i < SUM_UP_NUM; i++) {
YusukeWakuta 24:72f9cb6e9440 75 rollSum += rollPin.read();
YusukeWakuta 24:72f9cb6e9440 76 pitchSum += pitchPin.read();
YusukeWakuta 24:72f9cb6e9440 77 }
YusukeWakuta 25:12408563540d 78 rollNeutral = rollSum / SUM_UP_NUM;
YusukeWakuta 25:12408563540d 79 pitchNeutral = pitchSum / SUM_UP_NUM;
YusukeWakuta 26:745735b0479d 80
YusukeWakuta 26:745735b0479d 81 neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定
YusukeWakuta 26:745735b0479d 82 rollNeutral += neutralDiff;
YusukeWakuta 25:12408563540d 83 }
YusukeWakuta 25:12408563540d 84
YusukeWakuta 25:12408563540d 85 void setMaxAndMin(InputType it,float value)
YusukeWakuta 25:12408563540d 86 {
YusukeWakuta 26:745735b0479d 87 if(it == enumPitch) {
YusukeWakuta 25:12408563540d 88 if(value >pitchNeutral + pitchUpperDiff)
YusukeWakuta 25:12408563540d 89 pitchUpperDiff = value - pitchNeutral;
YusukeWakuta 26:745735b0479d 90 else if(value < rollNeutral + pitchLowerDiff)
YusukeWakuta 26:745735b0479d 91 pitchLowerDiff = value - pitchNeutral;
YusukeWakuta 26:745735b0479d 92 return;
YusukeWakuta 26:745735b0479d 93 } else if(it == enumRoll) {
YusukeWakuta 25:12408563540d 94 if(value >rollNeutral + rollUpperDiff)
YusukeWakuta 25:12408563540d 95 rollUpperDiff = value - rollNeutral;
YusukeWakuta 26:745735b0479d 96 else if(value < rollNeutral + rollLowerDiff)
YusukeWakuta 26:745735b0479d 97 rollLowerDiff = value -rollNeutral ;
YusukeWakuta 26:745735b0479d 98 return;
YusukeWakuta 25:12408563540d 99 }
YusukeWakuta 24:72f9cb6e9440 100 }
YusukeWakuta 24:72f9cb6e9440 101
YusukeWakuta 20:d4951f491642 102 //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
YusukeWakuta 26:745735b0479d 103 float MatchUpperAndLower(InputType it, float max,float min,float neutral,float value)
YusukeWakuta 18:31722545ecf1 104 {
YusukeWakuta 20:d4951f491642 105 float Upper = max- neutral;
YusukeWakuta 20:d4951f491642 106 float Lower = neutral - min;
YusukeWakuta 25:12408563540d 107
YusukeWakuta 26:745735b0479d 108 if(it == enumRoll) {
YusukeWakuta 25:12408563540d 109 if(Upper > Lower) {
YusukeWakuta 25:12408563540d 110 if(value < neutral) {
YusukeWakuta 25:12408563540d 111 value = neutral - ((neutral - value) * (Upper / Lower));
YusukeWakuta 26:745735b0479d 112 // rollLowerDiff = (rollLowerDiff)* (Upper / Lower); //現状範囲値を変えれてないから、ここは要対応。0で割ってしまってnanが出るようになったからコメントアウトした。
YusukeWakuta 25:12408563540d 113 }
YusukeWakuta 25:12408563540d 114 } else {
YusukeWakuta 25:12408563540d 115 if(value > neutral) {
YusukeWakuta 25:12408563540d 116 value = neutral + ((value - neutral) * (Lower / Upper));
YusukeWakuta 26:745735b0479d 117 // rollUpperDiff = rollUpperDiff* (Upper / Lower);
YusukeWakuta 25:12408563540d 118 }
YusukeWakuta 20:d4951f491642 119 }
YusukeWakuta 25:12408563540d 120 return value;
YusukeWakuta 26:745735b0479d 121 } else if(it == enumPitch) {
YusukeWakuta 25:12408563540d 122 if(Upper > Lower) {
YusukeWakuta 25:12408563540d 123 if(value < neutral) {
YusukeWakuta 25:12408563540d 124 value = neutral - ((neutral - value) * (Upper / Lower));
YusukeWakuta 26:745735b0479d 125 // pitchLowerDiff = (pitchLowerDiff)* (Upper / Lower);
YusukeWakuta 25:12408563540d 126 }
YusukeWakuta 25:12408563540d 127 } else {
YusukeWakuta 25:12408563540d 128 if(value > neutral) {
YusukeWakuta 25:12408563540d 129 value = neutral + ((value - neutral) * (Lower / Upper));
YusukeWakuta 26:745735b0479d 130 // pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower);
YusukeWakuta 25:12408563540d 131 }
YusukeWakuta 20:d4951f491642 132 }
YusukeWakuta 25:12408563540d 133 return value;
YusukeWakuta 27:1af92a603532 134 } else return value;
YusukeWakuta 18:31722545ecf1 135 }
YusukeWakuta 18:31722545ecf1 136
YusukeWakuta 21:9b1f5123f4a8 137 //範囲外に値がない場合にエラーが発生するので範囲内に収める
YusukeWakuta 21:9b1f5123f4a8 138 float Format2Range(float value,float max,float min)
YusukeWakuta 18:31722545ecf1 139 {
YusukeWakuta 18:31722545ecf1 140 float result;
YusukeWakuta 21:9b1f5123f4a8 141
YusukeWakuta 18:31722545ecf1 142 if(value > max)
YusukeWakuta 18:31722545ecf1 143 result= max;
YusukeWakuta 18:31722545ecf1 144 else if(value < min)
YusukeWakuta 18:31722545ecf1 145 result = min;
YusukeWakuta 21:9b1f5123f4a8 146 else if(value > 1)
YusukeWakuta 21:9b1f5123f4a8 147 result = 1;
YusukeWakuta 21:9b1f5123f4a8 148 else if(value < 0)
YusukeWakuta 21:9b1f5123f4a8 149 result = 0;
YusukeWakuta 18:31722545ecf1 150 else
YusukeWakuta 18:31722545ecf1 151 result = value;
YusukeWakuta 18:31722545ecf1 152 return result;
YusukeWakuta 18:31722545ecf1 153 }
YusukeWakuta 18:31722545ecf1 154
YusukeWakuta 21:9b1f5123f4a8 155 //値をint型の段階に分ける
YusukeWakuta 18:31722545ecf1 156 int PhaseFloat(float value,float max,float min)
YusukeWakuta 18:31722545ecf1 157 {
YusukeWakuta 18:31722545ecf1 158 float PhaseWidth = (max - min) / PHASE_NUM;
YusukeWakuta 18:31722545ecf1 159 if(value< max&& value > min) {
YusukeWakuta 18:31722545ecf1 160 for(int i = 1; i <= PHASE_NUM; i++) {
YusukeWakuta 18:31722545ecf1 161 if(value < min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) )
YusukeWakuta 18:31722545ecf1 162 return i;
YusukeWakuta 18:31722545ecf1 163 }
YusukeWakuta 18:31722545ecf1 164 } else if(value <= min)
YusukeWakuta 27:1af92a603532 165 return 1;
YusukeWakuta 18:31722545ecf1 166 else if(value>=max)
YusukeWakuta 18:31722545ecf1 167 return PHASE_NUM;
YusukeWakuta 18:31722545ecf1 168 }
YusukeWakuta 18:31722545ecf1 169
YusukeWakuta 29:8bff47ab602d 170 float SetRollPitchRacio(float pitch,float roll){
YusukeWakuta 29:8bff47ab602d 171 return (roll + pitch * PITCHPERROLL) / (1.0 + PITCHPERROLL);
YusukeWakuta 29:8bff47ab602d 172 }
YusukeWakuta 29:8bff47ab602d 173
YusukeWakuta 15:c5ade23e5de4 174 void InputControlValues()
YusukeWakuta 15:c5ade23e5de4 175 {
YusukeWakuta 26:745735b0479d 176 setMaxAndMin(enumRoll, rollPin.read());
YusukeWakuta 26:745735b0479d 177 setMaxAndMin(enumPitch, pitchPin.read());
YusukeWakuta 26:745735b0479d 178
YusukeWakuta 26:745735b0479d 179 // 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());
YusukeWakuta 26:745735b0479d 180 float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollLowerDiff + rollLowerDiff,rollNeutral,rollPin.read() + neutralDiff);
YusukeWakuta 26:745735b0479d 181 float MatchedPitch = MatchUpperAndLower(enumPitch, pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchPin.read());
YusukeWakuta 24:72f9cb6e9440 182
YusukeWakuta 29:8bff47ab602d 183 float FormatedRoll_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll ),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
YusukeWakuta 29:8bff47ab602d 184 *(int *)inputDatas_R =PhaseFloat(FormatedRoll_R,SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
YusukeWakuta 24:72f9cb6e9440 185
YusukeWakuta 29:8bff47ab602d 186 float FormatedRoll_L = Format2Range(SetRollPitchRacio(MatchedPitch, - MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,-rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral +pitchLowerDiff, - rollNeutral - rollUpperDiff));
YusukeWakuta 27:1af92a603532 187 //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);
YusukeWakuta 29:8bff47ab602d 188 *(int *)inputDatas_L = PhaseFloat(FormatedRoll_L,SetRollPitchRacio(pitchNeutral + pitchUpperDiff ,- rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff ,- rollNeutral - rollUpperDiff));
YusukeWakuta 27:1af92a603532 189 //pc.printf("Format:%f max:%f min:%f\n\r",FormatedRoll_L,(pitchNeutral + pitchUpperDiff - rollNeutral - rollLowerDiff) / 2.0,(pitchNeutral + pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0);
YusukeWakuta 28:056287cb001c 190 if(*(int *)inputDatas_R < 1)
YusukeWakuta 28:056287cb001c 191 *(int *)inputDatas_R = 1;
YusukeWakuta 28:056287cb001c 192 else if(*(int *)inputDatas_R > PHASE_NUM)
YusukeWakuta 28:056287cb001c 193 *(int *)inputDatas_R = PHASE_NUM;
YusukeWakuta 28:056287cb001c 194 if(*(int *)inputDatas_L < 1)
YusukeWakuta 28:056287cb001c 195 *(int *)inputDatas_L = 1 ;
YusukeWakuta 28:056287cb001c 196 else if(*(int *)inputDatas_L > PHASE_NUM)
YusukeWakuta 28:056287cb001c 197 *(int *)inputDatas_L =PHASE_NUM;
YusukeWakuta 16:336e8b102555 198
YusukeWakuta 27:1af92a603532 199 pc.printf("input_R:%d input_L:%d\n\r",*(int *)inputDatas_R,*(int *)inputDatas_L);
tsumagari 10:b7159feb11fd 200 inputDatas_R[4] = (char)drug_R;
YusukeWakuta 15:c5ade23e5de4 201 led4 =! led4;
YusukeWakuta 26:745735b0479d 202 // pc.printf("%c",*(char *)inputDatas_R[4]);
YusukeWakuta 15:c5ade23e5de4 203 //pc.printf("%c",(char)drug_R);
tsumagari 10:b7159feb11fd 204 inputDatas_L[4] = (char)drug_L;
YusukeWakuta 15:c5ade23e5de4 205 // pc.printf("");
YusukeWakuta 0:b1290ca6c4a2 206 }
YusukeWakuta 0:b1290ca6c4a2 207
YusukeWakuta 15:c5ade23e5de4 208 void SendDatas()
YusukeWakuta 15:c5ade23e5de4 209 {
tsumagari 10:b7159feb11fd 210 can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM));
tsumagari 10:b7159feb11fd 211 can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, INPUT_DATAS_NUM));
tsumagari 8:dbc8c87dac78 212 toKeikiSerial.putc(';');
YusukeWakuta 15:c5ade23e5de4 213 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
YusukeWakuta 0:b1290ca6c4a2 214 toKeikiSerial.putc(yokutanDatas_R[i]);
YusukeWakuta 0:b1290ca6c4a2 215 toKeikiSerial.putc(yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 216 }
YusukeWakuta 15:c5ade23e5de4 217 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 218 toKeikiSerial.putc(inputDatas_R[i]);
taurin 4:4a13dd263a7b 219 toKeikiSerial.putc(inputDatas_L[i]);
taurin 4:4a13dd263a7b 220 }
YusukeWakuta 0:b1290ca6c4a2 221 }
tsumagari 8:dbc8c87dac78 222
YusukeWakuta 15:c5ade23e5de4 223 void init()
YusukeWakuta 15:c5ade23e5de4 224 {
tsumagari 9:c32999b4d2f9 225 //--------------------------------------(resetInterrupt init)
tsumagari 9:c32999b4d2f9 226 resetPin.rise(resetInterrupt);
tsumagari 9:c32999b4d2f9 227 resetPin.mode(PullDown);
tsumagari 9:c32999b4d2f9 228 //-----------------------------------------------------------
tsumagari 9:c32999b4d2f9 229 // sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
YusukeWakuta 15:c5ade23e5de4 230 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 2:9dc7d5f1e910 231 inputDatas_R[i] = 0;
taurin 2:9dc7d5f1e910 232 inputDatas_L[i] = 0;
taurin 2:9dc7d5f1e910 233 }
YusukeWakuta 0:b1290ca6c4a2 234 }
YusukeWakuta 0:b1290ca6c4a2 235
YusukeWakuta 15:c5ade23e5de4 236 void receiveDatas()
YusukeWakuta 15:c5ade23e5de4 237 {
YusukeWakuta 15:c5ade23e5de4 238 if(can_R.read(recmsg_R)) {
YusukeWakuta 15:c5ade23e5de4 239 for(int i = 0; i < recmsg_R.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 240 yokutanDatas_R[i] = recmsg_R.data[i];
YusukeWakuta 15:c5ade23e5de4 241 // pc.printf("%c",yokutanDatas_R[i]);
YusukeWakuta 0:b1290ca6c4a2 242 }
YusukeWakuta 25:12408563540d 243 led1 = !led1;
YusukeWakuta 0:b1290ca6c4a2 244 }
YusukeWakuta 15:c5ade23e5de4 245 if(can_L.read(recmsg_L)) {
YusukeWakuta 15:c5ade23e5de4 246 for(int i = 0; i < recmsg_L.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 247 yokutanDatas_L[i] = recmsg_L.data[i];
YusukeWakuta 15:c5ade23e5de4 248 led3 = !led3;
YusukeWakuta 15:c5ade23e5de4 249 // pc.printf("%c",yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 250 }
YusukeWakuta 25:12408563540d 251 led2 = !led2;
YusukeWakuta 0:b1290ca6c4a2 252 }
YusukeWakuta 0:b1290ca6c4a2 253 }
YusukeWakuta 0:b1290ca6c4a2 254
YusukeWakuta 15:c5ade23e5de4 255 int main()
YusukeWakuta 15:c5ade23e5de4 256 {
YusukeWakuta 0:b1290ca6c4a2 257 init();
YusukeWakuta 24:72f9cb6e9440 258 setNeutral();
YusukeWakuta 15:c5ade23e5de4 259 while(1) {
tsumagari 8:dbc8c87dac78 260 InputControlValues();
tsumagari 10:b7159feb11fd 261 wait_us(5);
YusukeWakuta 0:b1290ca6c4a2 262 receiveDatas();
tsumagari 9:c32999b4d2f9 263 SendDatas();
YusukeWakuta 0:b1290ca6c4a2 264 wait(WAIT_LOOP_TIME);
YusukeWakuta 0:b1290ca6c4a2 265 }
YusukeWakuta 0:b1290ca6c4a2 266 }