File content as of revision 30:63a8a5cefc6b:

#include "mbed.h"
#include "TextLCD.h"
#include "MCP23017.h"
#include "Train.h"
#include "Switch.h"
#include "Track.h"
#include "Detector.h"
#include <cstdlib>
#include <algorithm>

//Board 1
/*----------------------------------------------------------------------------
Pin definitions
*----------------------------------------------------------------------------*/
DigitalOut Track(p20);      // train track

DigitalOut myled1(LED1), myled2(LED2), myled3(LED3), myled4(LED4);
DigitalOut externalLed1(p15), externalLed2(p16), externalLed3(p17), externalLed4(p18);
TextLCD lcd(p22, p21, p23, p24, p25, p26); // lcd

DigitalIn sw1(p29), sw2(p30), sw3(p11), sw4(p12);

DigitalOut buzz(p10);

InterruptIn int0(p13), int1(p14);
I2C i2c(p28, p27);
MCP23017 *mcp;

//DigitalOut en(p6);

/*----------------------------------------------------------------------------
Addresses
*----------------------------------------------------------------------------*/
const unsigned int DCCaddress_silver = 0x01;
const unsigned int DCCaddress_red = 0x03;
const unsigned int DCCaddress_switch = 0x06;

/*----------------------------------------------------------------------------
Train movement
*----------------------------------------------------------------------------*/
//move backwards/reverse

//speed dial forward
const unsigned int slow_speed = 0x76; //step 4
//const unsigned int DCCinst_step6 = 0x68; //step 6 1/4 speed
const unsigned int normal_speed = 0x78; //step 13 1/2 speed

const unsigned int high_speed = 0x7A; //high speed

const unsigned int DCCinst_switch1 = 0x81; //Activate switch1
const unsigned int DCCinst_switch2 = 0x82; //Activate switch2
const unsigned int DCCinst_switch3 = 0x84; //Activate switch3
const unsigned int DCCinst_switch4 = 0x88; //Activate switch4
//stop 
const unsigned int stop = 0x40; //forward and stop 01100000

static const int cRight[] = {6,7,8};
vector<int> C_right (cRight, cRight + sizeof(cRight) / sizeof(cRight[0]) );

static const int cLeft[] = {0,1,2,12,13};
vector<int> C_left (cLeft, cLeft + sizeof(cLeft) / sizeof(cLeft[0]) );

static const int junction39[] = {3,9};
vector<int> junction_39 (junction39, junction39 + sizeof(junction39) / sizeof(junction39[0]) );

static const int junction511[] = {5,11};
vector<int> junction_511 (junction511, junction511 + sizeof(junction511) / sizeof(junction511[0]) );

static const int lane4[] = {4};
vector<int> lane_4 (lane4, lane4 + sizeof(lane4) / sizeof(lane4[0]) );

static const int lane10[] = {10};
vector<int> lane_10 (lane10, lane10 + sizeof(lane10) / sizeof(lane10[0]) );

int redStartVal = 10;
int silverStartVal = 15;

/*----------------------------------------------------------------------------
Function definitions
*----------------------------------------------------------------------------*/
bool readSwitch(DigitalIn theSwitch);
bool sameSection();
int convertHextoDec(int pos, int intnr);
void checkDetector(int inter);
void init_mcp();
void init_interrupt();
void init_trains();
void executeCase(int pos, Train* currTrain);
Train& assignTrain(int pos);

//Trains
Train redTrain(DCCaddress_red, normal_speed, redStartVal, high_speed, normal_speed);
Train silverTrain(DCCaddress_silver,normal_speed, silverStartVal, normal_speed, slow_speed);
//Switches
Switch switch1(DCCaddress_switch,DCCinst_switch1);
Switch switch2(DCCaddress_switch,DCCinst_switch2);
Switch switch3(DCCaddress_switch,DCCinst_switch3);
Switch switch4(DCCaddress_switch,DCCinst_switch4);
//Detectors
Detector dect(false);

