Shinya Matsuyama / Mbed OS MF_FUJIKO_BASE

update from matsu

Dependencies:   libTCS34725

main.cpp@6:fd9d4fed9946, 2019-03-16 (annotated)

Committer:
shinyamatsuyama
Date:
Sat Mar 16 15:42:24 2019 +0000
Revision:
6:fd9d4fed9946
Parent:
5:01ad87ee4f28 
091_sendfix; 11_udpread

Who changed what in which revision?

UserRevisionLine numberNew contents of line
poipoi 0:bed71d1853ef 1 #include "mbed.h"
poipoi 0:bed71d1853ef 2 #include "UDPManager.h"
poipoi 0:bed71d1853ef 3 #include "I2CMotorDriver.h"
poipoi 0:bed71d1853ef 4 #include "DoorController.h"
poipoi 0:bed71d1853ef 5
poipoi 0:bed71d1853ef 6 #include "TCS34725.h"
poipoi 0:bed71d1853ef 7 #include "ColorUtil.h"
poipoi 0:bed71d1853ef 8
shinyamatsuyama 3:c27e42a28010 9 // 011 送り出し
shinyamatsuyama 3:c27e42a28010 10 // 091 最後のエンディングの通過検知（スモールライトLED制御あり）
shinyamatsuyama 3:c27e42a28010 11 // 111 階段分岐のところ
shinyamatsuyama 3:c27e42a28010 12 // 131 前半の分岐のところ
shinyamatsuyama 3:c27e42a28010 13
poipoi 0:bed71d1853ef 14 // #define POSITION_011
poipoi 1:db5fca88ffb4 15 #define POSITION_091
poipoi 1:db5fca88ffb4 16 // #define POSITION_111
poipoi 0:bed71d1853ef 17 // #define POSITION_131
poipoi 0:bed71d1853ef 18
poipoi 0:bed71d1853ef 19
poipoi 0:bed71d1853ef 20 // ----- Serial -----
poipoi 0:bed71d1853ef 21 Serial serial(USBTX, USBRX);
poipoi 0:bed71d1853ef 22
poipoi 0:bed71d1853ef 23
poipoi 0:bed71d1853ef 24 // ----- GPIO -----
poipoi 0:bed71d1853ef 25 DigitalOut led1(LED1);
poipoi 0:bed71d1853ef 26 InterruptIn btn(USER_BUTTON);
poipoi 0:bed71d1853ef 27
poipoi 0:bed71d1853ef 28 #if defined(POSITION_091) || defined(POSITION_131)
poipoi 0:bed71d1853ef 29 InterruptIn sensor1 (D2);
poipoi 0:bed71d1853ef 30 #endif
poipoi 0:bed71d1853ef 31 #if defined(POSITION_091)
poipoi 0:bed71d1853ef 32 InterruptIn sensor2(D4);
poipoi 0:bed71d1853ef 33 InterruptIn sensor3(D6);
poipoi 0:bed71d1853ef 34 DigitalOut light(D8);
poipoi 0:bed71d1853ef 35 #endif
poipoi 0:bed71d1853ef 36 #if defined(POSITION_011)
poipoi 0:bed71d1853ef 37 DigitalIn input(D2);
poipoi 0:bed71d1853ef 38 #endif
poipoi 0:bed71d1853ef 39
poipoi 0:bed71d1853ef 40
poipoi 0:bed71d1853ef 41 // ----- Motor -----
poipoi 0:bed71d1853ef 42 #if defined(POSITION_011)
poipoi 0:bed71d1853ef 43 I2C i2c(SDA, SCL);
poipoi 0:bed71d1853ef 44 Mutex i2cLock;
poipoi 0:bed71d1853ef 45 I2CMotorDriver stepper(i2c, i2cLock, 10);
poipoi 0:bed71d1853ef 46 #endif
poipoi 0:bed71d1853ef 47 #if defined(POSITION_111) || defined(POSITION_131)
poipoi 0:bed71d1853ef 48 I2C i2c(SDA, SCL);
poipoi 0:bed71d1853ef 49 Mutex i2cLock;
poipoi 0:bed71d1853ef 50 I2CMotorDriver stepper1(i2c, i2cLock, 10);
poipoi 0:bed71d1853ef 51 I2CMotorDriver stepper2(i2c, i2cLock, 10);
poipoi 0:bed71d1853ef 52 DoorController door1(stepper1);
poipoi 0:bed71d1853ef 53 DoorController door2(stepper2);
poipoi 0:bed71d1853ef 54 #endif
poipoi 0:bed71d1853ef 55
poipoi 0:bed71d1853ef 56
poipoi 0:bed71d1853ef 57 // ----- Network -----
poipoi 0:bed71d1853ef 58 UDPManager udp;
poipoi 0:bed71d1853ef 59
poipoi 0:bed71d1853ef 60
poipoi 0:bed71d1853ef 61
poipoi 0:bed71d1853ef 62 // ========== POSITION_011 ==========
poipoi 0:bed71d1853ef 63 #ifdef POSITION_011
poipoi 0:bed71d1853ef 64
poipoi 0:bed71d1853ef 65 #define CHECK_INTERVAL (40.0)
poipoi 0:bed71d1853ef 66
poipoi 0:bed71d1853ef 67 TCS34725 colorSensor = TCS34725();
poipoi 0:bed71d1853ef 68 bool isON = false;
poipoi 0:bed71d1853ef 69
poipoi 0:bed71d1853ef 70 osThreadId mainThID;
poipoi 0:bed71d1853ef 71 Ticker tick;
poipoi 0:bed71d1853ef 72
poipoi 0:bed71d1853ef 73 bool isBtnON = false;
poipoi 0:bed71d1853ef 74
poipoi 0:bed71d1853ef 75 void onBtnON()
poipoi 0:bed71d1853ef 76 {
poipoi 0:bed71d1853ef 77 if (isBtnON) return;
poipoi 0:bed71d1853ef 78
poipoi 0:bed71d1853ef 79 isBtnON = true;
poipoi 0:bed71d1853ef 80 osSignalSet(mainThID, 0x01);
poipoi 0:bed71d1853ef 81 }
poipoi 0:bed71d1853ef 82
poipoi 0:bed71d1853ef 83 void onInterrupt()
poipoi 0:bed71d1853ef 84 {
poipoi 0:bed71d1853ef 85 osSignalSet(mainThID, 0x01);
poipoi 0:bed71d1853ef 86 }
poipoi 0:bed71d1853ef 87
poipoi 0:bed71d1853ef 88 int main()
poipoi 0:bed71d1853ef 89 {
poipoi 0:bed71d1853ef 90 mainThID = osThreadGetId();
poipoi 0:bed71d1853ef 91
poipoi 0:bed71d1853ef 92 serial.baud(9600);
poipoi 0:bed71d1853ef 93 serial.printf("=== Wakeup ===\n");
poipoi 0:bed71d1853ef 94
shinyamatsuyama 6:fd9d4fed9946 95 //tick.attach(&onInterrupt, CHECK_INTERVAL);
shinyamatsuyama 6:fd9d4fed9946 96 //btn.fall(&onBtnON);
poipoi 0:bed71d1853ef 97
poipoi 0:bed71d1853ef 98 colorSensor.init(TCS34725_INTEGRATIONTIME_101MS, TCS34725_GAIN_1X);
poipoi 0:bed71d1853ef 99
poipoi 0:bed71d1853ef 100 stepper.begin(0x0F);
poipoi 0:bed71d1853ef 101 stepper.free();
poipoi 0:bed71d1853ef 102
poipoi 0:bed71d1853ef 103 udp.init("192.168.0.11", "255.255.255.0", "192.168.0.1");
poipoi 0:bed71d1853ef 104 printf("IP is %s\n", udp.getIPAdr());
poipoi 0:bed71d1853ef 105
poipoi 0:bed71d1853ef 106 while (true) {
shinyamatsuyama 6:fd9d4fed9946 107 //osSignalWait(0x01, osWaitForever);
shinyamatsuyama 6:fd9d4fed9946 108 MessageID msgID;
shinyamatsuyama 6:fd9d4fed9946 109 int timestamp;
shinyamatsuyama 6:fd9d4fed9946 110 ColorID colorID;
shinyamatsuyama 6:fd9d4fed9946 111 CourseID courseID;
shinyamatsuyama 6:fd9d4fed9946 112 bool isOK = udp.receive(msgID, timestamp, colorID, courseID);
shinyamatsuyama 6:fd9d4fed9946 113 if (!isOK) continue;
shinyamatsuyama 6:fd9d4fed9946 114 printf("receive:%d, %d, %d\n", msgID);
shinyamatsuyama 6:fd9d4fed9946 115
shinyamatsuyama 6:fd9d4fed9946 116 if (msgID != MESSAGE_H) continue;
poipoi 0:bed71d1853ef 117
shinyamatsuyama 5:01ad87ee4f28 118 // センサーの反応がなくてもとにかく送り出すように変更
shinyamatsuyama 5:01ad87ee4f28 119 // if (input && !isBtnON) continue;
poipoi 0:bed71d1853ef 120
poipoi 0:bed71d1853ef 121 led1 = true;
poipoi 0:bed71d1853ef 122
poipoi 0:bed71d1853ef 123 uint16_t r = 0, g = 0, b = 0, c = 0;
poipoi 0:bed71d1853ef 124 colorSensor.getColor(r, g, b, c);
poipoi 0:bed71d1853ef 125 serial.printf("%d, %d, %d, %d, ", r, g, b, c);
poipoi 0:bed71d1853ef 126
poipoi 0:bed71d1853ef 127 ColorUtil::RGB rgb = { r, g, b, c };
poipoi 0:bed71d1853ef 128 ColorUtil::HSV hsv = ColorUtil::RGBtoHSV(rgb);
poipoi 0:bed71d1853ef 129 ColorUtil::BALL_COLOR col = ColorUtil::detectBallColor(hsv);
poipoi 0:bed71d1853ef 130
shinyamatsuyama 6:fd9d4fed9946 131 colorID = COLOR_NONE;
poipoi 0:bed71d1853ef 132 switch (col) {
poipoi 0:bed71d1853ef 133 case ColorUtil::BALL_RED:
poipoi 0:bed71d1853ef 134 colorID = COLOR_RED;
poipoi 0:bed71d1853ef 135 serial.printf("RED\n");
poipoi 0:bed71d1853ef 136 break;
poipoi 0:bed71d1853ef 137 case ColorUtil::BALL_BLUE:
poipoi 0:bed71d1853ef 138 colorID = COLOR_BLUE;
poipoi 0:bed71d1853ef 139 serial.printf("BLUE\n");
poipoi 0:bed71d1853ef 140 break;
poipoi 0:bed71d1853ef 141 case ColorUtil::BALL_GREEN:
poipoi 0:bed71d1853ef 142 colorID = COLOR_GREEN;
poipoi 0:bed71d1853ef 143 serial.printf("GREEN\n");
poipoi 0:bed71d1853ef 144 break;
poipoi 0:bed71d1853ef 145 case ColorUtil::BALL_YELLOW:
poipoi 0:bed71d1853ef 146 colorID = COLOR_YELLOW;
poipoi 0:bed71d1853ef 147 serial.printf("YELLOW\n");
poipoi 0:bed71d1853ef 148 break;
shinyamatsuyama 5:01ad87ee4f28 149 default:
shinyamatsuyama 5:01ad87ee4f28 150 // いろが該当しない場合はとりあえず赤にする
shinyamatsuyama 5:01ad87ee4f28 151 colorID = COLOR_RED;
shinyamatsuyama 5:01ad87ee4f28 152 serial.printf("COLOR_NONE\n");
shinyamatsuyama 5:01ad87ee4f28 153 break;
poipoi 0:bed71d1853ef 154 }
poipoi 0:bed71d1853ef 155 udp.sendA(colorID);
poipoi 0:bed71d1853ef 156 printf("send: %s\n", udp.getLatestMessage());
poipoi 0:bed71d1853ef 157
poipoi 0:bed71d1853ef 158 stepper.step(400, 1, 1);
poipoi 0:bed71d1853ef 159 Thread::wait(500);
poipoi 0:bed71d1853ef 160
poipoi 0:bed71d1853ef 161 led1 = false;
poipoi 0:bed71d1853ef 162 isBtnON = false;
poipoi 0:bed71d1853ef 163 }
poipoi 0:bed71d1853ef 164 }
poipoi 0:bed71d1853ef 165
poipoi 0:bed71d1853ef 166 #endif
poipoi 0:bed71d1853ef 167
poipoi 0:bed71d1853ef 168
poipoi 0:bed71d1853ef 169
poipoi 0:bed71d1853ef 170 // ========== POSITION_091 ==========
poipoi 0:bed71d1853ef 171 #ifdef POSITION_091
poipoi 0:bed71d1853ef 172
poipoi 0:bed71d1853ef 173 int sensorIndex = -1;
poipoi 0:bed71d1853ef 174 int colorIndex = -1; // ForDebug
poipoi 0:bed71d1853ef 175
poipoi 0:bed71d1853ef 176 enum INT_STATE {
poipoi 0:bed71d1853ef 177 INT_NONE = -1,
poipoi 0:bed71d1853ef 178 INT_SENSOR_1 = 0,
poipoi 0:bed71d1853ef 179 INT_SENSOR_2,
poipoi 0:bed71d1853ef 180 INT_SENSOR_3,
poipoi 0:bed71d1853ef 181 INT_SW,
poipoi 0:bed71d1853ef 182 };
poipoi 0:bed71d1853ef 183 INT_STATE intState = INT_NONE;
poipoi 0:bed71d1853ef 184
shinyamatsuyama 6:fd9d4fed9946 185 Timer myTimer[3];
shinyamatsuyama 6:fd9d4fed9946 186
shinyamatsuyama 6:fd9d4fed9946 187 bool sensorFlag[3];
shinyamatsuyama 6:fd9d4fed9946 188
poipoi 0:bed71d1853ef 189 void onSensorON()
poipoi 0:bed71d1853ef 190 {
poipoi 0:bed71d1853ef 191 if (intState != INT_NONE) return;
poipoi 0:bed71d1853ef 192
poipoi 0:bed71d1853ef 193 printf("SW");
poipoi 0:bed71d1853ef 194 intState = INT_SW;
poipoi 0:bed71d1853ef 195 led1 = true;
poipoi 0:bed71d1853ef 196 }
poipoi 0:bed71d1853ef 197
shinyamatsuyama 6:fd9d4fed9946 198 void intSensor(int idx);
shinyamatsuyama 6:fd9d4fed9946 199
poipoi 0:bed71d1853ef 200 void onSensor1ON() {
shinyamatsuyama 6:fd9d4fed9946 201 intSensor(INT_SENSOR_1);
poipoi 0:bed71d1853ef 202 }
poipoi 0:bed71d1853ef 203
poipoi 0:bed71d1853ef 204 void onSensor2ON() {
shinyamatsuyama 6:fd9d4fed9946 205 intSensor(INT_SENSOR_2);
poipoi 0:bed71d1853ef 206 }
poipoi 0:bed71d1853ef 207
poipoi 0:bed71d1853ef 208 void onSensor3ON() {
shinyamatsuyama 6:fd9d4fed9946 209 intSensor(INT_SENSOR_3);
poipoi 0:bed71d1853ef 210 }
poipoi 0:bed71d1853ef 211
poipoi 0:bed71d1853ef 212 ColorID nowColor[] = {
poipoi 0:bed71d1853ef 213 COLOR_RED,
poipoi 0:bed71d1853ef 214 COLOR_BLUE,
poipoi 0:bed71d1853ef 215 COLOR_GREEN,
poipoi 0:bed71d1853ef 216 }; // random default color
poipoi 0:bed71d1853ef 217
poipoi 0:bed71d1853ef 218
poipoi 0:bed71d1853ef 219 Timeout lightTimer;
shinyamatsuyama 3:c27e42a28010 220
shinyamatsuyama 4:e4cb72bba1c9 221 #define LIGHT_TIME_BEFORE_ON (4.0)
poipoi 1:db5fca88ffb4 222 #define LIGHT_TIME_WHILE_ON (3.0)
poipoi 0:bed71d1853ef 223
poipoi 0:bed71d1853ef 224 void lightTimerDone() {
poipoi 0:bed71d1853ef 225 light = false;
poipoi 0:bed71d1853ef 226 }
poipoi 0:bed71d1853ef 227
poipoi 0:bed71d1853ef 228 void lightTurnON() {
poipoi 0:bed71d1853ef 229 light = true;
poipoi 1:db5fca88ffb4 230 lightTimer.attach(&lightTimerDone, LIGHT_TIME_WHILE_ON);
poipoi 0:bed71d1853ef 231 }
poipoi 0:bed71d1853ef 232
shinyamatsuyama 6:fd9d4fed9946 233 void sendMessageD(int state) {
poipoi 0:bed71d1853ef 234 ColorID colorID = COLOR_NONE;
shinyamatsuyama 6:fd9d4fed9946 235 switch (state) {
poipoi 0:bed71d1853ef 236 case INT_NONE: // Error
poipoi 0:bed71d1853ef 237 return;
poipoi 0:bed71d1853ef 238
poipoi 0:bed71d1853ef 239 case INT_SW: // ForDebug
poipoi 0:bed71d1853ef 240 colorID = (ColorID)colorIndex;
poipoi 0:bed71d1853ef 241 if (++sensorIndex >= 3) sensorIndex = 0;
poipoi 0:bed71d1853ef 242 if (++colorIndex >= 4) colorIndex = 0;
poipoi 0:bed71d1853ef 243 break;
poipoi 0:bed71d1853ef 244
poipoi 0:bed71d1853ef 245 default:
shinyamatsuyama 6:fd9d4fed9946 246 sensorIndex = state;
poipoi 0:bed71d1853ef 247 colorID = nowColor[sensorIndex];
poipoi 0:bed71d1853ef 248 // nowColor[sensorIndex] = COLOR_NONE; // keep color for dummy!
poipoi 0:bed71d1853ef 249 }
poipoi 0:bed71d1853ef 250
poipoi 0:bed71d1853ef 251 CourseID courseID = COURSE_NONE;
poipoi 0:bed71d1853ef 252 switch (sensorIndex) {
poipoi 0:bed71d1853ef 253 case 0:
poipoi 0:bed71d1853ef 254 courseID = COURSE_TOP;
poipoi 0:bed71d1853ef 255 break;
poipoi 0:bed71d1853ef 256 case 1:
poipoi 0:bed71d1853ef 257 courseID = COURSE_MIDDLE;
poipoi 0:bed71d1853ef 258 break;
poipoi 0:bed71d1853ef 259 case 2:
poipoi 0:bed71d1853ef 260 courseID = COURSE_BOTTOM;
poipoi 0:bed71d1853ef 261 break;
poipoi 0:bed71d1853ef 262 }
poipoi 0:bed71d1853ef 263
poipoi 0:bed71d1853ef 264 printf("send: %i, ", sensorIndex);
poipoi 0:bed71d1853ef 265 if (colorID != COLOR_NONE) {
poipoi 0:bed71d1853ef 266 udp.sendD(colorID, courseID);
poipoi 0:bed71d1853ef 267 printf("%s\n", udp.getLatestMessage());
poipoi 0:bed71d1853ef 268 } else {
poipoi 0:bed71d1853ef 269 printf("\n");
poipoi 0:bed71d1853ef 270 }
poipoi 0:bed71d1853ef 271
poipoi 0:bed71d1853ef 272 if ((courseID == COURSE_MIDDLE) && (colorID == COLOR_YELLOW)) {
poipoi 1:db5fca88ffb4 273 lightTimer.attach(&lightTurnON, LIGHT_TIME_BEFORE_ON);
poipoi 0:bed71d1853ef 274 }
poipoi 0:bed71d1853ef 275 }
poipoi 0:bed71d1853ef 276
shinyamatsuyama 6:fd9d4fed9946 277 void intSensor(int idx) {
shinyamatsuyama 6:fd9d4fed9946 278 if(idx<INT_SENSOR_1 || idx>INT_SENSOR_3)
shinyamatsuyama 6:fd9d4fed9946 279 return;
shinyamatsuyama 6:fd9d4fed9946 280
shinyamatsuyama 6:fd9d4fed9946 281 int dtime = myTimer[idx].read_ms();
shinyamatsuyama 6:fd9d4fed9946 282 if(dtime > 500) {
shinyamatsuyama 6:fd9d4fed9946 283 printf("sensor%d", idx+1);
shinyamatsuyama 6:fd9d4fed9946 284 //intState = INT_SENSOR_1;
shinyamatsuyama 6:fd9d4fed9946 285 //led1 = true;
shinyamatsuyama 6:fd9d4fed9946 286 sensorFlag[idx] = true;
shinyamatsuyama 6:fd9d4fed9946 287 myTimer[idx].reset();
shinyamatsuyama 6:fd9d4fed9946 288 }
shinyamatsuyama 6:fd9d4fed9946 289 }
poipoi 0:bed71d1853ef 290
poipoi 0:bed71d1853ef 291 int main()
poipoi 0:bed71d1853ef 292 {
shinyamatsuyama 6:fd9d4fed9946 293 for(int i=0;i<3;i++) {
shinyamatsuyama 6:fd9d4fed9946 294 myTimer[i].start();
shinyamatsuyama 6:fd9d4fed9946 295 sensorFlag[i] = false;
shinyamatsuyama 6:fd9d4fed9946 296 }
shinyamatsuyama 6:fd9d4fed9946 297
poipoi 0:bed71d1853ef 298 serial.baud(9600);
poipoi 0:bed71d1853ef 299 serial.printf("=== Wakeup (D)===\n");
poipoi 0:bed71d1853ef 300
poipoi 0:bed71d1853ef 301 light = false;
poipoi 0:bed71d1853ef 302
poipoi 0:bed71d1853ef 303 btn.fall(&onSensorON);
poipoi 0:bed71d1853ef 304 sensor1.mode(PullUp);
poipoi 0:bed71d1853ef 305 sensor1.fall(&onSensor1ON);
poipoi 0:bed71d1853ef 306 sensor2.mode(PullUp);
poipoi 0:bed71d1853ef 307 sensor2.fall(&onSensor2ON);
poipoi 0:bed71d1853ef 308 sensor3.mode(PullUp);
poipoi 0:bed71d1853ef 309 sensor3.fall(&onSensor3ON);
poipoi 0:bed71d1853ef 310
poipoi 0:bed71d1853ef 311 udp.init("192.168.0.91", "255.255.255.0", "192.168.0.1");
poipoi 0:bed71d1853ef 312 printf("IP is %s\n", udp.getIPAdr());
poipoi 0:bed71d1853ef 313
poipoi 0:bed71d1853ef 314 while (true) {
poipoi 0:bed71d1853ef 315 if (intState != INT_NONE) {
shinyamatsuyama 6:fd9d4fed9946 316 sendMessageD(intState);
poipoi 0:bed71d1853ef 317 wait_ms(500);
poipoi 0:bed71d1853ef 318 intState = INT_NONE;
poipoi 0:bed71d1853ef 319 led1 = false;
poipoi 0:bed71d1853ef 320 }
shinyamatsuyama 6:fd9d4fed9946 321 for(int i=0;i<3;i++) {
shinyamatsuyama 6:fd9d4fed9946 322 if(sensorFlag[i]) {
shinyamatsuyama 6:fd9d4fed9946 323 sensorFlag[i] = false;
shinyamatsuyama 6:fd9d4fed9946 324 sendMessageD(INT_SENSOR_1+i);
shinyamatsuyama 6:fd9d4fed9946 325 }
shinyamatsuyama 6:fd9d4fed9946 326 }
poipoi 0:bed71d1853ef 327 MessageID msgID;
poipoi 0:bed71d1853ef 328 int timestamp;
poipoi 0:bed71d1853ef 329 ColorID colorID;
poipoi 0:bed71d1853ef 330 CourseID courseID;
poipoi 0:bed71d1853ef 331
poipoi 0:bed71d1853ef 332 bool isOK = udp.receive(msgID, timestamp, colorID, courseID);
poipoi 0:bed71d1853ef 333 if (!isOK) continue;
poipoi 0:bed71d1853ef 334 printf("receive:%d, %d, %d\n", msgID, colorID, courseID);
poipoi 0:bed71d1853ef 335
poipoi 0:bed71d1853ef 336 if (msgID != MESSAGE_C) continue;
poipoi 0:bed71d1853ef 337
poipoi 0:bed71d1853ef 338 switch (courseID) {
poipoi 0:bed71d1853ef 339 case COURSE_TOP:
poipoi 0:bed71d1853ef 340 nowColor[0] = colorID;
poipoi 0:bed71d1853ef 341 break;
poipoi 0:bed71d1853ef 342 case COURSE_MIDDLE:
poipoi 0:bed71d1853ef 343 nowColor[1] = colorID;
poipoi 0:bed71d1853ef 344 break;
poipoi 0:bed71d1853ef 345 case COURSE_BOTTOM:
poipoi 0:bed71d1853ef 346 nowColor[2] = colorID;
poipoi 0:bed71d1853ef 347 break;
poipoi 0:bed71d1853ef 348 default:
poipoi 0:bed71d1853ef 349 continue;
poipoi 0:bed71d1853ef 350 }
poipoi 0:bed71d1853ef 351 }
poipoi 0:bed71d1853ef 352 }
poipoi 0:bed71d1853ef 353 #endif
poipoi 0:bed71d1853ef 354
poipoi 0:bed71d1853ef 355
poipoi 0:bed71d1853ef 356
poipoi 0:bed71d1853ef 357 // ========== POSITION_111 ==========
poipoi 0:bed71d1853ef 358 #ifdef POSITION_111
poipoi 0:bed71d1853ef 359
poipoi 0:bed71d1853ef 360 bool isON = false;
poipoi 0:bed71d1853ef 361 int doorState = 0; // ForDebug
poipoi 0:bed71d1853ef 362 void onSW()
poipoi 0:bed71d1853ef 363 {
poipoi 0:bed71d1853ef 364 if (isON) return;
poipoi 0:bed71d1853ef 365
poipoi 0:bed71d1853ef 366 isON = true;
poipoi 0:bed71d1853ef 367 led1 = true;
poipoi 0:bed71d1853ef 368 }
poipoi 0:bed71d1853ef 369
poipoi 0:bed71d1853ef 370 void main() {
poipoi 0:bed71d1853ef 371 serial.baud(9600);
poipoi 0:bed71d1853ef 372 serial.printf("=== Wakeup ===\n");
poipoi 0:bed71d1853ef 373
poipoi 0:bed71d1853ef 374 btn.fall(&onSW);
poipoi 0:bed71d1853ef 375
poipoi 0:bed71d1853ef 376 i2c.frequency(100000);
poipoi 0:bed71d1853ef 377
poipoi 0:bed71d1853ef 378 stepper1.begin(0x0F);
poipoi 0:bed71d1853ef 379 stepper1.free();
poipoi 0:bed71d1853ef 380
poipoi 0:bed71d1853ef 381 stepper2.begin(0x0E);
poipoi 0:bed71d1853ef 382 stepper2.free();
poipoi 0:bed71d1853ef 383
poipoi 0:bed71d1853ef 384 udp.init("192.168.0.111", "255.255.255.0", "192.168.0.1");
poipoi 0:bed71d1853ef 385 printf("IP is %s\n", udp.getIPAdr());
poipoi 0:bed71d1853ef 386
poipoi 0:bed71d1853ef 387 door1.forceClose();
poipoi 0:bed71d1853ef 388 door2.forceClose();
poipoi 0:bed71d1853ef 389
poipoi 0:bed71d1853ef 390 while (true) {
poipoi 0:bed71d1853ef 391 if (isON) {
poipoi 0:bed71d1853ef 392 printf("SW\n");
poipoi 0:bed71d1853ef 393 switch (doorState) {
poipoi 0:bed71d1853ef 394 case 0:
poipoi 0:bed71d1853ef 395 door1.close();
poipoi 0:bed71d1853ef 396 door2.close();
poipoi 0:bed71d1853ef 397 break;
poipoi 0:bed71d1853ef 398 case 1:
poipoi 0:bed71d1853ef 399 door1.close();
poipoi 0:bed71d1853ef 400 door2.open();
poipoi 0:bed71d1853ef 401 break;
poipoi 0:bed71d1853ef 402 case 2:
poipoi 0:bed71d1853ef 403 door1.open();
poipoi 0:bed71d1853ef 404 // door2.close();
poipoi 0:bed71d1853ef 405 }
poipoi 0:bed71d1853ef 406 if (++doorState >= 3) doorState = 0;
poipoi 0:bed71d1853ef 407 Thread::wait(500);
poipoi 0:bed71d1853ef 408
poipoi 0:bed71d1853ef 409 isON = false;
poipoi 0:bed71d1853ef 410 led1 = false;
poipoi 0:bed71d1853ef 411 }
poipoi 0:bed71d1853ef 412
poipoi 0:bed71d1853ef 413 MessageID msgID;
poipoi 0:bed71d1853ef 414 int timestamp;
poipoi 0:bed71d1853ef 415 ColorID colorID;
poipoi 0:bed71d1853ef 416 CourseID courseID;
poipoi 0:bed71d1853ef 417
poipoi 0:bed71d1853ef 418 bool isOK = udp.receive(msgID, timestamp, colorID, courseID);
poipoi 0:bed71d1853ef 419 if (!isOK) continue;
poipoi 0:bed71d1853ef 420 printf("receive:%d, %d, %d\n", msgID, colorID, courseID);
poipoi 0:bed71d1853ef 421
poipoi 0:bed71d1853ef 422 if (msgID != MESSAGE_C) continue;
poipoi 0:bed71d1853ef 423
poipoi 0:bed71d1853ef 424 switch (courseID) {
poipoi 0:bed71d1853ef 425 case COURSE_TOP:
poipoi 0:bed71d1853ef 426 printf("Course Top Open\n");
poipoi 0:bed71d1853ef 427 door1.close();
poipoi 0:bed71d1853ef 428 door2.close();
poipoi 0:bed71d1853ef 429 break;
poipoi 0:bed71d1853ef 430 case COURSE_MIDDLE:
poipoi 0:bed71d1853ef 431 printf("Course Middle Open\n");
poipoi 0:bed71d1853ef 432 door1.close();
poipoi 0:bed71d1853ef 433 door2.open();
poipoi 0:bed71d1853ef 434 break;
poipoi 0:bed71d1853ef 435 case COURSE_BOTTOM:
poipoi 0:bed71d1853ef 436 printf("Course Bottom Open\n");
poipoi 0:bed71d1853ef 437 door1.open();
poipoi 0:bed71d1853ef 438 // door2.close();
poipoi 0:bed71d1853ef 439 break;
poipoi 0:bed71d1853ef 440 default:
poipoi 0:bed71d1853ef 441 continue;
poipoi 0:bed71d1853ef 442 }
poipoi 0:bed71d1853ef 443 }
poipoi 0:bed71d1853ef 444 }
poipoi 0:bed71d1853ef 445
poipoi 0:bed71d1853ef 446 #endif
poipoi 0:bed71d1853ef 447
poipoi 0:bed71d1853ef 448
poipoi 0:bed71d1853ef 449 // ========== POSITION_131 ==========
poipoi 0:bed71d1853ef 450 #ifdef POSITION_131
poipoi 0:bed71d1853ef 451
poipoi 0:bed71d1853ef 452 /*
poipoi 0:bed71d1853ef 453 ５連 COURSE_LEFT
poipoi 0:bed71d1853ef 454 0: タイムマシンシリーズ
poipoi 0:bed71d1853ef 455 1: たまごシリーズ
poipoi 0:bed71d1853ef 456 2: モンガーテレポート
poipoi 0:bed71d1853ef 457
poipoi 0:bed71d1853ef 458 2連 COURSE_CENTER
poipoi 0:bed71d1853ef 459 0: オバケの壁抜け
poipoi 0:bed71d1853ef 460 1: 通り抜けフープ
poipoi 0:bed71d1853ef 461 2: 天狗の抜け穴
poipoi 0:bed71d1853ef 462
poipoi 0:bed71d1853ef 463 縦大型 COURSE_RIGHT
poipoi 0:bed71d1853ef 464 0: 火星人(黄)
poipoi 0:bed71d1853ef 465 1: 火星人(緑)
poipoi 0:bed71d1853ef 466 2: ティラノサウルス(青)
poipoi 0:bed71d1853ef 467 3: ティラノサウルス(黄)
poipoi 0:bed71d1853ef 468 4: マンモス（緑)
poipoi 0:bed71d1853ef 469 5: マンモス(赤)
poipoi 0:bed71d1853ef 470 */
poipoi 0:bed71d1853ef 471
poipoi 0:bed71d1853ef 472 struct ContentsSet {
poipoi 0:bed71d1853ef 473 uint8_t contentNum;
poipoi 0:bed71d1853ef 474 uint8_t contents[2];
poipoi 0:bed71d1853ef 475 };
poipoi 0:bed71d1853ef 476
poipoi 0:bed71d1853ef 477 ContentsSet B_L_contents = { 1, { 2, } };
poipoi 0:bed71d1853ef 478 ContentsSet Y_L_contents = { 2, { 0, 3 } };
poipoi 0:bed71d1853ef 479 ContentsSet R_L_contents = { 1, { 5, } };
poipoi 0:bed71d1853ef 480 ContentsSet G_L_contents = { 2, { 1, 4 } };
poipoi 0:bed71d1853ef 481 ContentsSet B_C_contents = { 2, { 1, 0 } };
poipoi 0:bed71d1853ef 482 ContentsSet Y_C_contents = { 1, { 2, } };
poipoi 0:bed71d1853ef 483 ContentsSet R_C_contents = { 2, { 1, 2 } };
poipoi 0:bed71d1853ef 484 ContentsSet G_C_contents = { 1, { 0, } };
poipoi 0:bed71d1853ef 485 ContentsSet B_R_contents = { 1, { 1, } };
poipoi 0:bed71d1853ef 486 ContentsSet Y_R_contents = { 2, { 1, 0 } };
poipoi 0:bed71d1853ef 487 ContentsSet R_R_contents = { 1, { 2, } };
poipoi 0:bed71d1853ef 488 ContentsSet G_R_contents = { 2, { 2, 0 } };
poipoi 0:bed71d1853ef 489
poipoi 0:bed71d1853ef 490 ContentsSet contentsList[][3] = {
poipoi 0:bed71d1853ef 491 {
poipoi 0:bed71d1853ef 492 R_L_contents,
poipoi 0:bed71d1853ef 493 R_C_contents,
poipoi 0:bed71d1853ef 494 R_R_contents,
poipoi 0:bed71d1853ef 495 },
poipoi 0:bed71d1853ef 496 {
poipoi 0:bed71d1853ef 497 B_L_contents,
poipoi 0:bed71d1853ef 498 B_C_contents,
poipoi 0:bed71d1853ef 499 B_R_contents,
poipoi 0:bed71d1853ef 500 },
poipoi 0:bed71d1853ef 501 {
poipoi 0:bed71d1853ef 502 G_L_contents,
poipoi 0:bed71d1853ef 503 G_C_contents,
poipoi 0:bed71d1853ef 504 G_R_contents,
poipoi 0:bed71d1853ef 505 },
poipoi 0:bed71d1853ef 506 {
poipoi 0:bed71d1853ef 507 Y_L_contents,
poipoi 0:bed71d1853ef 508 Y_C_contents,
poipoi 0:bed71d1853ef 509 Y_R_contents,
poipoi 0:bed71d1853ef 510 },
poipoi 0:bed71d1853ef 511 };
poipoi 0:bed71d1853ef 512
poipoi 0:bed71d1853ef 513 bool isON = false;
poipoi 0:bed71d1853ef 514 bool isSensorON = false;
poipoi 0:bed71d1853ef 515 int doorState = 0; // ForDebug
poipoi 0:bed71d1853ef 516
poipoi 0:bed71d1853ef 517 #include <queue>
poipoi 0:bed71d1853ef 518 queue<ColorID> colorQueue;
poipoi 0:bed71d1853ef 519 CourseID courseID = COURSE_LEFT;
poipoi 0:bed71d1853ef 520 uint32_t contentsCounter = 0;
poipoi 0:bed71d1853ef 521
poipoi 0:bed71d1853ef 522 void onSW()
poipoi 0:bed71d1853ef 523 {
poipoi 0:bed71d1853ef 524 if (isON) return;
poipoi 0:bed71d1853ef 525
poipoi 0:bed71d1853ef 526 isON = true;
poipoi 0:bed71d1853ef 527 led1 = true;
poipoi 0:bed71d1853ef 528 }
poipoi 0:bed71d1853ef 529
poipoi 0:bed71d1853ef 530 void onSensorON()
poipoi 0:bed71d1853ef 531 {
poipoi 0:bed71d1853ef 532 if (isSensorON) return;
poipoi 0:bed71d1853ef 533
poipoi 0:bed71d1853ef 534 isSensorON = true;
poipoi 0:bed71d1853ef 535 led1 = true;
poipoi 0:bed71d1853ef 536 }
poipoi 0:bed71d1853ef 537
poipoi 0:bed71d1853ef 538 void main() {
poipoi 0:bed71d1853ef 539 serial.baud(9600);
poipoi 0:bed71d1853ef 540 serial.printf("=== Wakeup ===\n");
poipoi 0:bed71d1853ef 541
poipoi 0:bed71d1853ef 542 btn.fall(&onSW);
poipoi 0:bed71d1853ef 543 sensor1.fall(&onSensorON);
poipoi 0:bed71d1853ef 544
poipoi 0:bed71d1853ef 545 i2c.frequency(100000);
poipoi 0:bed71d1853ef 546
poipoi 0:bed71d1853ef 547 stepper1.begin(0x0F);
poipoi 0:bed71d1853ef 548 stepper1.free();
poipoi 0:bed71d1853ef 549
poipoi 0:bed71d1853ef 550 stepper2.begin(0x0E);
poipoi 0:bed71d1853ef 551 stepper2.free();
poipoi 0:bed71d1853ef 552
poipoi 0:bed71d1853ef 553 udp.init("192.168.0.131", "255.255.255.0", "192.168.0.1");
poipoi 0:bed71d1853ef 554 printf("IP is %s\n", udp.getIPAdr());
poipoi 0:bed71d1853ef 555
poipoi 0:bed71d1853ef 556 door1.forceClose();
poipoi 0:bed71d1853ef 557 door2.forceClose();
poipoi 0:bed71d1853ef 558
poipoi 0:bed71d1853ef 559 while (true) {
poipoi 0:bed71d1853ef 560 if (isON) {
poipoi 0:bed71d1853ef 561 printf("SW\n");
poipoi 0:bed71d1853ef 562 switch (doorState) {
poipoi 0:bed71d1853ef 563 case 0:
poipoi 0:bed71d1853ef 564 door1.close();
poipoi 0:bed71d1853ef 565 door2.close();
poipoi 0:bed71d1853ef 566 break;
poipoi 0:bed71d1853ef 567 case 1:
poipoi 0:bed71d1853ef 568 door1.close();
poipoi 0:bed71d1853ef 569 door2.open();
poipoi 0:bed71d1853ef 570 break;
poipoi 0:bed71d1853ef 571 case 2:
poipoi 0:bed71d1853ef 572 door1.open();
poipoi 0:bed71d1853ef 573 door2.close();
poipoi 0:bed71d1853ef 574 }
poipoi 0:bed71d1853ef 575 if (++doorState >= 3) doorState = 0;
poipoi 0:bed71d1853ef 576 Thread::wait(500);
poipoi 0:bed71d1853ef 577
poipoi 0:bed71d1853ef 578 isON = false;
poipoi 0:bed71d1853ef 579 led1 = false;
poipoi 0:bed71d1853ef 580 }
poipoi 0:bed71d1853ef 581
poipoi 0:bed71d1853ef 582 if (isSensorON) {
poipoi 0:bed71d1853ef 583 printf("SensorON\n");
poipoi 0:bed71d1853ef 584
poipoi 0:bed71d1853ef 585 if (!colorQueue.empty()) {
poipoi 0:bed71d1853ef 586 ColorID colorID = colorQueue.front();
poipoi 0:bed71d1853ef 587 colorQueue.pop();
poipoi 0:bed71d1853ef 588
poipoi 0:bed71d1853ef 589 ContentsSet set = contentsList[colorID][courseID];
poipoi 0:bed71d1853ef 590 int contentID = set.contents[contentsCounter % set.contentNum];
poipoi 0:bed71d1853ef 591 contentsCounter++;
poipoi 0:bed71d1853ef 592 printf("SendB(%d, %d, %d)\n", colorID, courseID, contentID);
poipoi 0:bed71d1853ef 593
poipoi 0:bed71d1853ef 594 udp.sendB(colorID, courseID, contentID);
poipoi 0:bed71d1853ef 595
poipoi 0:bed71d1853ef 596 switch (courseID) {
poipoi 0:bed71d1853ef 597 case COURSE_LEFT:
poipoi 0:bed71d1853ef 598 printf("Course Left Open\n");
poipoi 0:bed71d1853ef 599 door1.close();
poipoi 0:bed71d1853ef 600 door2.close();
poipoi 0:bed71d1853ef 601 break;
poipoi 0:bed71d1853ef 602 case COURSE_CENTER:
poipoi 0:bed71d1853ef 603 printf("Course Center Open\n");
poipoi 0:bed71d1853ef 604 door1.close();
poipoi 0:bed71d1853ef 605 door2.open();
poipoi 0:bed71d1853ef 606 break;
poipoi 0:bed71d1853ef 607 case COURSE_RIGHT:
poipoi 0:bed71d1853ef 608 printf("Course Right Open\n");
poipoi 0:bed71d1853ef 609 door1.open();
poipoi 0:bed71d1853ef 610 door2.close();
poipoi 0:bed71d1853ef 611 break;
poipoi 0:bed71d1853ef 612 }
poipoi 0:bed71d1853ef 613
poipoi 0:bed71d1853ef 614 courseID = (CourseID)((int)courseID + 1);
poipoi 0:bed71d1853ef 615 if (courseID > COURSE_RIGHT) courseID = COURSE_LEFT;
poipoi 0:bed71d1853ef 616 }
poipoi 0:bed71d1853ef 617
poipoi 0:bed71d1853ef 618 wait(3);
poipoi 0:bed71d1853ef 619
poipoi 0:bed71d1853ef 620 isSensorON = false;
poipoi 0:bed71d1853ef 621 led1 = false;
poipoi 0:bed71d1853ef 622 }
poipoi 0:bed71d1853ef 623
poipoi 0:bed71d1853ef 624 MessageID msgID;
poipoi 0:bed71d1853ef 625 int timestamp;
poipoi 0:bed71d1853ef 626 ColorID colorID;
poipoi 0:bed71d1853ef 627 CourseID dCourseID; // Dummy
poipoi 0:bed71d1853ef 628
poipoi 0:bed71d1853ef 629 bool isOK = udp.receive(msgID, timestamp, colorID, dCourseID);
poipoi 0:bed71d1853ef 630 if (!isOK) continue;
poipoi 0:bed71d1853ef 631
poipoi 0:bed71d1853ef 632 printf("receive:%d, %d, %d\n", msgID, colorID, dCourseID);
poipoi 0:bed71d1853ef 633
poipoi 0:bed71d1853ef 634 if (msgID != MESSAGE_A) continue;
poipoi 0:bed71d1853ef 635
poipoi 0:bed71d1853ef 636 colorQueue.push(colorID);
poipoi 0:bed71d1853ef 637 }
poipoi 0:bed71d1853ef 638 }
poipoi 0:bed71d1853ef 639
poipoi 0:bed71d1853ef 640 #endif