File content as of revision 37:bb15bea420a3:

#include "mbed.h"
#include "TextLCD.h"
#include "MCP23017.h"
#include <string>
#include <iostream>
#include <vector>

using namespace std;

//mbed DCC Model Train Demo
 
/******PINS AND DECLARATIONS*******/

//------PINS

//SWITCHES p5 - p8
DigitalIn switch1(p5);
DigitalIn switch2(p6);
DigitalIn switch3(p7);
DigitalIn switch4(p8);

//RAIL SENSORS - INT0,INT1
//INT0 - p9
InterruptIn int0(p9);
//INT1 - p10
InterruptIn int1(p10);

///p11
///p12

//M0 - p13
DigitalIn d21stat(p13);     //Sensor right of the station
//M1 - p14
DigitalIn d22stat(p14);     //Sensor left of the station
//M2 - p15
DigitalIn station(p15); //Sensor in the middle of the station

//p16
//p17

//BUZZER - p18
DigitalOut buzz(p18); // buzz=0 doesn't beep, buzz=1 beeps

//POTENTIOMETER - p19
AnalogIn pot(p19);  //Gives float value pot.read(). Convert analog input to V with f*3.3

//DAT - p20
DigitalOut Track(p20); //Digital output bit used to drive track power via H-bridge

//LCD SCREEN - p21, p22, p23, p24, p25, p26
TextLCD lcd(p22,p21,p23,p24,p25,p26); // RS, E, A4, A5, A6, A7 // ldc.cls() to clear and printf(String up to 16char)

///p27
///p28
I2C i2c(p28,p27);

//LED1 - p29
DigitalOut redled(p29);
//LED2 - p30
DigitalOut greenled(p30);

//MBED LEDS
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);

//MCP
MCP23017 *mcp;


//------GLOBAL VARS

#define D0 0
#define D1 1
#define D2 2
#define D3 3
#define D4 4
#define D5 5
#define D6 6
#define D7 7
#define D8 8
#define D9 9
#define D10 10
#define D11 11
#define D12 12
#define D13 13
#define D21 14
#define D22 15


//.....DCC TRAIN COMMAND VARS

//typical out of box default engine DCC address is 3 (at least for Bachmann trains)
//Note: A DCC controller can reprogram the address whenever needed
const unsigned int DCCaddressDR = 0x01; //Address for train 1 DARK-RED
const unsigned int DCCaddressLR = 0x03; //Address for train 3 LIGHT-RED

//01DCSSSS for speed, D is direction (fwd=1 and rev=0), C is speed(SSSSC) LSB
const unsigned int DCCinst_forward = 0x68; //forward half speed
const unsigned int DCCinst_forward_slow = 0x66; //forward half speed
const unsigned int DCCinst_reverse = 0x48; //reverse half speed
const unsigned int DCCinst_stop = 0x50;    //stop the train

//100DDDDD for basic headlight functions
const unsigned int DCC_func_lighton = 0x90; //F0 turns on headlight function
const unsigned int DCC_func_dimlight = 0x91; //F0 + F1 dims headlight


//.....SWITCH COMMAND VARS
    
const unsigned int SWBaddress = 0x06; //Address for switch box

//100DDDDD where DDDDD is the switch command and 100 is constant:

//00001(F1 active)-00010(F2 active)-00100(F3 active)-01000(F4 active)
//Example - 111111 0 00000101 0 10000000 0 10000101 1 - idle

const unsigned int SWBidle = 0x80; //IDLE - Flip last activated SW.
const unsigned int SWBflip_1 = 0x81; //Flip SW1
const unsigned int SWBflip_2 = 0x82; //Flip SW2
const unsigned int SWBflip_3 = 0x84; //Flip SW3
const unsigned int SWBflip_4 = 0x88; //Flip SW4

//Arrays that will keep track of the position and direction of th trains.
//[pos_trains] -1 = not in track / any other number = sensor where the train is at (sensors 1-16)
//[dir_trains] -1 = left / 1 = right
int pos_trains [2] = {-1,-1};
int dir_trains [2] = {-1,-1};





//**************** FUNCTIONS FOR DENVER TRAIN ****************//

class Position{
    private:
        int position; 
        vector <int> previous_cw;
        vector <int> previous_ccw;
    public:
        Position(int p){
            position = p;
        }

        vector <int> get_prev_cw(){
            return previous_cw;    
        }
        
        vector <int> get_prev_ccw(){
            return previous_ccw;    
        }
        
        void add_prev_cw(int pos){
            previous_cw.push_back(pos);
        };
        
        void add_prev_ccw(int pos){
            previous_ccw.push_back(pos);
        };
};

Position d0(D0);
Position d1(D1);
Position d2(D2);
Position d3(D3);
Position d4(D4);
Position d5(D5);
Position d6(D6);
Position d7(D7);
Position d8(D8);
Position d9(D9);
Position d10(D10);
Position d11(D11);
Position d12(D12);
Position d13(D13);
Position d21(D21);
Position d22(D22);