/*----------------------------------------------------------------------------
Main
*----------------------------------------------------------------------------*/
int main() {
    buzz = 0;
    lcd.printf("Start journey");
    init_mcp();
    init_interrupt();
    init_trains();
    while(1){ 
        redTrain.sendCommand();
        silverTrain.sendCommand();
        
        if(!readSwitch(sw3)){ //Change speed
            lcd.cls();
            lcd.printf("Go forward");
            redTrain.changeSpeed(high_speed);
            silverTrain.changeSpeed(normal_speed);
        }
        
        if(redTrain.checkStop()){                       //check if Train stopped
            lcd.cls();
            lcd.printf("%d", redTrain.getPosition());
            executeCase(redTrain.getPosition(), &redTrain);
        }
        else if(silverTrain.checkStop()){
            executeCase(silverTrain.getPosition(), &silverTrain);
        }
        
        if (sameSection()){
            lcd.cls();
            lcd.printf("SAME SEC");
            silverTrain.Stop();
            redTrain.Stop();
        }     
        
        if(redTrain.checkStop() && silverTrain.checkStop())  {
            lcd.cls();
            lcd.printf("Stop loop...");
            break;    
        } 
    }
    
    lcd.printf("Shutting down...");
    bool in = true;
    while(in){ 
        redTrain.sendCommand();
        silverTrain.sendCommand();
        externalLed2 = 1;
       // wait(0.2);
        //externalLed2 = 0;
        buzz = 1;
        if(!readSwitch(sw3)){
             in = false;   
        }
            
    }
    buzz = 0;
    return 0;
}

/*----------------------------------------------------------------------------
Functions
*----------------------------------------------------------------------------*/
int convertHextoDec(int pos, int intnr){
    int newPos;
    switch(pos){
        case 0xfe: 
            newPos = 0;
            break;
        case 0xfd: 
            newPos = 1;
            break; 
        case 0xfb: 
            newPos = 2;
            break; 
        case 0xf7: 
            newPos = 3;
            break; 
        case 0xef: 
            newPos = 4;
            break; 
        case 0xdf: 
            newPos = 5;
            break; 
        case 0xbf: 
            newPos = 6;
            break; 
        case 0x7f: 
            newPos = 7;
            break; 
        default:
            return -1;
        }
        
        if(intnr == 1)
            newPos += 8;
    return newPos;
}

Train& assignTrain(int pos){
    
    //Check which train got the interupt
    if(redTrain.checkInterupt(pos) && !redTrain.checkStop()){
        redTrain.setPosition(pos);
        return redTrain;
    }
    else if(silverTrain.checkInterupt(pos) && !silverTrain.checkStop()){
        silverTrain.setPosition(pos);
        return silverTrain;
    }else if(redTrain.checkInterupt(pos) && redTrain.checkStop()){
        return redTrain;    
    }else if(silverTrain.checkInterupt(pos) && silverTrain.checkStop()){
        return silverTrain;    
    }
    else{
        lcd.cls();
        lcd.printf("NO TRAIN ASSIGNED");
        lcd.printf("%d", pos);
        silverTrain.Stop();
        redTrain.Stop();
        switch2.switchOff();
    }
}

