Diff: main.cpp

Revision:

0:9c82986d7cb9

Child:

1:1648ce825c31

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 21 10:21:36 2019 +0000
@@ -0,0 +1,536 @@
+#include "mbed.h"
+#include <string>
+#include <stdarg.h>
+#include "TextLCD.h"
+#include "MCP23017.h"
+#include "math.h"
+//#include "Track.hpp"
+#include <list>    
+
+bool test = false;
+int iter;
+std::list<int> passedMarkers;
+std::list<int>::iterator it;
+
+
+//BOX IO
+DigitalIn button1 (p8);
+DigitalIn button2 (p9);
+DigitalIn button3 (p10);
+DigitalIn button4 (p30);
+AnalogIn potentio (p15); // configures pin15 for analog input. Creates object Ain. It is the potentiometer.
+TextLCD lcd(p22, p21, p23, p24, p25, p26); // configures the LCD pins. Creates object lcd
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+//DigitalOut led4(LED4);
+//DigitalOut buzzer (p29);
+
+//CONSTANTS
+unsigned int DCCaddress_train1 = 0x01;
+unsigned int DCCaddress_train2 = 0x02;
+unsigned int DCCaddress_decoder = 0x06;
+unsigned int DCCinst_stop = 0x70;         //stop 01 1 1 0000
+unsigned int DCCinst_estop = 0x71;        //01 1 1 0001
+unsigned int DCCswitchneutral = 0b10000000;
+unsigned int DCCswitch1 = 0b10000001;
+unsigned int DCCswitch2 = 0b10000010;
+unsigned int DCCswitch3 = 0b10000100;
+unsigned int DCCswitch4 = 0b10001000;
+
+//CONNECTIONS
+Serial pc(USBTX, USBRX); // Initialise the serial connection for terminal output
+InterruptIn int0(p13);
+InterruptIn int1(p14);
+I2C i2c(p28, p27);
+MCP23017 *mcp;  //hall detectors
+//MCP23017 *mcp2; //signals VANITY
+DigitalOut data(p16); // configures pin16 for digital output. Creates object train. This signal should go to the booster
+DigitalOut enable(p17); //configures pin 17 for digital output. creates object enable, this enables the train track or disables it in case of emergency
+
+//interrupt flags
+bool int0flag;
+bool int1flag;
+
+//global variables for train1
+bool dir1;
+bool prevdir1;
+bool light1;
+int speed1;
+int prev1;
+int curr1;
+int next1;
+
+//global variables for train2
+bool dir2;
+bool prevdir2;
+bool light2;
+int speed2;
+int prev2;
+int curr2;
+int next2;
+
+//global variables for switches
+std::time_t switch1action;
+std::time_t switch2action;
+std::time_t switch3action;
+std::time_t switch4action;
+bool switch1;
+bool switch2;
+bool switch3;
+bool switch4;
+double safetytime = 5.; //contant time limit
+int switchwait = 1500; //time inbetween different switch commands
+//Track t;
+
+//FUNCTION PROTOTYPES
+void init_mcp(void);
+void on_int0_change(void);
+void on_int1_change(void);
+void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count);
+std::list<int> circuit0markers(int sensor_data);
+std::list<int> circuit1markers(int sensor_data);
+
+//void printLCD(string s);
+//void printPC(string s);
+void displayTerminalStatus(void);
+void displayLCDstatus(void);
+
+void init(void);
+void initSwitches(void);
+void startupWait(void);
+void emergencyStop(void);
+void handleInt0(void);
+void handleInt1(void);
+void trainCommand(void);
+void iterAction(void);
+
+void changeSwitch1(bool);
+void changeSwitch2(bool);
+void changeSwitch3(bool);
+void changeSwitch4(bool);
+
+unsigned int trainInstruction(bool direction, bool light, int speed);
+
+//drive train
+//reverse train
+//stop train
+//void switch1and3(bool inward);
+
+//void switch2and4(bool inward);
+
+//stop train at station
+
+int main() {
+    wait_ms(1000); //safety wait when starting (power surge)
+    enable = false;
+    while(1){
+        init(); // initialize all the things to starting values
+        startupWait(); //wait for user to press button 3 to begin
+        enable = true;
+        initSwitches(); //put switches in starting position (can only happen after enable)
+        
+        if(!test){
+            iter = 0; //iter count   
+            while(enable){
+                if(!button4){ 
+                    emergencyStop();
+                    break;
+                }
+            
+                if(int0flag){
+                    handleInt0(); //store passed circuit 0 markers in the list passed markers
+                }
+            
+                if(int1flag){
+                    handleInt1(); //same for circuit 1
+                }
+                
+                if(!passedMarkers.empty()){ //TODO temporary
+                    for (it=passedMarkers.begin(); it!=passedMarkers.end(); ++it){
+                        if(*it != next1 && *it != next2){
+                            pc.printf("Unexpected marker %d. Expected train1: %d or train2: %d. Sensor failure or unknown object on track. Aborting.\n\r", *it, next1, next2);
+                            enable = false;
+                            return 1;   
+                        }
+                        
+                        updateTrainStatus(*it);
+                        //pc.printf("Marker: %d\n\r", *it);    
+                    }
+                    passedMarkers.clear();  
+                }
+                
+                trainCommand(); //decides for a command to send to the track
+                
+                iterAction(); //every X iterations do some stuff
+                iter++;
+            }
+        } else {
+            //test scripts   
+        }
+    }
+}
+
+//MAIN LOOP ROUTINES
+void startupWait(){
+    lcd.cls();
+    lcd.printf("Press button 3 to start.");
+    pc.printf("Press button 3 to start.\n\r");  
+    while(button3){
+        //do nothing   
+    }
+    pc.printf("Starting...\n\r");  
+}
+
+void emergencyStop(){
+    enable = false;
+    pc.printf("Emergency stop! Press button 3 when ready to restart\n\r");
+    
+    lcd.cls();
+    lcd.printf("Press button 3 when ready to restart");
+    while(button3){
+        //do nothing   
+    }
+}
+
+void init(){    
+    //initialize clock time
+    set_time(1);
+    switch1action = time(NULL);
+    switch2action = time(NULL);
+    switch3action = time(NULL);
+    switch4action = time(NULL);
+    
+    led1 = 0;
+    led2 = 0;
+    led3 = 0;
+    
+    //pc.baud(921600); //faster printing
+    pc.baud(19200);
+    
+    //initialize mcp   //(initialize interrupt flags)
+    init_mcp();
+  
+    //initialize train globals
+    dir1 = true;
+    prevdir1 = true;
+    light1 = true;
+    speed1 = 7;
+
+    //global variables for train2
+    dir2 = false;
+    prevdir2 = true;
+    light2 = true;
+    speed2 = 8;
+
+    //initialize switch positions
+    switch1 = false;
+    switch2 = false;
+    switch3 = false;
+    switch4 = false;
+    
+    //VANITY green signals
+}
+
+void initSwitches(){
+    wait_ms(switchwait);
+    changeSwitch1(false);
+    wait_ms(switchwait);
+    changeSwitch2(false);
+    wait_ms(switchwait);
+    changeSwitch3(false);
+    wait_ms(switchwait);
+    changeSwitch4(false);
+    wait_ms(500);   
+}
+
+void iterAction(){
+    displayLCDstatus();
+    if(iter % 100 == 0){
+        displayTerminalStatus();
+        //iter = 0;
+    }
+}
+
+void trainCommand(){
+    //decide what to do
+    
+    
+    //check reverse
+    
+    //drive
+    DCC_send_command(DCCaddress_train1, trainInstruction(dir1, light1, speed1), 1);   //TODO this is the only decision right now
+    DCC_send_command(DCCaddress_train2, trainInstruction(dir2, light2, speed2), 2); //TODO train 2 seems unresponsive  
+}
+
+void handleInt0(){
+    wait_us(2000);
+    int sensor_data = mcp->_read(INTCAPA);
+    int0flag = false;
+    std::list<int> temp = circuit0markers(sensor_data);
+    passedMarkers.splice(passedMarkers.end(), temp);
+}
+
+void handleInt1(){
+    wait_us(2000);
+    int sensor_data = mcp->_read(INTCAPB);
+    int1flag = false;  
+    std::list<int> temp = circuit1markers(sensor_data);
+    passedMarkers.splice(passedMarkers.end(), temp); 
+}
+
+
+
+
+
+//TRACK COMMANDS
+//code from the tutorial, sends a message to the track through the train pin
+void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count){
+    switch(address){
+        case DCCaddr_train1: led1 = 1; break;
+        case DCCaddr_train2: led2 = 1; break;
+        case DCCaddr_decoder: led3 = 1; break;    
+    }
+    
+    
+    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
+                data=0;
+                wait_us(100);
+                data=1;
+                wait_us(100);
+                //printf("0011");
+            } else {
+                //send data for a "1"bit
+                data=0;
+                wait_us(58);
+                data=1;
+                wait_us(58);
+                //printf("01");
+            }
+            // next bit in packet
+            temp_command = temp_command<<1;
+        }
+        i++;
+    }
+    
+    led1 = 0;
+    led2 = 0;
+    led3 = 0; //TODO maybe too fast
+}
+
+void changeSwitch1(bool inwards){
+    if(inwards){
+        DCC_send_command(DCCaddress_decoder, DCCswitch1, 1);
+        pc.printf("SW 1 inward!\n\r");
+    } else {
+        DCC_send_command(DCCaddress_decoder, DCCswitch1, 1);
+        DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
+        pc.printf("SW 1 outward!\n\r");        
+    }
+}
+
+void changeSwitch2(bool inwards){
+    if(inwards){
+        DCC_send_command(DCCaddress_decoder, DCCswitch2, 1);
+        pc.printf("SW 2 inward!\n\r");
+    } else {
+        DCC_send_command(DCCaddress_decoder, DCCswitch2, 1);
+        DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
+        pc.printf("SW 2 outward!\n\r");        
+    }    
+}
+
+void changeSwitch3(bool inwards){
+    if(inwards){
+        DCC_send_command(DCCaddress_decoder, DCCswitch3, 1);
+        DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);  //switch 3 has reverse behavior
+        pc.printf("SW 3 inward!\n\r");
+    } else {
+        DCC_send_command(DCCaddress_decoder, DCCswitch3, 1);
+        pc.printf("SW 3 outward!\n\r");
+    }    
+}
+
+void changeSwitch4(bool inwards){
+    if(inwards){
+        DCC_send_command(DCCaddress_decoder, DCCswitch4, 1);
+        pc.printf("SW 4 inward!\n\r");
+    } else {
+        DCC_send_command(DCCaddress_decoder, DCCswitch4, 1);
+        DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
+        pc.printf("SW 4 outward!\n\r");        
+    }    
+}
+
+unsigned int trainInstruction(bool direction, bool light, int speed){
+    unsigned int result = 0x40; //0100 0000
+    if(direction){
+        result += 0x20;  //0010 0000
+    } 
+    if(light){
+        result += 0x10;  //0001 0000
+    }
+    if(speed>0 && speed<15){
+        result += speed + 1;   
+    }
+    return result;   
+}
+
+void stationReached(int station){
+    /*DCC_send_command(DCCaddress_train2, DCCinst_estop, 2);
+    
+    lcd.cls();
+    lcd.printf("Station %d reached!\n", station);
+    pc.printf("Station %d reached!\n\r", station);
+    wait(5);
+    direction = switch1; //TODO
+    speed = getSpeed();
+    light = true; //TODO
+    DCC_send_command(DCCaddress_train2, trainInstruction(direction, light, speed), 2);
+    */
+    
+    lcd.cls();
+    lcd.printf("Station %d reached!\n", station);
+    pc.printf("Station %d reached!\n\r", station);
+}
+
+
+
+
+
+//INTERPRET INTERRUPT SIGNALS
+//INTERRUPTS
+//code from the tutorial, initializes MCP and sets it up for interrupts
+void init_mcp() {
+    // Initialisation of MCP registers, documentation on registers is available at
+    //Niels/Abel/Robert/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);
+
+    // Clear current interrupts
+    mcp->_read(GPIOA);
+    mcp->_read(GPIOB);
+    // Register callbacks
+    int0.fall(&on_int0_change);
+    int1.fall(&on_int1_change);
+    // Enable interrupts on the MCP
+    mcp->_write(GPINTENA, (unsigned char )0xff);
+    mcp->_write(GPINTENB, (unsigned char )0xff);
+    
+    int0flag = false;
+    int1flag = false;
+}
+
+void on_int0_change() {
+    int0flag = true;
+}
+
+void on_int1_change() {
+    int1flag = true;
+}
+
+std::list<int> circuit0markers(int sensor_data){
+    std::list<int> result;
+    sensor_data = ~sensor_data;
+    if((sensor_data & 0b00000001) == 0b00000001){
+        result.push_back(0);
+    }
+    if((sensor_data & 0b00000010) == 0b00000010){
+        result.push_back(1);
+    }
+    if((sensor_data & 0b00000100) == 0b00000100){
+        result.push_back(2);
+    }
+    if((sensor_data & 0b00001000) == 0b00001000){
+        result.push_back(3);
+    }
+    if((sensor_data & 0b00010000) == 0b00010000){
+        result.push_back(4);
+    }
+    if((sensor_data & 0b00100000) == 0b00100000){
+        result.push_back(5);
+    }
+    if((sensor_data & 0b01000000) == 0b01000000){
+        result.push_back(6);
+    }
+    if((sensor_data & 0b10000000) == 0b10000000){
+        result.push_back(7);
+    }
+    return result;
+}    
+    
+std::list<int> circuit1markers(int sensor_data){
+    std::list<int> result;
+    sensor_data = ~sensor_data;
+    if((sensor_data & 0b00000001) == 0b00000001){
+        result.push_back(8);
+    }
+    if((sensor_data & 0b00000010) == 0b00000010){
+        result.push_back(9);
+    }
+    if((sensor_data & 0b00000100) == 0b00000100){
+        result.push_back(10);
+    }
+    if((sensor_data & 0b00001000) == 0b00001000){
+        result.push_back(11);
+    }
+    if((sensor_data & 0b00010000) == 0b00010000){
+        result.push_back(12);
+    }
+    if((sensor_data & 0b00100000) == 0b00100000){
+        result.push_back(13);
+    }
+    if((sensor_data & 0b01000000) == 0b01000000){
+        result.push_back(21);
+    }
+    if((sensor_data & 0b10000000) == 0b10000000){
+        result.push_back(22);
+    }
+    return result;
+}
+
+
+
+
+
+//LOG OUTPUT
+void displayLCDstatus(){
+    lcd.cls();
+    lcd.printf("T1: %d \n T2: %d", speed1, speed2);   
+}
+
+void displayTerminalStatus(){
+    string dir1Text = dir1?"forward":"reverse";
+    string dir2Text = dir2?"forward":"reverse";
+    string light1Text = light1?"on, ":"off,";
+    string light2Text = light2?"on, ":"off,";
+    pc.printf("Iteration %d\n\r", iter);
+    pc.printf("T1: dir %s, light %s spd %d\n\rT2: dir %s, light %s spd %d\n\r", dir1Text.c_str(), light1Text.c_str(), speed1, dir2Text.c_str(), light2Text.c_str(), speed2);   
+}
\ No newline at end of file