vector<Position> positions;


//Starting position and orientation of the trains
int DR_train_pos = D4;
bool DR_cw = true;

int LR_train_pos= D10;
bool LR_cw = true;


/**
*Activates the buzzer for 0.5 seconds.
**/
void doBuzz(){
    
    buzz = 1;
    wait(0.5);
    buzz = 0;    
}

void init_positions(){
    //Initialize array with positions
    positions.push_back(d0);
    positions.push_back(d1);
    positions.push_back(d2);
    positions.push_back(d3);
    positions.push_back(d4);
    positions.push_back(d5);
    positions.push_back(d6);
    positions.push_back(d7);
    positions.push_back(d8);
    positions.push_back(d9);
    positions.push_back(d10);
    positions.push_back(d11);    
    positions.push_back(d12);
    positions.push_back(d13);
    positions.push_back(d21);
    positions.push_back(d22);
    
    
    d0.add_prev_cw(D1);
    d0.add_prev_ccw(D13);
    
    d1.add_prev_cw(D22);
    d1.add_prev_ccw(D0);
    
    d22.add_prev_cw(D2);
    d22.add_prev_ccw(D1);
    
    d2.add_prev_cw(D21);
    d2.add_prev_ccw(D22);
    
    d21.add_prev_cw(D3);
    d21.add_prev_cw(D4);
    d21.add_prev_ccw(D2);
    
    d3.add_prev_cw(D9);
    d3.add_prev_ccw(D21);
    
    d4.add_prev_cw(D6);
    d4.add_prev_ccw(D21);
    
    d5.add_prev_cw(D6);
    d5.add_prev_ccw(D11); 
    
    d6.add_prev_cw(D7);
    d6.add_prev_ccw(D4);
    d6.add_prev_ccw(D5);
    
    d7.add_prev_cw(D8);
    d7.add_prev_ccw(D6);
    
    d8.add_prev_cw(D7);
    d8.add_prev_ccw(D9);
    d8.add_prev_ccw(D10);
    
    d9.add_prev_cw(D3);
    d9.add_prev_ccw(D8);
    
    d10.add_prev_cw(D12);
    d10.add_prev_ccw(D8);  
    
    d11.add_prev_cw(D12);
    d11.add_prev_ccw(D5);  
    
    d12.add_prev_cw(D13);
    d12.add_prev_ccw(D10); 
    d12.add_prev_ccw(D11);  
    
    d13.add_prev_cw(D0);
    d13.add_prev_ccw(D12);  
}

/**
*
*Here we initialize the mcp that will be used to manage the interrupts.
*
**/
void initialize_mcp(){
    mcp = new MCP23017(i2c,0x40); //Connect to SCL - p28 and SDA - p27 and MPC I2C address 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);    
    
}

/**
*
*Returns the number of the sensor where the train was detected.
*
**/
int get_sensor(unsigned int number,int interrupt){
    int sensor = -1;
    
    for(int i=0; i<8; i++){
        
        if(~number & 1<<i){            
            sensor = i; 
        }
    }
    
    if(interrupt == 1){
        sensor+= 8; // Sensors caught by interreupt1 are identified from 8 to 15.
    }
    return sensor;
}

void update_train_pos(int sensor){
    bool found_DR = false;
    bool found_LR = false;
    
    Position pos = positions[sensor];
    for(int i = 0; i<pos.get_prev_cw().size();i++){
            int prev = pos.get_prev_cw()[i];
            
            if(DR_train_pos == prev){
              found_DR = true;
              DR_train_pos = prev; //We update the position of the train  
            }
            
            if(LR_train_pos == prev){
                found_LR = true;
                LR_train_pos = prev;   //We update the position of the train  
            }
     }
     
     if(found_DR){
        lcd.cls();
        lcd.printf("DR is at D%d",DR_train_pos);
    }
    if(found_LR){
        lcd.cls();
        lcd.printf("LR is at D%d",LR_train_pos);   
    }
    if(!found_DR && !found_LR){
        lcd.cls();
        lcd.printf("No train before :(");
    }
}


/**
*
*Method to catch interrupts 0
*
**/
void on_int0_change(){
     
     wait_us(2000);
     int sensor_data = mcp->_read(INTCAPA);
     int sensor = get_sensor(sensor_data,0);
     lcd.cls();
     lcd.printf("int0 0x%x \n Sensor: %d",sensor_data,sensor);
     
     update_train_pos(sensor);
}



/**
*
*Method to catch interrupts 1
*
**/
void on_int1_change(){
    
    wait_us(2000);
     int sensor_data = mcp->_read(INTCAPB);
     int sensor = get_sensor(sensor_data,1);
     lcd.cls();
     lcd.printf("int1 0x%x \n Sensor: %d",sensor_data,sensor);
     
     update_train_pos(sensor);
}


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