void executeCase(int pos, Train* currTrain){
    lcd.cls();
    switch(pos){
    case 0:{
        lcd.printf("at 0");
        currTrain->changeSpeed(currTrain->normalSpeed());
        break;}
    case 1:{
        lcd.printf("at 1");
        //TO DO: Check which train is going and only slow down if it started from 2
        /*if(!currTrain->isClockwise()){
            currTrain->changeSpeed(slow_speed);//Slow down
            lcd.printf("slow down");
        }*/
        //else
            currTrain->changeSpeed(currTrain->normalSpeed());
        
        break;}
    case 2:{
       /*
       Stop everytime at 2     
       */
       lcd.printf("at 2"); 
       dect.showReservation();
       if(!dect.checkReservation(10) && !dect.checkReservation(11)){
            currTrain->Stop();
            lcd.printf("Stopping at 2");
       }
       else{ 
                                
           if(!dect.checkReservation(4) && !dect.checkReservation(3)){     //Check if 4 or 3 is reserved
               int v1 = rand() % 100;                                       //IF NOT randomize which way to go
               if (v1 < 50){                                                //0.25 chance to enable the switch2,  reserve 3,9
                    switch2.switchOn();
                    dect.makeReservation(junction_39);
                } 
                else {                                                      //0.75 chance to disable switch2, reserve 4
                   switch2.switchOff(); 
                   dect.makeReservation(lane_4);
                }        
            }else if(dect.checkReservation(4)){                             //IF 4 is reserved
                switch2.switchOn();                                        //Enable switch2, reserve 3,9
                dect.makeReservation(junction_39);
                    
            }else{                                                          //IF 3 reserved 
                switch2.switchOff();                                         //Disable switch2, reserve 4
                dect.makeReservation(lane_4);
            }
            currTrain->changeSpeed(currTrain->normalSpeed());                          //Go forward
        }    
        break;}
    case 3:{
        lcd.printf("at 3");
       
        
        switch2.switchOff();                        //Disable switch2
        dect.clearReservation(C_left);              //free nr 0,1,2,12,13 
        if(!dect.checkReservation(5)){ 
            currTrain->changeDirection();               //change direction  
            currTrain->changeSpeed(currTrain->normalSpeed());     //go forward 
        }   
        else{
            currTrain->Stop();
        }                                                           
        break;
        }
    case 4:{
        lcd.printf("At 4");
        dect.clearReservation(C_left);   //free nr 0,1,2,12,13
        switch3.switchOn();                 //Turn on switch3
        dect.showReservation();
        if(dect.checkReservation(6)){   //Check if 6 is reserved
            currTrain->Stop();           //IF yes STOP
        }
        else{
            dect.makeReservation(C_right);  //else reserve 6,7,8 and go forward
            currTrain->changeSpeed(currTrain->normalSpeed());
        }
         break;  
        }    
    case 5:{
        lcd.printf("at 5");
        
        switch3.switchOn();                        //Enable switch3
        dect.clearReservation(C_right);              //free nr 6,7,8
        
        if(!dect.checkReservation(3)){  
            currTrain->changeDirection();               //change direction  
            currTrain->changeSpeed(currTrain->slowlySpeed());     //go forward 
        }   
        else{
            currTrain->Stop();
        }                                                                 
        break;
        }
    case 6:{
        lcd.printf("At 6");
        if(!currTrain->isClockwise())        //IF CC goes towards 7
        {
            switch3.switchOn();                                             //Enable switch3
            //int arr [3] = {4,5,11};
            //vector<int> detectors(arr, arr + sizeof(arr) / sizeof(int));
            dect.clearReservation(lane_4); 
            dect.clearReservation(junction_511);                              //free nr 4,5,11
            currTrain->changeSpeed(currTrain->normalSpeed());                          //Move forward
        }
        else{                           
           if(dect.checkReservation(5)){                           //IF 5 is reserved
                currTrain->Stop();
                    
            }else{                                                          //else 5 reserved
                switch3.switchOff();                                         //Enable switch3, reserve 4
                dect.makeReservation(junction_511);
                currTrain->changeSpeed(currTrain->normalSpeed());
            }
        }         
        break;
        }
    case 7:{
        lcd.printf("At 7");        
        currTrain->changeSpeed(currTrain->normalSpeed());
        break;}
    case 8:{
          lcd.printf("At 8");        
        if(currTrain->isClockwise())        //IF C goes towards 7
        {
            switch4.switchOff();                                             //Disable switch4
            dect.clearReservation(junction_39);                                //Free 3,9,10
            currTrain->changeSpeed(currTrain->normalSpeed());                          //Move forward
        }
       else{                                                               //IF CC        if 10 is reserved stop
            if(dect.checkReservation(10)){
                currTrain->Stop();
            }else{                
                switch4.switchOff();                                         //Disable switch4, reserve 10
                dect.makeReservation(lane_10);
                currTrain->changeSpeed(currTrain->slowlySpeed());                          //Go forward
            }
        }         
        break;
        }
    case 9:{
        lcd.printf("at 9");         
        if(!dect.checkReservation(8)){
            switch4.switchOn();         //Enable switch4
            dect.makeReservation(C_right);                                 //Reserve 6,7,8
            currTrain->changeSpeed(currTrain->normalSpeed());                    //Go forward

        }
        else{
            lcd.printf("8 is reserved");         
            currTrain->Stop();       
        }
        break;
        }
    case 10:{
        lcd.printf("At 10");        

        dect.clearReservation(C_right); //free nr 6, 7, 8, 9
        if(dect.checkReservation(12)){   //Check if 12 is reserved
            currTrain->Stop();           //IF yes STOP
        }
        else{     
            switch1.switchOff();          //Turn off switch1                     
            dect.makeReservation(C_left);
            currTrain->changeSpeed(currTrain->normalSpeed());      //go forward
        }     
        break;
        }
        
    case 11:{
        lcd.printf("at 11");        
        
        if(!dect.checkReservation(12)){
            
            switch1.switchOn();                        //Enable switch1
            dect.makeReservation(C_left);               //Reserve 0,1,2, 12, 13   
            currTrain->changeSpeed(currTrain->normalSpeed());         //go forward   
        }
        else{
            currTrain->Stop();   
        }                                                             
        break;
        }
    case 12: {
        lcd.printf("at 12");        
        switch1.switchOff();               //Turn of switch1
        //int arr2 [3] = {5,10,11};
        //vector<int> detectors2(arr2, arr2 + sizeof(arr2) / sizeof(int));
        dect.clearReservation(lane_10); //free nr 5, 10, 11
        dect.clearReservation(junction_511);
        currTrain->changeSpeed(currTrain->normalSpeed());                          //Move forward
        
        break;
        }
    case 13: {
        lcd.printf("at 13");
        currTrain->changeSpeed(currTrain->normalSpeed());
        break;
        }
    case 14:{
        lcd.printf("at 21");            
        currTrain->changeSpeed(currTrain->slowlySpeed());
        break;
        }
    case 15:{
        lcd.printf("at 22");
        currTrain->changeSpeed(currTrain->normalSpeed());
        break;
        }
    default:   {
        lcd.printf("Not in a valid case");
        currTrain->Stop(); 
        break; 
        }
    }
    
    mcp->_read(GPIOA);
    mcp->_read(GPIOB);
}

