albatross / Mbed 2 deprecated ControlMain2017_2

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

Dependencies:   mbed-rtos mbed

Fork of ControlMain2017 by albatross

main.cpp@23:358e97dab762, 2017-03-20 (annotated)

Committer:
YusukeWakuta
Date:
Mon Mar 20 09:50:04 2017 +0000
Revision:
23:358e97dab762
Parent:
22:5253367e7d78 
???????

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 16:336e8b102555 10 #define SUM_UP_NUM 20
YusukeWakuta 15:c5ade23e5de4 11 #define SAMPLE_INTERVAL 0.05
YusukeWakuta 0:b1290ca6c4a2 12
YusukeWakuta 22:5253367e7d78 13 #define PITCH_NEUTRAL 0.468 //1って書いた方
YusukeWakuta 22:5253367e7d78 14 #define PITCH_MAX 0.884
YusukeWakuta 22:5253367e7d78 15 #define PITCH_MIN 0.110
YusukeWakuta 18:31722545ecf1 16
YusukeWakuta 19:6387e3f02b37 17 #define OLD_ROLL_NEUTRAL 0.739
YusukeWakuta 22:5253367e7d78 18 #define ROLL_NEUTRAL 0.468 //２て書いた方 //中心の値は二つでお同じにする
YusukeWakuta 22:5253367e7d78 19 #define ROLL_MAX 0.963 - (OLD_ROLL_NEUTRAL - PITCH_NEUTRAL)
YusukeWakuta 22:5253367e7d78 20 #define ROLL_MIN 0.555 - (OLD_ROLL_NEUTRAL - PITCH_NEUTRAL)
YusukeWakuta 18:31722545ecf1 21
YusukeWakuta 22:5253367e7d78 22 #define PHASE_NUM 12 //偶数にしてください
YusukeWakuta 19:6387e3f02b37 23 /*
YusukeWakuta 19:6387e3f02b37 24 roll入力とピッチ入力だとピッチの方が1.5倍効くように
YusukeWakuta 19:6387e3f02b37 25 */
YusukeWakuta 18:31722545ecf1 26
tsumagari 9:c32999b4d2f9 27 //-----------------------------------(resetInterrupt def)
tsumagari 9:c32999b4d2f9 28 extern "C" void mbed_reset();
YusukeWakuta 13:79eb74eb86c4 29 InterruptIn resetPin(p22);
tsumagari 9:c32999b4d2f9 30 Timer resetTimeCount;
YusukeWakuta 15:c5ade23e5de4 31 void resetInterrupt()
YusukeWakuta 15:c5ade23e5de4 32 {
YusukeWakuta 15:c5ade23e5de4 33 while(resetPin) {
tsumagari 9:c32999b4d2f9 34 resetTimeCount.start();
tsumagari 9:c32999b4d2f9 35 if(resetTimeCount.read()>3) mbed_reset();
tsumagari 9:c32999b4d2f9 36 }
tsumagari 9:c32999b4d2f9 37 resetTimeCount.reset();
tsumagari 9:c32999b4d2f9 38 }
tsumagari 9:c32999b4d2f9 39 //-------------------------------------------------------
tsumagari 9:c32999b4d2f9 40
taurin 6:7484315ba88a 41 CAN can_R(p30,p29);
taurin 6:7484315ba88a 42 CAN can_L(p9,p10);
YusukeWakuta 14:ffe9460922cb 43 Serial toKeikiSerial(p28,p27);
tsumagari 7:59ddbe239835 44 Serial pc(USBTX,USBRX);
tsumagari 10:b7159feb11fd 45 AnalogIn rollPin(p15);
YusukeWakuta 18:31722545ecf1 46 AnalogIn pitchPin(p20);
YusukeWakuta 13:79eb74eb86c4 47 DigitalIn drug_R(p14);
taurin 6:7484315ba88a 48 DigitalIn drug_L(p19);
YusukeWakuta 0:b1290ca6c4a2 49 DigitalOut myled1(LED1);
YusukeWakuta 0:b1290ca6c4a2 50 DigitalOut myled2(LED2);
YusukeWakuta 15:c5ade23e5de4 51 DigitalOut led4(LED4);
YusukeWakuta 15:c5ade23e5de4 52 DigitalOut led3(LED3);
tsumagari 9:c32999b4d2f9 53 //Ticker sendDatasTicker;
YusukeWakuta 0:b1290ca6c4a2 54
taurin 4:4a13dd263a7b 55 char yokutanDatas_R[YOKUTAN_DATAS_NUM];
taurin 4:4a13dd263a7b 56 char yokutanDatas_L[YOKUTAN_DATAS_NUM];
tsumagari 10:b7159feb11fd 57 char inputDatas_R[INPUT_DATAS_NUM];
tsumagari 10:b7159feb11fd 58 char inputDatas_L[INPUT_DATAS_NUM];
taurin 4:4a13dd263a7b 59 bool stateP = true;
taurin 4:4a13dd263a7b 60 bool stateQ = true;
taurin 4:4a13dd263a7b 61 bool stateP_old = true;
taurin 4:4a13dd263a7b 62 bool stateQ_old = true;
YusukeWakuta 20:d4951f491642 63 float changedRollMax;
YusukeWakuta 20:d4951f491642 64 float changedRollMin;
YusukeWakuta 20:d4951f491642 65 float changedPitchMax;
YusukeWakuta 20:d4951f491642 66 float changedPitchMin;
YusukeWakuta 0:b1290ca6c4a2 67
YusukeWakuta 3:0e66ce2ab2fb 68 CANMessage recmsg_R;
YusukeWakuta 3:0e66ce2ab2fb 69 CANMessage recmsg_L;
YusukeWakuta 0:b1290ca6c4a2 70
YusukeWakuta 20:d4951f491642 71 //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
YusukeWakuta 20:d4951f491642 72 float MatchRollUpperAndLower(float max,float min,float neutral,float value)
YusukeWakuta 18:31722545ecf1 73 {
YusukeWakuta 20:d4951f491642 74 float Upper = max- neutral;
YusukeWakuta 20:d4951f491642 75 float Lower = neutral - min;
YusukeWakuta 20:d4951f491642 76 if(Upper > Lower) {
YusukeWakuta 20:d4951f491642 77 if(value < neutral) {
YusukeWakuta 20:d4951f491642 78 value = neutral - ((neutral - value) * (Upper / Lower));
YusukeWakuta 22:5253367e7d78 79 changedRollMin = ROLL_NEUTRAL - (ROLL_NEUTRAL - ROLL_MIN)* (Upper / Lower);
YusukeWakuta 22:5253367e7d78 80 changedRollMax = ROLL_MAX;
YusukeWakuta 20:d4951f491642 81 }
YusukeWakuta 20:d4951f491642 82 } else {
YusukeWakuta 20:d4951f491642 83 if(value > neutral) {
YusukeWakuta 20:d4951f491642 84 value = neutral + ((value - neutral) * (Lower / Upper));
YusukeWakuta 22:5253367e7d78 85 changedRollMax = ROLL_NEUTRAL + (ROLL_MAX - ROLL_NEUTRAL)* (Upper / Lower);
YusukeWakuta 22:5253367e7d78 86 changedRollMin = ROLL_MIN;
YusukeWakuta 20:d4951f491642 87 }
YusukeWakuta 20:d4951f491642 88 }
YusukeWakuta 20:d4951f491642 89 return value;
YusukeWakuta 20:d4951f491642 90 }
YusukeWakuta 20:d4951f491642 91
YusukeWakuta 20:d4951f491642 92 //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
YusukeWakuta 20:d4951f491642 93 float MatchPitchUpperAndLower(float max,float min,float neutral,float value)
YusukeWakuta 20:d4951f491642 94 {
YusukeWakuta 20:d4951f491642 95 float Upper = max- neutral;
YusukeWakuta 20:d4951f491642 96 float Lower = neutral - min;
YusukeWakuta 20:d4951f491642 97 if(Upper > Lower) {
YusukeWakuta 20:d4951f491642 98 if(value < neutral) {
YusukeWakuta 20:d4951f491642 99 value = neutral - ((neutral - value) * (Upper / Lower));
YusukeWakuta 22:5253367e7d78 100 //ここで範囲値を再設定する
YusukeWakuta 22:5253367e7d78 101 changedPitchMin = PITCH_NEUTRAL - (PITCH_NEUTRAL - PITCH_MIN)* (Upper / Lower);
YusukeWakuta 22:5253367e7d78 102 changedPitchMax = PITCH_MAX;
YusukeWakuta 20:d4951f491642 103 }
YusukeWakuta 20:d4951f491642 104 } else {
YusukeWakuta 20:d4951f491642 105 if(value > neutral) {
YusukeWakuta 20:d4951f491642 106 value = neutral + ((value - neutral) * (Lower / Upper));
YusukeWakuta 22:5253367e7d78 107 changedPitchMax = PITCH_NEUTRAL + (PITCH_MAX - PITCH_NEUTRAL)* (Upper / Lower);
YusukeWakuta 22:5253367e7d78 108 changedPitchMin = PITCH_MIN;
YusukeWakuta 20:d4951f491642 109 }
YusukeWakuta 20:d4951f491642 110 }
YusukeWakuta 20:d4951f491642 111 return value;
YusukeWakuta 18:31722545ecf1 112 }
YusukeWakuta 18:31722545ecf1 113
YusukeWakuta 21:9b1f5123f4a8 114 //範囲外に値がない場合にエラーが発生するので範囲内に収める
YusukeWakuta 21:9b1f5123f4a8 115 float Format2Range(float value,float max,float min)
YusukeWakuta 18:31722545ecf1 116 {
YusukeWakuta 18:31722545ecf1 117 float result;
YusukeWakuta 18:31722545ecf1 118 if(value > max)
YusukeWakuta 18:31722545ecf1 119 result= max;
YusukeWakuta 18:31722545ecf1 120 else if(value < min)
YusukeWakuta 18:31722545ecf1 121 result = min;
YusukeWakuta 21:9b1f5123f4a8 122 else if(value > 1)
YusukeWakuta 21:9b1f5123f4a8 123 result = 1;
YusukeWakuta 21:9b1f5123f4a8 124 else if(value < 0)
YusukeWakuta 21:9b1f5123f4a8 125 result = 0;
YusukeWakuta 18:31722545ecf1 126 else
YusukeWakuta 18:31722545ecf1 127 result = value;
YusukeWakuta 18:31722545ecf1 128 return result;
YusukeWakuta 18:31722545ecf1 129 }
YusukeWakuta 18:31722545ecf1 130
YusukeWakuta 21:9b1f5123f4a8 131 //値をint型の段階に分ける
YusukeWakuta 18:31722545ecf1 132 int PhaseFloat(float value,float max,float min)
YusukeWakuta 18:31722545ecf1 133 {
YusukeWakuta 18:31722545ecf1 134 float PhaseWidth = (max - min) / PHASE_NUM;
YusukeWakuta 18:31722545ecf1 135 if(value< max&& value > min) {
YusukeWakuta 18:31722545ecf1 136 for(int i = 1; i <= PHASE_NUM; i++) {
YusukeWakuta 18:31722545ecf1 137 if(value < min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) )
YusukeWakuta 18:31722545ecf1 138 return i;
YusukeWakuta 18:31722545ecf1 139 }
YusukeWakuta 18:31722545ecf1 140 } else if(value <= min)
YusukeWakuta 18:31722545ecf1 141 return 0;
YusukeWakuta 18:31722545ecf1 142 else if(value>=max)
YusukeWakuta 18:31722545ecf1 143 return PHASE_NUM;
YusukeWakuta 18:31722545ecf1 144 }
YusukeWakuta 18:31722545ecf1 145
YusukeWakuta 15:c5ade23e5de4 146 void InputControlValues()
YusukeWakuta 15:c5ade23e5de4 147 {
YusukeWakuta 22:5253367e7d78 148 pc.printf("Roll:%f Pitch:%f ",rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_NEUTRAL),pitchPin.read());
YusukeWakuta 22:5253367e7d78 149 float MatchedRoll = MatchRollUpperAndLower(ROLL_MAX,ROLL_MIN,ROLL_NEUTRAL,rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_NEUTRAL));
YusukeWakuta 22:5253367e7d78 150 float MatchedPitch = MatchPitchUpperAndLower(PITCH_MAX,ROLL_MIN,PITCH_NEUTRAL,pitchPin.read());
YusukeWakuta 22:5253367e7d78 151
YusukeWakuta 22:5253367e7d78 152 float FormatedR = Format2Range((MatchedPitch +MatchedRoll ) / 2.0,(changedRollMax + changedPitchMax) / 2.0,(changedRollMin + changedPitchMin) / 2.0);
YusukeWakuta 23:358e97dab762 153 *(int *)inputDatas_R =(int)PhaseFloat(FormatedR,(changedRollMax + changedPitchMax) / 2.0,(changedRollMin + changedPitchMin) / 2.0);
YusukeWakuta 22:5253367e7d78 154
YusukeWakuta 22:5253367e7d78 155 float FormatedL = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(changedPitchMax - changedRollMin) /2.0,(changedPitchMin - changedRollMax) / 2.0);
YusukeWakuta 23:358e97dab762 156 *(int *)inputDatas_L = (int)PhaseFloat(FormatedL,changedPitchMax - changedRollMin,changedPitchMin - changedRollMax);
YusukeWakuta 16:336e8b102555 157
YusukeWakuta 19:6387e3f02b37 158 if(*(int *)inputDatas_R < 0)
YusukeWakuta 19:6387e3f02b37 159 *(int *)inputDatas_R = 0;
YusukeWakuta 18:31722545ecf1 160 else if(*(int *)inputDatas_R > PHASE_NUM)
YusukeWakuta 18:31722545ecf1 161 *(int *)inputDatas_R = PHASE_NUM;
YusukeWakuta 19:6387e3f02b37 162 if(*(int *)inputDatas_L < 0)
YusukeWakuta 19:6387e3f02b37 163 *(int *)inputDatas_L = 0 ;
YusukeWakuta 18:31722545ecf1 164 else if(*(int *)inputDatas_L > PHASE_NUM)
YusukeWakuta 18:31722545ecf1 165 *(int *)inputDatas_L =PHASE_NUM;
YusukeWakuta 16:336e8b102555 166
YusukeWakuta 22:5253367e7d78 167 pc.printf("input_R:%d input_L:%d ",*(int *)inputDatas_R,*(int *)inputDatas_L);
tsumagari 10:b7159feb11fd 168 inputDatas_R[4] = (char)drug_R;
YusukeWakuta 15:c5ade23e5de4 169 led4 =! led4;
YusukeWakuta 15:c5ade23e5de4 170 pc.printf("%c",*(char *)inputDatas_R[4]);
tsumagari 10:b7159feb11fd 171 inputDatas_L[4] = (char)drug_L;
YusukeWakuta 0:b1290ca6c4a2 172 }
YusukeWakuta 0:b1290ca6c4a2 173
YusukeWakuta 18:31722545ecf1 174
YusukeWakuta 18:31722545ecf1 175
YusukeWakuta 15:c5ade23e5de4 176 void toString_R()
YusukeWakuta 15:c5ade23e5de4 177 {
taurin 4:4a13dd263a7b 178 pc.printf("R:");
YusukeWakuta 15:c5ade23e5de4 179 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 180 pc.printf("%d:%i ",i,inputDatas_R[i]);
taurin 4:4a13dd263a7b 181 }
YusukeWakuta 0:b1290ca6c4a2 182 }
YusukeWakuta 0:b1290ca6c4a2 183
YusukeWakuta 15:c5ade23e5de4 184 void toString_L()
YusukeWakuta 15:c5ade23e5de4 185 {
taurin 4:4a13dd263a7b 186 pc.printf("L:");
YusukeWakuta 15:c5ade23e5de4 187 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 188 pc.printf("%d:%i ",i,inputDatas_L[i]);
taurin 4:4a13dd263a7b 189 }
taurin 4:4a13dd263a7b 190 pc.printf("\n\r");
YusukeWakuta 0:b1290ca6c4a2 191 }
YusukeWakuta 0:b1290ca6c4a2 192
YusukeWakuta 15:c5ade23e5de4 193 void SendDatas()
YusukeWakuta 15:c5ade23e5de4 194 {
tsumagari 10:b7159feb11fd 195 can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM));
tsumagari 10:b7159feb11fd 196 can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, INPUT_DATAS_NUM));
tsumagari 8:dbc8c87dac78 197 toKeikiSerial.putc(';');
YusukeWakuta 15:c5ade23e5de4 198 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
YusukeWakuta 0:b1290ca6c4a2 199 toKeikiSerial.putc(yokutanDatas_R[i]);
YusukeWakuta 0:b1290ca6c4a2 200 toKeikiSerial.putc(yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 201 }
YusukeWakuta 15:c5ade23e5de4 202 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 203 toKeikiSerial.putc(inputDatas_R[i]);
taurin 4:4a13dd263a7b 204 toKeikiSerial.putc(inputDatas_L[i]);
taurin 4:4a13dd263a7b 205 }
YusukeWakuta 0:b1290ca6c4a2 206 }
tsumagari 8:dbc8c87dac78 207
YusukeWakuta 15:c5ade23e5de4 208 void init()
YusukeWakuta 15:c5ade23e5de4 209 {
tsumagari 9:c32999b4d2f9 210 //--------------------------------------(resetInterrupt init)
tsumagari 9:c32999b4d2f9 211 resetPin.rise(resetInterrupt);
tsumagari 9:c32999b4d2f9 212 resetPin.mode(PullDown);
tsumagari 9:c32999b4d2f9 213 //-----------------------------------------------------------
tsumagari 9:c32999b4d2f9 214 // sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
YusukeWakuta 15:c5ade23e5de4 215 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 2:9dc7d5f1e910 216 inputDatas_R[i] = 0;
taurin 2:9dc7d5f1e910 217 inputDatas_L[i] = 0;
taurin 2:9dc7d5f1e910 218 }
YusukeWakuta 0:b1290ca6c4a2 219 }
YusukeWakuta 0:b1290ca6c4a2 220
YusukeWakuta 15:c5ade23e5de4 221 void receiveDatas()
YusukeWakuta 15:c5ade23e5de4 222 {
YusukeWakuta 15:c5ade23e5de4 223 if(can_R.read(recmsg_R)) {
YusukeWakuta 15:c5ade23e5de4 224 for(int i = 0; i < recmsg_R.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 225 yokutanDatas_R[i] = recmsg_R.data[i];
YusukeWakuta 15:c5ade23e5de4 226 // pc.printf("%c",yokutanDatas_R[i]);
YusukeWakuta 0:b1290ca6c4a2 227 }
taurin 4:4a13dd263a7b 228 myled1 = !myled1;
YusukeWakuta 0:b1290ca6c4a2 229 }
YusukeWakuta 15:c5ade23e5de4 230 if(can_L.read(recmsg_L)) {
YusukeWakuta 15:c5ade23e5de4 231 for(int i = 0; i < recmsg_L.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 232 yokutanDatas_L[i] = recmsg_L.data[i];
YusukeWakuta 15:c5ade23e5de4 233 led3 = !led3;
YusukeWakuta 15:c5ade23e5de4 234 // pc.printf("%c",yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 235 }
YusukeWakuta 0:b1290ca6c4a2 236 }
YusukeWakuta 0:b1290ca6c4a2 237 }
YusukeWakuta 0:b1290ca6c4a2 238
YusukeWakuta 15:c5ade23e5de4 239 int main()
YusukeWakuta 15:c5ade23e5de4 240 {
YusukeWakuta 0:b1290ca6c4a2 241 init();
YusukeWakuta 15:c5ade23e5de4 242 while(1) {
YusukeWakuta 22:5253367e7d78 243 myled2 =! myled2;
tsumagari 8:dbc8c87dac78 244 InputControlValues();
tsumagari 10:b7159feb11fd 245 wait_us(5);
YusukeWakuta 0:b1290ca6c4a2 246 receiveDatas();
tsumagari 9:c32999b4d2f9 247 SendDatas();
YusukeWakuta 0:b1290ca6c4a2 248 wait(WAIT_LOOP_TIME);
YusukeWakuta 0:b1290ca6c4a2 249 }
YusukeWakuta 0:b1290ca6c4a2 250 }