Denver
Denver trai project
main.cpp
- Committer:
- carlosperales95
- Date:
- 2018-06-26
- Revision:
- 68:9859a3ff9298
- Parent:
- 67:7ff2425141ce
- Child:
- 69:609c69e91b4e
File content as of revision 68:9859a3ff9298:
#include "mbed.h"
#include "TextLCD.h"
#include "MCP23017.h"
#include <string>
#include <iostream>
#include <vector>
#include <fstream>
#include <sstream>
using namespace std;
//FileSystem for log creation
LocalFileSystem local("local");
/******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
//ENABLE - p17
DigitalOut enable(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
//......SENSOR POSITION VARS
//Definition of D sensors, will be interpreted as ints for the program's logic
#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
//Definition of the different train speeds, will be interpreted as ints for the program's logic
#define STOP 0
#define SLOW 1
#define MEDIUM 2
#define FAST 3
#define FULL 4
#define R_MEDIUM 5
/**
*
*Position class.
*
*@position -
*@previous_cw -
*@previous_ccw -
*
*Position(int) -
*
*get_pos() -
*get_prev_cw() -
*get_ccw() -
*add_prev_cw() -
*add_ccw() -
*
**/
class Position{
private:
int position;
vector <int> previous_cw;
vector <int> previous_ccw;
public:
Position(int p){
position = p;
}
int get_pos(){
return position;
}
vector<int> get_next_cw(){
return previous_ccw;
}
vector<int> get_next_ccw(){
return previous_cw;
}
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);
};
};
//Creating a vector with all the positions.
vector<Position> positions;
/**
*
*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++;
}
}
/**
*Defining areas for train detection and collision logic.
*area_A_arr/area_B_arr - Arrays that hold the Dsensors for each area, used to initialize the vectors.
*area_A/area_B - Vectors that hold the different sensors of the corresponding areas of the track.
**/
int area_A_arr[] = {D21,D2,D22,D1,D0,D13,D12};
int area_B_arr[] = {D6,D7,D8};
const vector<int> area_A(area_A_arr,area_A_arr + sizeof(area_A_arr) / sizeof(int));
const vector<int> area_B(area_B_arr,area_B_arr + sizeof(area_B_arr) / sizeof(int));
/**
*
*Train class.
*
*@position -
*@going_cw -
*
*Train(int, bool) -
*Train(bool) -
*
*Vector get_next_sensors() -
*set_position(int) -
*set_goes_cw(bool) -
*Position get_position() -
*Int get_position_number() -
*Bool goes_cw() -
*
**/
class Train{
private:
unsigned int train_address; //stop the train
Position *position;
bool going_cw;
int speed;
public:
Train(int pos, bool cw){
position = &positions[pos];
going_cw = cw;
}
/**
* Contructor that takes the address of the train and the speed with default value MEDIUM.
*/
Train(unsigned int address, int s=MEDIUM){
train_address = address;
speed = s;
}
Train(bool cw){ going_cw = cw; }
vector<int> get_next_sensors(){
//Checking direction
if(going_cw){
return position->get_next_cw();
}else{
return position->get_next_ccw();
}
}
void set_speed(int s){
speed = s;
}
/**
* Sends a DCC command to the train with the speed indicaed by the attribute speed
* The number of times the command is sent can be indicated as an optional parameter. Default value is 1.
*/
void run(int times=1){
const unsigned int DCCinst_forward_slow = 0x66; //forward slow speed (step 9)
const unsigned int DCCinst_forward_medium = 0x68; //forward half speed
const unsigned int DCCinst_forward_fast = 0x6C; //Forward fast speed (step 22)
const unsigned int DCCinst_forward_full = 0x6F; //Forward full speed
const unsigned int DCCinst_reverse_medium = 0x48; //reverse half speed
const unsigned int DCCinst_stop = 0x50; //stop the train
switch(speed){
case STOP:
DCC_send_command(train_address, DCCinst_stop,times);
break;
case SLOW:
DCC_send_command(train_address, DCCinst_forward_slow,times);
break;
case MEDIUM:
DCC_send_command(train_address, DCCinst_forward_medium,times);
break;
case FAST:
DCC_send_command(train_address, DCCinst_forward_fast,times);
break;
case FULL:
DCC_send_command(train_address, DCCinst_forward_full,times);
break;
case R_MEDIUM:
DCC_send_command(train_address, DCCinst_reverse_medium,times);
break;
}
}
void set_position(int pos){
position = &positions[pos]; //Taking the new position from the positions vector
}
void set_goes_cw(bool cw){
going_cw = cw;
}
Position get_position(){
return *position;
}
int get_position_number(){
return position->get_pos();
}
bool goes_cw(){
return going_cw;
}
/**
*
*Checks if the element exists within the vector.
*
*@v - The vector (of ints) the method will go through.
*@element - The element the method will look for.
*
**/
bool in_vector(vector<int>v,int element){
bool exist = false;
for(int i=0; i< v.size(); i++){
if(v[i] == element){
exist = true;
}
}
return exist;
}
bool is_in_A(){
return in_vector(area_A,get_position_number());
}
bool is_in_B(){
return in_vector(area_B,get_position_number());
}
};
//Creation of all the positions. One for every sensor on the table - Position name(mapping)
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);
//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 slow speed (step 9)
const unsigned int DCCinst_forward_fast = 0x6C; //Forward fast speed (step 22)
const unsigned int DCCinst_forward_full = 0x6F; //Forward full 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
//.....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
/**
*Creation of 2 Train objects.
*Using boolean constructor because position initialization will be done after initializing all position vectors.
*DR_train = Dark Red train - LR_train = Light Red Train
**/
Train DR_train(DCCaddressDR,MEDIUM);
Train LR_train(DCCaddressLR,MEDIUM);
//possibility of an array having {dr_train, lr_train}? for reuse and modularity of functions
//****** SYSTEM LOGGER FUNCTIONS ******//
/**
*
*
*
**/
string train_dir_toString(Train* train){
string dir_string;
if(train->goes_cw()){
dir_string = "cw";
}else{
dir_string = "ccw";
}
return dir_string;
}
/**
*
*
*
**/
string train_pos_toString(Train* train){
int i = train->get_position_number();
std::string tr_pos;
std::stringstream ss;
ss << i;
tr_pos = ss.str();
return tr_pos;
}
/**
*
*
*
**/
void logger_add(string s){
FILE *fp = fopen("/local/sys_log.txt", "a");
fprintf(fp, "%s \n", s);
fclose(fp);
}
/**
*
*
*
**/
void init_logger(){
FILE *fp = fopen("/local/sys_log.txt", "w");
fprintf(fp, "Log for the main train program: \n");
fprintf(fp, "------------------------------- \n \n");
fclose(fp);
}
/**
*
*
*
**/
void train_pos0_log(){
FILE *fp = fopen("/local/sys_log.txt", "a");
fprintf(fp, "DR_train START: D%s sensor, %s direction. \n", train_pos_toString(&DR_train), train_dir_toString(&DR_train));
fprintf(fp, "LR_train START: D%s sensor, %s direction. \n \n", train_pos_toString(&LR_train), train_dir_toString(&LR_train));
fprintf(fp, "..............Program Start................ \n");
fclose(fp);
}
/**
*
*
*
**/
void print_next_sensors(Train* train){
FILE *fp = fopen("/local/sys_log.txt", "a");
for(int i=0; i<train->get_next_sensors().size(); i++){
fprintf(fp, "%d" ,train->get_next_sensors()[i]);
}
fprintf(fp, "\n");
fclose(fp);
}
/**
*
*
*
**/
void trains_posn_log(int sensor){
FILE *fp = fopen("/local/sys_log.txt", "a");
fprintf(fp, "SIGNAL FROM SENSOR D%d \n", sensor);
fprintf(fp, "DR_train: D%s sensor, %s direction. // Next positions: ", train_pos_toString(&DR_train), train_dir_toString(&DR_train));
print_next_sensors(&DR_train);
fprintf(fp, "LR_train: D%s sensor, %s direction. // Next positions: ", train_pos_toString(&LR_train), train_dir_toString(&LR_train));
print_next_sensors(&LR_train);
fprintf(fp, "\n");
fclose(fp);
}
//**************** FUNCTIONS FOR DENVER TRAIN ****************//
/**
*
*Activates the buzzer for 0.5 seconds.
*
**/
void doBuzz(){
buzz = 1;
wait(0.5);
buzz = 0;
}
/**
*
*Initializes every position's vectors (prev_cw and prev_ccw) with the corresponding sensors.
*prev_cw - Sensors previous to the current in clockwise sense.
*prev_ccw - Sensors previous to the current in counter-clockwise sense.
*
**/
void init_positions(){
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(D9);
d8.add_prev_cw(D10);
d8.add_prev_ccw(D7);
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);
//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);
}
/**
*
*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.
*
*@number -
*@interrupt -
*
**/
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;
}
/**
*
*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 flip_switch(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 = SWBidle; //FLIP SW3
break;
case 4:
SWBflip = SWBflip_4; //FLIP SW4
break;
default:
break;
}
//Security measure not to burn the switch.
DCC_send_command(SWBaddress,SWBflip,times); //Activating switchç
/*
if(switchId == 3){
DCC_send_command(SWBaddress,SWBidle,times); //Activating switch
}
*/
if(!activate){
if(switchId == 3){
DCC_send_command(SWBaddress,SWBflip_3,times); // Exception for switch 3
}else{
DCC_send_command(SWBaddress,SWBidle,times);
}
}
}
/**
* Action to do when NAC is detected
* Booster is disabled and the buzz is activated
*
*/
void NAC_action(){
enable = 0;
doBuzz();
}
/**
*This method will check if there is a non-avoidable frontal collision(NAFC).
*A NAFC will happen if:
*
*Both trains in area A or B with different direction
*Trains in (D11 and D5) or (D9 and D3) with same direction
*
*/
bool check_NAC(){
bool NAC = false;
if((DR_train.is_in_A() && LR_train.is_in_A()) || (DR_train.is_in_B() && LR_train.is_in_B()) ){ //Check if both are in same area
if(DR_train.goes_cw() ^ LR_train.goes_cw()){ //XOR: They must have different values to be true (Different direction)
lcd.cls();
lcd.printf("NAC Both area A or B");
NAC = true;
}
}else if(((DR_train.get_position_number() == D11) && (LR_train.get_position_number() == D5 ) ) || ((LR_train.get_position_number() == D5) && (DR_train.get_position_number() == D11 ))){ //Check if they are in position D11 and D5
if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction)
lcd.cls();
lcd.printf("NAC D11 and D5");
NAC = true;
}
}else if(((DR_train.get_position_number() == D9) && (LR_train.get_position_number() == D3)) || ((LR_train.get_position_number() == D9) && (DR_train.get_position_number() == D3)) ){//Check if they are in position D9 and D3
if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction)
lcd.cls();
lcd.printf("NAC D9 and D3");
NAC = true;
}
}
return NAC;
}
/**
* Switch_n switch that needs to switch
* cont_sensor sensor that when activated the stopped train continues
* switch_sensor sensor where the switch should be activated
*/
void AFC_action(int switch_n, int cont_sensor, int switch_sensor, Train *stop_train, Train * cont_train ){
bool send_pack_switch = false;
//flip_switch(switch_n,5);
if(switch_n == 3){
DCC_send_command(SWBaddress,SWBflip_3,15); //Activating switchç
}
while(cont_train->get_position_number() != cont_sensor){
if(cont_train->get_position_number() == switch_sensor){
send_pack_switch = true;
}
stop_train->set_speed(STOP);
stop_train->run(); //Stopping train on sensor D4 or D10
cont_train->run();
if(send_pack_switch){
lcd.cls();
lcd.printf("Switching SW%d",switch_n);
flip_switch(switch_n,5);
}
}
if(switch_n == 3){
DCC_send_command(SWBaddress,SWBflip_3,15); //Activating switch
}else{
flip_switch(5,5); //Send IDLE command
}
stop_train->set_speed(MEDIUM);
}
/**
* Switch_n switch that needs to switch
* cont_sensor sensor that when activated the stopped train continues
* switch_sensor sensor where the switch should be activated
*/
void ALC_action(int cont_sensor, Train *stop_train, Train * cont_train ){
//flip_switch(switch_n,5);
while(cont_train->get_position_number() != cont_sensor){
stop_train->set_speed(STOP);
stop_train->run(); //Stopping train on sensor D4 or D10
cont_train->run();
}
stop_train->set_speed(MEDIUM);
}
/**
*
*The function will check if there is an Avoidable Frontal Collision (AFC).
*AFC will occur if:
*
*Train in area A(ccw) and train in D4(cw)
*Train in area A(cw) and train in D10(ccw)
*Train in area B(cw) and train in D4(ccw)
*Train in area B(ccw) and train in D10(ccw)
*
*stop_train is the train that is going to stop in sensors D4 or D10 until the other train passes
*cont_train is the train that won't stop and will do the switch
*
**/
bool check_AFC(Train *stop_train, Train *cont_train){ //TODO - Add same for LR train
bool detected_AFC = false;
if( stop_train->get_position_number() == D4){
if(stop_train->goes_cw()){
if(cont_train->is_in_A() && !cont_train->goes_cw()){
lcd.cls();
lcd.printf("AFC!!! STOP D4 SW2 CONT D3");
AFC_action(2,D3,D2,stop_train,cont_train);
//Activate switch2
//When cont_train is at D3 stop_train continues
detected_AFC = true;
}
}else{ //DR goes ccw
if(cont_train->is_in_B() && cont_train->goes_cw()){
lcd.cls();
lcd.printf("AFC!!! STOP D4 SW3 CONT D5");
AFC_action(3,D5,D6,stop_train,cont_train);
//DR_train stops
//Activate switch3
//When LR is at D5 DR continues
detected_AFC = true;
}
}
}else if(stop_train->get_position_number() == D10){
if(stop_train->goes_cw()){
if(cont_train->is_in_B() && !cont_train->goes_cw()){
lcd.cls();
lcd.printf("AFC!!! STOP D10 SW4 CONT D9");
AFC_action(4,D9,D8,stop_train,cont_train);
//DR train stops
//Activate switch4
//When LR is at D9 DR continues
detected_AFC = true;
}
}else{
if(cont_train->is_in_A() && cont_train->goes_cw()){
lcd.cls();
lcd.printf("AFC!!! STOP D10 SW1 CONT D11");
AFC_action(1,D11,D12,stop_train,cont_train);
//DR train stops
//Activate switch1
//When LR is at D11 DR continues
detected_AFC = true;
}
}
}else if(stop_train->get_position_number() == D9){
if(stop_train->goes_cw()){
if(cont_train->is_in_B() && !cont_train->goes_cw()){
lcd.cls();
lcd.printf("AFC!!! STOP D9 CONT D8");
AFC_action(5, D10, D4, stop_train, cont_train);
//train in 9 stops
//when cont_train is at d10 stop train continues
detected_AFC = true;
}
}
}else if(stop_train->get_position_number() == D11){
if(!stop_train->goes_cw()){
if(cont_train->is_in_A() && cont_train->goes_cw()){
lcd.cls();
lcd.printf("AFC!!! STOP D11 CONT D12");
AFC_action(5, D10, D4, stop_train, cont_train);
//train in 11 stops
//when cont_train is at d10 stop train continues
detected_AFC = true;
}
}
}else if(stop_train->get_position_number() == D3){
if(stop_train->goes_cw()){
if(cont_train->is_in_A() && !cont_train->goes_cw()){
lcd.cls();
lcd.printf("AFC!!! STOP D3 CONT D14");
AFC_action(5, D4, D10, stop_train, cont_train);
//train in 3 stops
//when cont_train is at d4 stop train continues
detected_AFC = true;
}
}
}else if(stop_train->get_position_number() == D5){
if(!stop_train->goes_cw()){
if(cont_train->is_in_B() && cont_train->goes_cw()){
lcd.cls();
lcd.printf("AFC!!! STOP D5 CONT D6");
AFC_action(5, D4, D10, stop_train, cont_train);
//train in 5 stops
//when cont_train is at d4 stop train continues
detected_AFC = true;
}
}
}
return detected_AFC;
}
/**
*
*
*
**/
bool check_ALC(Train *stop_train, Train *cont_train){ //TODO - Add same for LR train
bool detected_ALC = false;
if( stop_train->get_position_number() == D4){
if(stop_train->goes_cw()){
if(cont_train->get_position_number() == D3 && cont_train->goes_cw()){
lcd.cls();
lcd.printf("ALC!!! STOP D4 CONT D3");
ALC_action(2, stop_train, cont_train );
//When cont_train is at D22 stop_train continues
detected_ALC = true;
}
}else{ //DR goes ccw
if(cont_train->get_position_number() == D5 && !cont_train->goes_cw()){
lcd.cls();
lcd.printf("ALC!!! STOP D4 SW3 CONT D5");
ALC_action(7, stop_train, cont_train );
//Train stops
//When CONT is at D6 DR continues
detected_ALC = true;
}
}
}else if(stop_train->get_position_number() == D10){
if(stop_train->goes_cw()){
if(cont_train->get_position_number() == D9 && cont_train->goes_cw()){
lcd.cls();
lcd.printf("ALC!!! STOP D10 CONT D9");
ALC_action(8, stop_train, cont_train );
//D10 train stops
//When CONT is at D8, D10 continues
detected_ALC = true;
}
}else{
if(cont_train->get_position_number() == D11 && !cont_train->goes_cw()){
lcd.cls();
lcd.printf("ALC!!! STOP D10 CONT D11");
ALC_action(12, stop_train, cont_train );
//D10 train stops
//When CONT is at D12, D10 continues
detected_ALC = true;
}
}
}
return detected_ALC;
}
/**
*
*The method check_position will check if any of the trains is in any of the areas.
*It will go through all the area vectors (A,B) and call the function in_vector to check inside the vectors.
*
**/
void check_position(){
if(check_NAC()){
//lcd.cls();
//lcd.printf("NAC!!!");
NAC_action();
}
check_AFC(&DR_train,&LR_train);
check_AFC(&LR_train,&DR_train);
check_ALC(&LR_train,&DR_train);
check_ALC(&DR_train,&LR_train);
}
/**
*
*Description
*
*@sensor -
*
**/
void update_train_pos(int sensor){
led2 = 1;
logger_add("UPDATE IT");
bool found_DR = false;
bool found_LR = false;
string DR_dir,LR_dir;
if(DR_train.goes_cw()){
DR_dir = "cw";
}else{
DR_dir = "ccw";
}
if(LR_train.goes_cw()){
LR_dir = "cw";
}else{
LR_dir = "ccw";
}
//wait(0.7);
if(sensor == DR_train.get_position_number() || sensor == LR_train.get_position_number()){
led2 = 0;
}else{
//trains_posn_log(sensor);
lcd.cls();
lcd.printf("S:D%d DR%d(",sensor,DR_train.get_position_number());
//TODO: Do a for to print all next sensors.
for(int i=0; i<DR_train.get_next_sensors().size(); i++){
lcd.printf("%d,",DR_train.get_next_sensors()[i]);
}
lcd.printf(")%s LR%d(",DR_dir,LR_train.get_position_number());
for(int i=0; i<LR_train.get_next_sensors().size(); i++){
lcd.printf("%d,",LR_train.get_next_sensors()[i]);
}
lcd.printf(")%s",LR_dir);
//Checking next sensors for DR train
for(int i=0; i<DR_train.get_next_sensors().size(); i++){
if(DR_train.get_next_sensors()[i] == sensor){ //If the sensor is one expected to visit by the train we update the position
found_DR = true;
if(DR_train.goes_cw()){
if(DR_train.get_position_number() == D5 || DR_train.get_position_number() == D11){
DR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
}
}else{
if(DR_train.get_position_number() == D9 || DR_train.get_position_number() == D3){
DR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
}
}
DR_train.set_position(sensor);
}
}
//Checking next sensors for LR train
for(int i=0; i<LR_train.get_next_sensors().size(); i++){
if(LR_train.get_next_sensors()[i] == sensor){
found_LR = true;
if(LR_train.goes_cw()){
if(LR_train.get_position_number() == D5 || LR_train.get_position_number() == D11){
LR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
}
}else{
if(LR_train.get_position_number() == D9 || LR_train.get_position_number() == D3 ){
LR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
}
}
LR_train.set_position(sensor);
}
}
/*
if(found_DR){
//doBuzz();
lcd.cls();
lcd.printf("DR is at D%d",DR_train.get_position_number());
}
if(found_LR){
lcd.cls();
lcd.printf("LR is at D%d",LR_train.get_position_number());
}
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);
}
/**
*
*Clear current interrupts
*
**/
void init() {
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!
}
/**
*
*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");
flip_switch(1,5);
}else if(switch2 == 1){
lcd.cls();
lcd.printf("Switch 2 ON - SW2");
flip_switch(2,5);
}else if(switch3 == 0){
lcd.cls();
lcd.printf("Switch 3 ON - SW3");
flip_switch(3,5);
}else if(switch4 == 0){
lcd.cls();
lcd.printf("Switch 4 ON - IDLE");
flip_switch(0,5);
}
}
/**
* Returns a sensor number depending on how many times switch3 flips.
* When pressing switch4 it confirms the switch
* Init_sensor is the value where we start counting.
* string train is the name of the train that will be prited with the sensor
*/
int select_sensor(int init_sensor, string train){
lcd.cls();
lcd.printf("%s SENSOR D%d",train,init_sensor);
int sensor = init_sensor;
bool changed = false;
bool exit = false;
while(!exit){
if(switch3 == 0){
if(changed){
sensor++;
sensor=sensor%15; //Only sensors from 0 to 15.
changed=false;
lcd.cls();
lcd.printf("%s: D%d",train,sensor);
}
}else{
changed = true;
wait(0.2);
}
if(switch4 == 0){
exit = true;
wait(0.2);
}
}
return sensor;
}
/**
* Returns a boolean representing the direction. Everytimew switch3 is 0 it changes the direction.
* When switch4 is 0 the selection is confirmed.
* Init_going_cw is the initial direction.
* Train is the string with the name of the train that will be printed next to the direction
*/
bool select_direction(bool init_going_cw,string train){
string dir_string;
if(init_going_cw){
dir_string = "cw";
}else{
dir_string = "ccw";
}
lcd.cls();
lcd.printf("%s DIRECTION %s ",train,dir_string);
bool exit = false;
bool going_cw = init_going_cw;
bool changed = false;
while(!exit){
if(switch3 == 0){
if(changed){
going_cw = !going_cw;
changed = false;
string dir;
if(going_cw){
dir = "cw";
}else{
dir = "ccw";
}
lcd.cls();
lcd.printf("%s: %s",train,dir);
}
}else{
changed = true;
wait(0.2);
}
if(switch4 == 0){
exit = true;
wait(0.2);
}
}
return going_cw;
}
//**************** 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
enable = 0;
//Led routine to start main program
led1 = 1;
wait(0.2);
led1 = 0;
wait(0.2);
led1 = 1;
init_positions();
initialize_mcp(); //mcp initialization for interrupts before train running
init();
int DR_init_sensor = select_sensor(D6,"DR");
bool DR_init_dir = select_direction(true,"DR");
wait(0.5);
int LR_init_sensor = select_sensor(D3,"LR");
bool LR_init_dir = select_direction(true,"LR");
DR_train.set_position(DR_init_sensor);
DR_train.set_goes_cw(DR_init_dir);
LR_train.set_position(LR_init_sensor);
LR_train.set_goes_cw(LR_init_dir);
string DR_print_dir, LR_print_dir;
if(DR_train.goes_cw()){
DR_print_dir = "cw";
}else{
DR_print_dir = "ccw";
}
if(LR_train.goes_cw()){
LR_print_dir = "cw";
}else{
LR_print_dir = "ccw";
}
lcd.cls();
lcd.printf("DR(%d)%s \n LR(%d)%s",DR_train.get_position_number(),DR_print_dir,LR_train.get_position_number(),LR_print_dir);
wait(2);
//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");
wait(1);
init_logger();
train_pos0_log();
logger_add("There we go lads this is the start of the program");
enable = 1;
/*
flip_switch(5,15);//Send IDLE command at the beginning
flip_switch(1,40);
wait(0.5);
flip_switch(5,15);
flip_switch(2,15);
wait(0.5);
flip_switch(5,15);
flip_switch(3,40);
wait(0.5);
flip_switch(5,15);
flip_switch(4,15);
wait(0.5);
flip_switch(5,15);
*/
//Demo for stopping at the station
while(1) {
checkSwitch(); //Checks for switch commands everytime.
check_position();
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{
DR_train.run();
LR_train.run();
}
}
}