void checkDetector(int inter){
    lcd.cls();
    int pos;
    wait_us(100);
    if (inter == 0){
        //Read from first one
        pos = mcp->_read(INTCAPA);
        pos = convertHextoDec(pos, 0);
    }
    else if (inter == 1){
        //Read from second one
        pos = mcp->_read(INTCAPB);
        pos = convertHextoDec(pos, 1);
    }
    else 
        //PROBLEM
        lcd.printf("Fail to detect int");
    if(pos!=7){
        Train* currTrain = &assignTrain(pos);
        executeCase(pos, currTrain); 
    }
}

bool readSwitch(DigitalIn theSwitch){
    int val = theSwitch.read();
    if(val == 1)
        return true;
    else
        return false;
}

void init_mcp() {
    // Initialisation of MCP registers,documentation on registers is availableatNiels/Jacco/Natalia
    mcp = new MCP23017(i2c, 0x40);
    mcp->_write(IODIRA, (unsigned char )0xff);
    mcp->_write(IODIRB, (unsigned char )0xff);
    mcp->_write(IPOLA, (unsigned char )0x00);
    mcp->_write(IPOLB, (unsigned char )0x00);
    mcp->_write(DEFVALA, (unsigned char )0xff);
    mcp->_write(DEFVALB, (unsigned char )0xff);
    mcp->_write(INTCONA, (unsigned char )0xff);
    mcp->_write(INTCONB, (unsigned char )0xff);
    mcp->_write(IOCONA, (unsigned char )0x2);
    mcp->_write(IOCONB, (unsigned char )0x2);
    mcp->_write(GPPUA, (unsigned char )0xff);
    mcp->_write(GPPUB, (unsigned char )0xff);
}
void passZero(){
    checkDetector(0);
}

void passOne(){
    checkDetector(1);
}

void init_interrupt() {
    // Clear current interrupts
    //en = 1;
    mcp->_read(GPIOA);
    mcp->_read(GPIOB);
    // Register callbacks
    int0.fall(&passZero);
    int1.fall(&passOne);
    // Enable interrupts on MCP
    mcp->_write(GPINTENA, (unsigned char )0xff);
    mcp->_write(GPINTENB, (unsigned char )0xff);// Ready to go!
}

bool sameSection(){
    int posRed = redTrain.getPosition();
    int posSilver = silverTrain.getPosition();
    
    if ((std::find(C_left.begin(), C_left.end(), posRed) !=C_left.end()) && (std::find(C_left.begin(), C_left.end(), posSilver) !=C_left.end()))
        return true;
    else if ((std::find(C_right.begin(), C_right.end(), posRed) !=C_right.end()) && (std::find(C_right.begin(), C_right.end(), posSilver) !=C_right.end()))
        return true;
    else if ((std::find(junction_511.begin(), junction_511.end(), posRed) !=junction_511.end()) && (std::find(junction_511.begin(), junction_511.end(), posSilver) !=junction_511.end()))
        return true;
    else if ((std::find(junction_39.begin(), junction_39.end(), posRed) !=junction_39.end()) && (std::find(junction_39.begin(), junction_39.end(), posSilver) !=junction_39.end()))
        return true;      
         
    return false;
}

void init_trains(){
    redTrain.changeSpeed(stop);
    silverTrain.changeSpeed(stop);
    
    vector<int> res;
    res.push_back(2);
    //res.push_back(4);
    dect.makeReservation(C_left); //Run trains counterclockwise and reserve 
    vector<int> res2;
    res2.push_back(10);
    //res2.push_back(13);
    dect.makeReservation(res2);
    dect.showReservation();
}