File content as of revision 5:01ad87ee4f28:

#include "mbed.h"
#include "UDPManager.h"
#include "I2CMotorDriver.h"
#include "DoorController.h"

#include "TCS34725.h"
#include "ColorUtil.h"

// 011 送り出し
// 091 最後のエンディングの通過検知（スモールライトLED制御あり）
// 111 階段分岐のところ
// 131 前半の分岐のところ

// #define POSITION_011
#define POSITION_091
// #define POSITION_111
// #define POSITION_131


// ----- Serial -----
Serial serial(USBTX, USBRX);


// ----- GPIO -----
DigitalOut led1(LED1);
InterruptIn btn(USER_BUTTON);

#if defined(POSITION_091) || defined(POSITION_131)
InterruptIn sensor1 (D2);
#endif
#if defined(POSITION_091)
InterruptIn sensor2(D4);
InterruptIn sensor3(D6);
DigitalOut light(D8);
#endif
#if defined(POSITION_011)
DigitalIn input(D2);
#endif


// ----- Motor -----
#if defined(POSITION_011)
I2C i2c(SDA, SCL);
Mutex i2cLock;
I2CMotorDriver stepper(i2c, i2cLock, 10);
#endif
#if defined(POSITION_111) || defined(POSITION_131)
I2C i2c(SDA, SCL);
Mutex i2cLock;
I2CMotorDriver stepper1(i2c, i2cLock, 10);
I2CMotorDriver stepper2(i2c, i2cLock, 10);
DoorController door1(stepper1);
DoorController door2(stepper2);
#endif


// ----- Network -----
UDPManager udp;



// ========== POSITION_011 ==========
#ifdef POSITION_011

#define CHECK_INTERVAL      (40.0)

TCS34725 colorSensor = TCS34725();
bool isON = false;

osThreadId mainThID;
Ticker tick;

bool isBtnON = false;

void onBtnON()
{
    if (isBtnON) return;
    
    isBtnON = true;
    osSignalSet(mainThID, 0x01);
}

void onInterrupt()
{
    osSignalSet(mainThID, 0x01);
}

int main()
{
    mainThID = osThreadGetId();

    serial.baud(9600);
    serial.printf("=== Wakeup ===\n");
    
    tick.attach(&onInterrupt, CHECK_INTERVAL);
    btn.fall(&onBtnON);
    
    colorSensor.init(TCS34725_INTEGRATIONTIME_101MS, TCS34725_GAIN_1X);

    stepper.begin(0x0F);
    stepper.free();   
        
    udp.init("192.168.0.11", "255.255.255.0", "192.168.0.1");
    printf("IP is %s\n", udp.getIPAdr());

    while (true) {
        osSignalWait(0x01, osWaitForever);
        
        //  センサーの反応がなくてもとにかく送り出すように変更
        // if (input && !isBtnON) continue;
        
        led1 = true;

        uint16_t r = 0, g = 0, b = 0, c = 0;
        colorSensor.getColor(r, g, b, c);
        serial.printf("%d, %d, %d, %d, ", r, g, b, c);
        
        ColorUtil::RGB rgb = { r, g, b, c };
        ColorUtil::HSV hsv = ColorUtil::RGBtoHSV(rgb);
        ColorUtil::BALL_COLOR col = ColorUtil::detectBallColor(hsv);

        ColorID colorID = COLOR_NONE;        
        switch (col) {
            case ColorUtil::BALL_RED:
                colorID = COLOR_RED;
                serial.printf("RED\n");
                break;
            case ColorUtil::BALL_BLUE:
                colorID = COLOR_BLUE;
                serial.printf("BLUE\n");
                break;
            case ColorUtil::BALL_GREEN:
                colorID = COLOR_GREEN;
                serial.printf("GREEN\n");
                break;
            case ColorUtil::BALL_YELLOW:
                colorID = COLOR_YELLOW;
                serial.printf("YELLOW\n");
                break;
            default:
                //  いろが該当しない場合はとりあえず赤にする
                colorID = COLOR_RED;
                serial.printf("COLOR_NONE\n");
                break;
        }        
        udp.sendA(colorID);
        printf("send: %s\n", udp.getLatestMessage());
        
        stepper.step(400, 1, 1);
        Thread::wait(500);

        led1 = false;
        isBtnON = false;        
    }
}

#endif



// ========== POSITION_091 ==========
#ifdef POSITION_091

int sensorIndex = -1;
int colorIndex = -1;     // ForDebug

enum INT_STATE {
    INT_NONE = -1,
    INT_SENSOR_1 = 0,
    INT_SENSOR_2,
    INT_SENSOR_3,
    INT_SW,
};
INT_STATE intState = INT_NONE;

void onSensorON()
{
    if (intState != INT_NONE) return;
    
    printf("SW");
    intState = INT_SW;
    led1 = true;
}

void onSensor1ON() {
    if (intState != INT_NONE) return;
    
    printf("sensor1");
    intState = INT_SENSOR_1;
    led1 = true;
}

