albatross / Mbed 2 deprecated ControlMain2017_2

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

Dependencies:   mbed-rtos mbed

Fork of ControlMain2017 by albatross

main.cpp@18:31722545ecf1, 2017-03-19 (annotated)

Committer:
YusukeWakuta
Date:
Sun Mar 19 10:44:14 2017 +0000
Revision:
18:31722545ecf1
Parent:
16:336e8b102555
Child:
19:6387e3f02b37
itiou

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 18:31722545ecf1 13 #define PITCH_INPUT_NEUTRAL 0.457 //1って書いた方
YusukeWakuta 18:31722545ecf1 14 #define PITCH_INPUT_MAX 0.884
YusukeWakuta 18:31722545ecf1 15 #define PITCH_INPUT_MIN 0.110
YusukeWakuta 18:31722545ecf1 16
YusukeWakuta 18:31722545ecf1 17 #define ROLL_INPUT_NEUTRAL 0.739 //２て書いた方
YusukeWakuta 18:31722545ecf1 18 #define ROLL_INPUT_MAX 0.963
YusukeWakuta 18:31722545ecf1 19 #define ROLL_INPUT_MIN 0.555
YusukeWakuta 18:31722545ecf1 20
YusukeWakuta 18:31722545ecf1 21 #define SUMED_INPUT_NEUTRAL 0.598
YusukeWakuta 18:31722545ecf1 22 #define SUMED_INPUT_MAX 0.9235
YusukeWakuta 18:31722545ecf1 23 #define SUMED_INPUT_MIN 0.3325
YusukeWakuta 18:31722545ecf1 24
YusukeWakuta 18:31722545ecf1 25 #define DEC_INPUT_NEUTRAL -0.141
YusukeWakuta 18:31722545ecf1 26 #define DEC_INPUT_MAX -0.0395
YusukeWakuta 18:31722545ecf1 27 #define DEC_INPUT_MIN -0.2225
YusukeWakuta 18:31722545ecf1 28 #define PHASE_NUM 7 //奇数にしてください
YusukeWakuta 18:31722545ecf1 29
tsumagari 9:c32999b4d2f9 30 //-----------------------------------(resetInterrupt def)
tsumagari 9:c32999b4d2f9 31 extern "C" void mbed_reset();
YusukeWakuta 13:79eb74eb86c4 32 InterruptIn resetPin(p22);
tsumagari 9:c32999b4d2f9 33 Timer resetTimeCount;
YusukeWakuta 15:c5ade23e5de4 34 void resetInterrupt()
YusukeWakuta 15:c5ade23e5de4 35 {
YusukeWakuta 15:c5ade23e5de4 36 while(resetPin) {
tsumagari 9:c32999b4d2f9 37 resetTimeCount.start();
tsumagari 9:c32999b4d2f9 38 if(resetTimeCount.read()>3) mbed_reset();
tsumagari 9:c32999b4d2f9 39 }
tsumagari 9:c32999b4d2f9 40 resetTimeCount.reset();
tsumagari 9:c32999b4d2f9 41 }
tsumagari 9:c32999b4d2f9 42 //-------------------------------------------------------
tsumagari 9:c32999b4d2f9 43
taurin 6:7484315ba88a 44 CAN can_R(p30,p29);
taurin 6:7484315ba88a 45 CAN can_L(p9,p10);
YusukeWakuta 14:ffe9460922cb 46 Serial toKeikiSerial(p28,p27);
tsumagari 7:59ddbe239835 47 Serial pc(USBTX,USBRX);
tsumagari 10:b7159feb11fd 48 AnalogIn rollPin(p15);
YusukeWakuta 18:31722545ecf1 49 AnalogIn pitchPin(p20);
YusukeWakuta 13:79eb74eb86c4 50 DigitalIn drug_R(p14);
taurin 6:7484315ba88a 51 DigitalIn drug_L(p19);
YusukeWakuta 0:b1290ca6c4a2 52 DigitalOut myled1(LED1);
YusukeWakuta 0:b1290ca6c4a2 53 DigitalOut myled2(LED2);
YusukeWakuta 15:c5ade23e5de4 54 DigitalOut led4(LED4);
YusukeWakuta 15:c5ade23e5de4 55 DigitalOut led3(LED3);
tsumagari 9:c32999b4d2f9 56 //Ticker sendDatasTicker;
YusukeWakuta 0:b1290ca6c4a2 57
taurin 4:4a13dd263a7b 58 char yokutanDatas_R[YOKUTAN_DATAS_NUM];
taurin 4:4a13dd263a7b 59 char yokutanDatas_L[YOKUTAN_DATAS_NUM];
tsumagari 10:b7159feb11fd 60 char inputDatas_R[INPUT_DATAS_NUM];
tsumagari 10:b7159feb11fd 61 char inputDatas_L[INPUT_DATAS_NUM];
taurin 4:4a13dd263a7b 62 bool stateP = true;
taurin 4:4a13dd263a7b 63 bool stateQ = true;
taurin 4:4a13dd263a7b 64 bool stateP_old = true;
taurin 4:4a13dd263a7b 65 bool stateQ_old = true;
YusukeWakuta 0:b1290ca6c4a2 66
YusukeWakuta 3:0e66ce2ab2fb 67 CANMessage recmsg_R;
YusukeWakuta 3:0e66ce2ab2fb 68 CANMessage recmsg_L;
YusukeWakuta 0:b1290ca6c4a2 69
YusukeWakuta 15:c5ade23e5de4 70 float get_analogin_ave(AnalogIn pin)
YusukeWakuta 15:c5ade23e5de4 71 {
tsumagari 10:b7159feb11fd 72 float val = 0;
YusukeWakuta 15:c5ade23e5de4 73 for(int i = 0; i<SUM_UP_NUM; i++) {
tsumagari 10:b7159feb11fd 74 val += pin.read();
YusukeWakuta 15:c5ade23e5de4 75 }
YusukeWakuta 15:c5ade23e5de4 76 return val/SUM_UP_NUM;
tsumagari 10:b7159feb11fd 77 }
tsumagari 10:b7159feb11fd 78
YusukeWakuta 18:31722545ecf1 79 //値を0から1の範囲に収めます
YusukeWakuta 18:31722545ecf1 80 float FormatSumed2Range(float value)
YusukeWakuta 18:31722545ecf1 81 {
YusukeWakuta 18:31722545ecf1 82 float result;
YusukeWakuta 18:31722545ecf1 83 if(value > 1)
YusukeWakuta 18:31722545ecf1 84 result = 1;
YusukeWakuta 18:31722545ecf1 85 else if(value < 0)
YusukeWakuta 18:31722545ecf1 86 result = 0;
YusukeWakuta 18:31722545ecf1 87 else
YusukeWakuta 18:31722545ecf1 88 result = value;
YusukeWakuta 18:31722545ecf1 89 return result;
YusukeWakuta 18:31722545ecf1 90 }
YusukeWakuta 18:31722545ecf1 91
YusukeWakuta 18:31722545ecf1 92 float FormatEach2Range(float value,float max,float min)
YusukeWakuta 18:31722545ecf1 93 {
YusukeWakuta 18:31722545ecf1 94 float result;
YusukeWakuta 18:31722545ecf1 95 if(value > max)
YusukeWakuta 18:31722545ecf1 96 result= max;
YusukeWakuta 18:31722545ecf1 97 else if(value < min)
YusukeWakuta 18:31722545ecf1 98 result = min;
YusukeWakuta 18:31722545ecf1 99 else
YusukeWakuta 18:31722545ecf1 100 result = value;
YusukeWakuta 18:31722545ecf1 101 return result;
YusukeWakuta 18:31722545ecf1 102 }
YusukeWakuta 18:31722545ecf1 103
YusukeWakuta 18:31722545ecf1 104 //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
YusukeWakuta 18:31722545ecf1 105 float MatchUpperAndLower(float max,float min,float neutral,float value)
YusukeWakuta 18:31722545ecf1 106 {
YusukeWakuta 18:31722545ecf1 107 float Upper = max- neutral;
YusukeWakuta 18:31722545ecf1 108 float Lower = neutral - min;
YusukeWakuta 18:31722545ecf1 109 if(Upper > Lower) {
YusukeWakuta 18:31722545ecf1 110 if(value < neutral)
YusukeWakuta 18:31722545ecf1 111 value = neutral - ((neutral - value) * (Upper / Lower));
YusukeWakuta 18:31722545ecf1 112 } else {
YusukeWakuta 18:31722545ecf1 113 if(value > neutral)
YusukeWakuta 18:31722545ecf1 114 value = neutral + ((value - neutral) * (Lower / Upper));
YusukeWakuta 18:31722545ecf1 115 }
YusukeWakuta 18:31722545ecf1 116 return value;
YusukeWakuta 18:31722545ecf1 117 }
YusukeWakuta 18:31722545ecf1 118
YusukeWakuta 18:31722545ecf1 119 //Format2Rangeの後に呼び出してください
YusukeWakuta 18:31722545ecf1 120 int SampleFloat(float f)
YusukeWakuta 18:31722545ecf1 121 {
YusukeWakuta 18:31722545ecf1 122 int temp = ((f + 0.025) * 100.0) / 5;
YusukeWakuta 18:31722545ecf1 123 float result = temp / 20.0;
YusukeWakuta 18:31722545ecf1 124 return result;
YusukeWakuta 18:31722545ecf1 125
YusukeWakuta 18:31722545ecf1 126 }
YusukeWakuta 18:31722545ecf1 127
YusukeWakuta 18:31722545ecf1 128 int PhaseFloat(float value,float max,float min)
YusukeWakuta 18:31722545ecf1 129 {
YusukeWakuta 18:31722545ecf1 130 float PhaseWidth = (max - min) / PHASE_NUM;
YusukeWakuta 18:31722545ecf1 131 if(value< max&& value > min) {
YusukeWakuta 18:31722545ecf1 132 for(int i = 1; i <= PHASE_NUM; i++) {
YusukeWakuta 18:31722545ecf1 133 if(value < min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) )
YusukeWakuta 18:31722545ecf1 134 return i;
YusukeWakuta 18:31722545ecf1 135 }
YusukeWakuta 18:31722545ecf1 136 } else if(value <= min)
YusukeWakuta 18:31722545ecf1 137 return 0;
YusukeWakuta 18:31722545ecf1 138 else if(value>=max)
YusukeWakuta 18:31722545ecf1 139 return PHASE_NUM;
YusukeWakuta 18:31722545ecf1 140
YusukeWakuta 18:31722545ecf1 141 }
YusukeWakuta 18:31722545ecf1 142
YusukeWakuta 18:31722545ecf1 143
YusukeWakuta 15:c5ade23e5de4 144 void InputControlValues()
YusukeWakuta 15:c5ade23e5de4 145 {
YusukeWakuta 18:31722545ecf1 146 float MatchedRoll = MatchUpperAndLower(ROLL_INPUT_MAX,ROLL_INPUT_MIN,ROLL_INPUT_NEUTRAL,rollPin.read());
YusukeWakuta 18:31722545ecf1 147 float MatchedPitch = MatchUpperAndLower(PITCH_INPUT_MAX,ROLL_INPUT_MIN,PITCH_INPUT_NEUTRAL,pitchPin.read() - (ROLL_INPUT_NEUTRAL - PITCH_INPUT_NEUTRAL));
YusukeWakuta 18:31722545ecf1 148 float FormatedEachRollR = FormatEach2Range((MatchedRoll + MatchedPitch) / 2.0,SUMED_INPUT_MAX,SUMED_INPUT_MIN);
YusukeWakuta 18:31722545ecf1 149 float FormatSumedR = FormatSumed2Range(FormatedEachRollR);
YusukeWakuta 18:31722545ecf1 150 *(int *)inputDatas_R =PhaseFloat(FormatSumedR,ROLL_INPUT_MAX,ROLL_INPUT_MIN);
YusukeWakuta 18:31722545ecf1 151
YusukeWakuta 18:31722545ecf1 152 float FormatedEachRollL = FormatEach2Range((MatchedRoll - MatchedPitch) / 2.0,ROLL_INPUT_MAX,ROLL_INPUT_MIN);
YusukeWakuta 18:31722545ecf1 153 float FormatSumedL = FormatSumed2Range(FormatedEachRollL);
YusukeWakuta 18:31722545ecf1 154 *(int *)inputDatas_L = PhaseFloat(FormatSumedR,ROLL_INPUT_MAX,ROLL_INPUT_MIN);
YusukeWakuta 16:336e8b102555 155
YusukeWakuta 16:336e8b102555 156 if(*(float *)inputDatas_R < 0)
YusukeWakuta 16:336e8b102555 157 *(float *)inputDatas_R = 0;
YusukeWakuta 18:31722545ecf1 158 else if(*(int *)inputDatas_R > PHASE_NUM)
YusukeWakuta 18:31722545ecf1 159 *(int *)inputDatas_R = PHASE_NUM;
YusukeWakuta 16:336e8b102555 160 if(*(float *)inputDatas_L < 0)
YusukeWakuta 16:336e8b102555 161 *(float *)inputDatas_L = 0 ;
YusukeWakuta 18:31722545ecf1 162 else if(*(int *)inputDatas_L > PHASE_NUM)
YusukeWakuta 18:31722545ecf1 163 *(int *)inputDatas_L =PHASE_NUM;
YusukeWakuta 16:336e8b102555 164
YusukeWakuta 16:336e8b102555 165 pc.printf("input_R:%f input_L:%f\n\r",*(float *)inputDatas_R,*(float *)inputDatas_L);
tsumagari 10:b7159feb11fd 166 inputDatas_R[4] = (char)drug_R;
YusukeWakuta 15:c5ade23e5de4 167 led4 =! led4;
YusukeWakuta 15:c5ade23e5de4 168 pc.printf("%c",*(char *)inputDatas_R[4]);
YusukeWakuta 15:c5ade23e5de4 169 //pc.printf("%c",(char)drug_R);
tsumagari 10:b7159feb11fd 170 inputDatas_L[4] = (char)drug_L;
YusukeWakuta 15:c5ade23e5de4 171 // pc.printf("");
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 }
tsumagari 8:dbc8c87dac78 206 // toKeikiSerial.putc(cadence);
YusukeWakuta 15:c5ade23e5de4 207 // pc.printf("test\n\r");
YusukeWakuta 0:b1290ca6c4a2 208 }
tsumagari 8:dbc8c87dac78 209
YusukeWakuta 15:c5ade23e5de4 210 void init()
YusukeWakuta 15:c5ade23e5de4 211 {
tsumagari 9:c32999b4d2f9 212 //--------------------------------------(resetInterrupt init)
tsumagari 9:c32999b4d2f9 213 resetPin.rise(resetInterrupt);
tsumagari 9:c32999b4d2f9 214 resetPin.mode(PullDown);
tsumagari 9:c32999b4d2f9 215 //-----------------------------------------------------------
tsumagari 9:c32999b4d2f9 216 // sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
YusukeWakuta 15:c5ade23e5de4 217 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 2:9dc7d5f1e910 218 inputDatas_R[i] = 0;
taurin 2:9dc7d5f1e910 219 inputDatas_L[i] = 0;
taurin 2:9dc7d5f1e910 220 }
YusukeWakuta 0:b1290ca6c4a2 221 }
YusukeWakuta 0:b1290ca6c4a2 222
YusukeWakuta 15:c5ade23e5de4 223 void receiveDatas()
YusukeWakuta 15:c5ade23e5de4 224 {
YusukeWakuta 15:c5ade23e5de4 225 if(can_R.read(recmsg_R)) {
YusukeWakuta 15:c5ade23e5de4 226 for(int i = 0; i < recmsg_R.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 227 yokutanDatas_R[i] = recmsg_R.data[i];
YusukeWakuta 15:c5ade23e5de4 228 // pc.printf("%c",yokutanDatas_R[i]);
YusukeWakuta 0:b1290ca6c4a2 229 }
taurin 4:4a13dd263a7b 230 myled1 = !myled1;
YusukeWakuta 0:b1290ca6c4a2 231 }
YusukeWakuta 15:c5ade23e5de4 232 if(can_L.read(recmsg_L)) {
YusukeWakuta 15:c5ade23e5de4 233 for(int i = 0; i < recmsg_L.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 234 yokutanDatas_L[i] = recmsg_L.data[i];
YusukeWakuta 15:c5ade23e5de4 235 led3 = !led3;
YusukeWakuta 15:c5ade23e5de4 236 // pc.printf("%c",yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 237 }
taurin 4:4a13dd263a7b 238 myled2 = !myled2;
YusukeWakuta 0:b1290ca6c4a2 239 }
YusukeWakuta 0:b1290ca6c4a2 240 }
YusukeWakuta 0:b1290ca6c4a2 241
YusukeWakuta 15:c5ade23e5de4 242 int main()
YusukeWakuta 15:c5ade23e5de4 243 {
YusukeWakuta 0:b1290ca6c4a2 244 init();
YusukeWakuta 15:c5ade23e5de4 245 while(1) {
tsumagari 8:dbc8c87dac78 246 InputControlValues();
tsumagari 10:b7159feb11fd 247 wait_us(5);
YusukeWakuta 0:b1290ca6c4a2 248 receiveDatas();
tsumagari 9:c32999b4d2f9 249 SendDatas();
YusukeWakuta 15:c5ade23e5de4 250 // pc.printf("%d",drug_R.read());
tsumagari 10:b7159feb11fd 251 // toString_R();
taurin 4:4a13dd263a7b 252 // toString_L();
YusukeWakuta 0:b1290ca6c4a2 253 wait(WAIT_LOOP_TIME);
YusukeWakuta 0:b1290ca6c4a2 254 }
YusukeWakuta 0:b1290ca6c4a2 255 }