File content as of revision 25:90f7a34c253a:

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

//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);

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
const unsigned int DCCinst_reverse = 0x48; //reverse speed

//speed dial forward
const unsigned int DCCinst_step2 = 0x72; //step 2
const unsigned int DCCinst_step4 = 0x73; //step 4
//const unsigned int DCCinst_step6 = 0x68; //step 6 1/4 speed
const unsigned int DCCinst_step13 = 0x78; //step 13 1/2 speed
const unsigned int DCCinst_step20 = 0x75; //step 20 3/4 speed
//const unsigned int DCCinst_step28 = 0x7F; //step 28 Full 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
const unsigned int DCCinst_deactive_switch = 0x80; //Deactivate switches
//stop 
const unsigned int DCCinst_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]) );

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

//Trains
Train redTrain(DCCaddress_red, DCCinst_step13, 12);
Train silverTrain(DCCaddress_silver,DCCinst_step13, 2);
//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);

// Detectors address
/*
d0 = 0xfe //254
d1 = 0xfd //253
d2 = fb //251
d3 = f7 //247
d4 = ef //239
d5 = df //223
d6 = bf //191
d7 = 7f //255
d8 = 0xfe //254
d9 = fd
d10 = fb
d11 = f7
d12 = ef
d13 = df
*/
/*----------------------------------------------------------------------------
Main
*----------------------------------------------------------------------------*/
int main() {
    lcd.printf("Start journey");
    init_mcp();
    init_interrupt();
    init_trains();
    while(1){ 
        redTrain.sendCommand();
        silverTrain.sendCommand();
        
        if(!readSwitch(sw3)){ //Change speed
            lcd.printf("Go forward");
            redTrain.changeSpeed(DCCinst_step13);
            silverTrain.changeSpeed(DCCinst_step13);
        }
        
        if(redTrain.checkStop()){                       //check if Train stopped
            lcd.cls();
            lcd.printf("in stop");
            executeCase(redTrain.getPosition());
        }
        else if(silverTrain.checkStop()){
            executeCase(silverTrain.getPosition());
        }
        
        if(redTrain.checkStop() && silverTrain.checkStop())  {
            break;    
        }      
    }
    
    lcd.printf("Shutting down...");
    // en = 0; TO Do : emergency shutdown!!
    //wait(3);
    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.setPosition(pos);
        return redTrain;
    }
    else if(silverTrain.checkInterupt(pos)){
        silverTrain.setPosition(pos);
        return silverTrain;
    }else{
    lcd.cls();
    lcd.printf("NO TRAIN ASSIGNED");
    silverTrain.Stop();
    redTrain.Stop();
    switch2.switchOff();

    }
}

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

        }
        else{
            lcd.printf("from 9->8 : 8 reserved");         
            currTrain->Stop();       
        }
        break;}
    case 10:{
        lcd.printf("At 10");        
        dect.showReservation();
        wait(2);

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

bool readSwitch(DigitalIn theSwitch){
    int val = theSwitch.read();
    //lcd.printf("%d", val);
    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!
}

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