void onSensor2ON() {
    if (intState != INT_NONE) return;
    
    printf("sensor2");
    intState = INT_SENSOR_2;
    led1 = true;    
}

void onSensor3ON() {
    if (intState != INT_NONE) return;
    
    printf("sensor3");
    intState = INT_SENSOR_3;
    led1 = true;    
}

ColorID nowColor[] = {
    COLOR_RED,
    COLOR_BLUE,
    COLOR_GREEN,
};  // random default color


Timeout lightTimer;

#define LIGHT_TIME_BEFORE_ON    (4.0)
#define LIGHT_TIME_WHILE_ON     (3.0)

void lightTimerDone() {
    light = false;
}

void lightTurnON() {
    light = true;
    lightTimer.attach(&lightTimerDone, LIGHT_TIME_WHILE_ON);
}


void sendMessageD() {
    ColorID colorID = COLOR_NONE;
    switch (intState) {
        case INT_NONE:  // Error
            return;
            
        case INT_SW:    // ForDebug
            colorID = (ColorID)colorIndex;
            if (++sensorIndex >= 3) sensorIndex = 0;
            if (++colorIndex >= 4) colorIndex = 0;
            break;
            
        default:
            sensorIndex = intState;
            colorID = nowColor[sensorIndex];
            // nowColor[sensorIndex] = COLOR_NONE; // keep color for dummy!
    }

    CourseID courseID = COURSE_NONE;
    switch (sensorIndex) {
        case 0:
            courseID = COURSE_TOP;
            break;
        case 1:
            courseID = COURSE_MIDDLE;
            break;
        case 2:
            courseID = COURSE_BOTTOM;
            break;
    }

    printf("send: %i, ", sensorIndex);
    if (colorID != COLOR_NONE) {
        udp.sendD(colorID, courseID);
        printf("%s\n", udp.getLatestMessage());
    } else {
        printf("\n");
    }
    
    if ((courseID == COURSE_MIDDLE) && (colorID == COLOR_YELLOW)) {
        lightTimer.attach(&lightTurnON, LIGHT_TIME_BEFORE_ON);
    }
}


int main()
{
    serial.baud(9600);
    serial.printf("=== Wakeup (D)===\n");
    
    light = false;
    
    btn.fall(&onSensorON);
    sensor1.mode(PullUp);
    sensor1.fall(&onSensor1ON);
    sensor2.mode(PullUp);
    sensor2.fall(&onSensor2ON);
    sensor3.mode(PullUp);
    sensor3.fall(&onSensor3ON);
    
    udp.init("192.168.0.91", "255.255.255.0", "192.168.0.1");
    printf("IP is %s\n", udp.getIPAdr());

    while (true) {
        if (intState != INT_NONE) {
            sendMessageD();
            wait_ms(500);
            intState = INT_NONE;
            led1 = false;
        }
        
        MessageID msgID;
        int timestamp;
        ColorID colorID;
        CourseID courseID;
        
        bool isOK = udp.receive(msgID, timestamp, colorID, courseID);
        if (!isOK) continue;
        printf("receive:%d, %d, %d\n", msgID, colorID, courseID);
        
        if (msgID != MESSAGE_C) continue;
        
        switch (courseID) {
            case COURSE_TOP:
                nowColor[0] = colorID;
                break;
            case COURSE_MIDDLE:
                nowColor[1] = colorID;
                break;
            case COURSE_BOTTOM:
                nowColor[2] = colorID;
                break;
            default:
                continue;
        }
    }
}
#endif



// ========== POSITION_111 ==========
#ifdef POSITION_111

bool isON = false;
int doorState = 0;    // ForDebug
void onSW()
{
    if (isON) return;
    
    isON = true;
    led1 = true;
}

void main() {
    serial.baud(9600);
    serial.printf("=== Wakeup ===\n");

    btn.fall(&onSW);

    i2c.frequency(100000);

    stepper1.begin(0x0F);
    stepper1.free();   
    
    stepper2.begin(0x0E);
    stepper2.free();
    
    udp.init("192.168.0.111", "255.255.255.0", "192.168.0.1");
    printf("IP is %s\n", udp.getIPAdr());
    
    door1.forceClose();
    door2.forceClose();

    while (true) {
        if (isON) {
            printf("SW\n");
            switch (doorState) {
                case 0:
                    door1.close();
                    door2.close();
                    break;
                case 1:
                    door1.close();
                    door2.open();
                    break;
                case 2:
                    door1.open();
                    // door2.close();
            }
            if (++doorState >= 3) doorState = 0;
            Thread::wait(500);
            
            isON = false;
            led1 = false;
        }
        
        MessageID msgID;
        int timestamp;
        ColorID colorID;
        CourseID courseID;

        bool isOK = udp.receive(msgID, timestamp, colorID, courseID);
        if (!isOK) continue;
        printf("receive:%d, %d, %d\n", msgID, colorID, courseID);
        
        if (msgID != MESSAGE_C) continue;
        
        switch (courseID) {
            case COURSE_TOP:
                printf("Course Top Open\n");
                door1.close();
                door2.close();
                break;
            case COURSE_MIDDLE:
                printf("Course Middle Open\n");
                door1.close();
                door2.open();
                break;
            case COURSE_BOTTOM:
                printf("Course Bottom Open\n");
                door1.open();
                // door2.close();
                break;
            default:
                continue;
        }
    }
}