/**
*
*Method to send DCC commands to train and switches.
*
*@address - (HEX)Address where the commands will be sent
*@inst - (HEX)Number of instruction that will be commanded
*@repeat_count - Number of times the command will be sent
*
**/
void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count)
{
    unsigned __int64 command = 0x0000000000000000; // __int64 is the 64-bit integer type
    unsigned __int64 temp_command = 0x0000000000000000;
    unsigned __int64 prefix = 0x3FFF; // 14 "1" bits needed at start
    unsigned int error = 0x00; //error byte
    
    //calculate error detection byte with xor
    error = address ^ inst;
    
    //combine packet bits in basic DCC format
    command = (prefix<<28)|(address<<19)|(inst<<10)|((error)<<1)|0x01;
    //printf("\n\r %llx \n\r",command);
    
    int i=0;
    //repeat DCC command lots of times
    while(i < repeat_count) {
        
        temp_command = command;
        //loops through packet bits encoding and sending out digital pulses for a DCC command
        for (int j=0; j<64; j++) {
            
            if((temp_command&0x8000000000000000)==0) { 
            //test packet bit
                //send data for a "0" bit
                Track=0;
                wait_us(100);
                Track=1;
                wait_us(100);
                //printf("0011");
                
            }else{
                
                //send data for a "1"bit
                Track=0;
                wait_us(58);
                Track=1;
                wait_us(58);
                //printf("01");
            }
            // next bit in packet
            temp_command = temp_command<<1;
        }
        i++;
    }
}


/**
*
*Method to flip the switches
*
*@switchId - (1-4)The ID of the switch we want to flip
*@times - The number of times we want to send the command
*@activate - True if the switch is going to be activated. False if it needs to go back to rest position.
*
**/
void flipSwitch(int switchId, int times, bool activate=true){
    
    unsigned int SWBflip = SWBidle; //IDLE - Flip last activated SW.
    
    switch(switchId){
        case 1:
            SWBflip = SWBflip_1;     //FLIP SW1
            break;
        case 2:
            SWBflip = SWBflip_2;     //FLIP SW2
            break;
        case 3:
            SWBflip = SWBflip_3;     //FLIP SW3
            break;
        case 4:
            SWBflip = SWBflip_4;     //FLIP SW4
            break;
        default:
            break;    
    }          

    //Security measure not to burn the switch.
    if(times <=5){ 
        DCC_send_command(SWBaddress,SWBflip,times); //Activating switch
        if(!activate){
              DCC_send_command(SWBaddress,SWBidle,times);  //Sending IDLE to flip back.
        }
     }
    
}


/**
*
*Checks if any of the switches of the box has been activated. 
*Calls necessary function and displays LCD text.
*
**/
void checkSwitch(){
    
    if(switch1 == 1){
        
        lcd.cls();
        lcd.printf("Switch 1 ON - SW1");
        flipSwitch(1,5);
        
     }else if(switch2 == 1){
        
            lcd.cls();
            lcd.printf("Switch 2 ON - SW2");
            flipSwitch(2,5);
        
        }else if(switch3 == 0){
                
                lcd.cls();
                lcd.printf("Switch 3 ON - SW3");
                flipSwitch(3,5);
                
            }else if(switch4 == 0){
        
                    lcd.cls();
                    lcd.printf("Switch 4 ON - IDLE");
                    flipSwitch(0,5);         
            }
    }



//**************** MAIN PROGRAM FOR DENVER TRAIN ****************//

int main()
{
    //RISE FOR INTERRUPTS?? NOT WORKING ATM
    //int0.rise(&interrupt0);
    //int1.rise(&interrupt1);
    
    //Read and display potentiometer
    //float f = pot.read();
    //float vin = f * 3.3;
    //lcd.printf("vin: %.4f",vin);
    
    //0xFFFC     //1111111111111100
    
    
    //Led routine to start main program
    led1 = 1;
    wait(0.2);
    led1 = 0;
    wait(0.2);
    led1 = 1;
    
    initialize_mcp();   //mcp initialization for interrupts before train running
    init();
    init_positions();
    
    //Train light routine to start running
    DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // turn light on full
    DCC_send_command(DCCaddressDR,DCC_func_dimlight,400); //dim light
    DCC_send_command(DCCaddressDR,DCC_func_lighton,200);  //light full again

    //LED3 Shows start of route + LCD notif
    led3 = 1; // Entering the while
    lcd.cls();
    lcd.printf("Ready to start");
    
    //Demo for stopping at the station
    while(1) {
        
        checkSwitch();  //Checks for switch commands everytime.  
        
        if(1==0){
        //if(station == 1){      //If train is on the sensor at the middle of the station it stops and displays LCD text.
            
            lcd.cls();
            lcd.printf("All aboard\n mind the gap");
            DCC_send_command(DCCaddressDR,DCCinst_stop,400);  
            lcd.cls();
            
        }else{
            DCC_send_command(DCCaddressDR,DCCinst_forward,1); // Forward half speed train addres DARK-RED             
            DCC_send_command(DCCaddressLR,DCCinst_forward,1); // Forward half speed train address LIGHT-RED
        } 
    }
}