Shinya Matsuyama / Mbed OS MF_FUJIKO_BASE

update from matsu

Dependencies:   libTCS34725

main.cpp@5:01ad87ee4f28, 2019-03-16 (annotated)

Committer:
shinyamatsuyama
Date:
Sat Mar 16 07:30:44 2019 +0000
Revision:
5:01ad87ee4f28
Parent:
4:e4cb72bba1c9
Child:
6:fd9d4fed9946
01_without_sensor

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