#endif


// ========== POSITION_131 ==========
#ifdef POSITION_131

/*
５連 COURSE_LEFT
0: タイムマシンシリーズ
1: たまごシリーズ
2: モンガーテレポート

2連 COURSE_CENTER
0: オバケの壁抜け
1: 通り抜けフープ
2: 天狗の抜け穴

縦大型 COURSE_RIGHT
0: 火星人(黄)
1: 火星人(緑)
2: ティラノサウルス(青)
3: ティラノサウルス(黄)
4: マンモス（緑)
5: マンモス(赤)
*/

struct ContentsSet {
    uint8_t contentNum;
    uint8_t contents[2];
};

ContentsSet B_L_contents = { 1, { 2,   } };
ContentsSet Y_L_contents = { 2, { 0, 3 } };
ContentsSet R_L_contents = { 1, { 5,   } };
ContentsSet G_L_contents = { 2, { 1, 4 } };
ContentsSet B_C_contents = { 2, { 1, 0 } };
ContentsSet Y_C_contents = { 1, { 2,   } };
ContentsSet R_C_contents = { 2, { 1, 2 } };
ContentsSet G_C_contents = { 1, { 0,   } };
ContentsSet B_R_contents = { 1, { 1,   } };
ContentsSet Y_R_contents = { 2, { 1, 0 } };
ContentsSet R_R_contents = { 1, { 2,   } };
ContentsSet G_R_contents = { 2, { 2, 0 } };

ContentsSet contentsList[][3] = {
    {
        R_L_contents,
        R_C_contents,
        R_R_contents,
    },
    {
        B_L_contents,
        B_C_contents,
        B_R_contents,
    },
    {
        G_L_contents,
        G_C_contents,
        G_R_contents,
    },
    {
        Y_L_contents,
        Y_C_contents,
        Y_R_contents,
    },
};

bool isON = false;
bool isSensorON = false;
int doorState = 0;    // ForDebug

#include <queue>
queue<ColorID> colorQueue;
CourseID courseID = COURSE_LEFT;
uint32_t contentsCounter = 0;

void onSW()
{
    if (isON) return;
    
    isON = true;
    led1 = true;
}

void onSensorON()
{
    if (isSensorON) return;
    
    isSensorON = true;
    led1 = true;
}

void main() {
    serial.baud(9600);
    serial.printf("=== Wakeup ===\n");

    btn.fall(&onSW);
    sensor1.fall(&onSensorON);

    i2c.frequency(100000);

    stepper1.begin(0x0F);
    stepper1.free();   
    
    stepper2.begin(0x0E);
    stepper2.free();
    
    udp.init("192.168.0.131", "255.255.255.0", "192.168.0.1");
    printf("IP is %s\n", udp.getIPAdr());
    
    door1.forceClose();
    door2.forceClose();

    while (true) {
        if (isON) {
            printf("SW\n");
            switch (doorState) {
                case 0:
                    door1.close();
                    door2.close();
                    break;
                case 1:
                    door1.close();
                    door2.open();
                    break;
                case 2:
                    door1.open();
                    door2.close();
            }
            if (++doorState >= 3) doorState = 0;
            Thread::wait(500);
            
            isON = false;
            led1 = false;
        }
        
        if (isSensorON) {
            printf("SensorON\n");
            
            if (!colorQueue.empty()) {
                ColorID colorID = colorQueue.front();
                colorQueue.pop();

                ContentsSet set = contentsList[colorID][courseID];
                int contentID = set.contents[contentsCounter % set.contentNum];
                contentsCounter++;
                printf("SendB(%d, %d, %d)\n", colorID, courseID, contentID);

                udp.sendB(colorID, courseID, contentID);
                
                switch (courseID) {
                    case COURSE_LEFT:
                        printf("Course Left Open\n");
                        door1.close();
                        door2.close();
                        break;
                    case COURSE_CENTER:
                        printf("Course Center Open\n");
                        door1.close();
                        door2.open();
                        break;
                    case COURSE_RIGHT:
                        printf("Course Right Open\n");
                        door1.open();
                        door2.close();
                        break;
                }
                        
                courseID = (CourseID)((int)courseID + 1);
                if (courseID > COURSE_RIGHT) courseID = COURSE_LEFT;
            }

            wait(3);
            
            isSensorON = false;
            led1 = false;
        }
        
        MessageID msgID;
        int timestamp;
        ColorID colorID;
        CourseID dCourseID;  // Dummy

        bool isOK = udp.receive(msgID, timestamp, colorID, dCourseID);
        if (!isOK) continue;
        
        printf("receive:%d, %d, %d\n", msgID, colorID, dCourseID);
        
        if (msgID != MESSAGE_A) continue;

        colorQueue.push(colorID);
    }
}

#endif