File content as of revision 31:9d973398554f:
#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,14,15};
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 endFunction();
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;
}
}
endFunction();
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;
}
void endFunction(){
lcd.printf("Shutting down...");
bool in = true;
while(in){
redTrain.sendCommand();
silverTrain.sendCommand();
externalLed2 = 1;
// wait(0.2);
//externalLed2 = 0;
buzz = 1;
if(!readSwitch(sw4)){
in = false;
}
}
externalLed2 = 0;
buzz = 0;
}
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();
endFunction();
}
}
void executeCase(int pos, Train* currTrain){
lcd.cls();
lcd.printf("At ");
lcd.printf("%d", pos);
switch(pos){
case 0:{
currTrain->changeSpeed(currTrain->normalSpeed());
break;}
case 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->slowlySpeed());
break;}
case 2:{
/*
Stop everytime 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:{
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->slowlySpeed()); //go forward
}
else{
currTrain->Stop();
}
break;
}
case 4:{
dect.clearReservation(C_left); //free nr 0,1,2,12,13
switch3.switchOn(); //Turn on switch3
dect.showReservation();
if(currTrain->isClockwise()){
lcd.printf("Switch 3 not activated");
redTrain.Stop();
silverTrain.Stop();
}
else 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:{
dect.makeReservation(junction_511);
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:{
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:{
currTrain->changeSpeed(currTrain->slowlySpeed());
break;}
case 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:{
if(!currTrain->isClockwise()){
lcd.printf("Switch 4 is activated");
redTrain.Stop();
silverTrain.Stop();
}
else 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:{
dect.makeReservation(lane_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:{
dect.makeReservation(junction_511);
dect.showReservation();
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: {
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);
dect.makeReservation(C_left);
currTrain->changeSpeed(currTrain->normalSpeed()); //Move forward
break;
}
case 13: {
currTrain->changeSpeed(currTrain->normalSpeed());
break;
}
case 14:{
currTrain->changeSpeed(currTrain->slowlySpeed());
break;
}
case 15:{
currTrain->changeSpeed(currTrain->slowlySpeed());
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");
return;
}
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();
}
Jasmine Karlsson
