denver_train_proj - Denver trai project

Denver » Code » denver_train_proj

Denver / Mbed 2 deprecated denver_train_proj

Denver trai project

Dependencies: mbed TextLCD

main.cpp@75:bae748a7905f, 2018-06-27 (annotated)

Committer:: carlosperales95
Date:: Wed Jun 27 13:13:07 2018 +0000
Revision:: 75:bae748a7905f
Parent:: 74:dbbc528f2b18

Last all implemented testing build

Who changed what in which revision?

User	Revision	Line number	New contents of line
mglmx	0:4d06a6a8e785	1	#include "mbed.h"
mglmx	3:fe7010b693a0	2	#include "TextLCD.h"
mglmx	18:aa43bb62e60f	3	#include "MCP23017.h"
mglmx	22:e4153ca757dd	4	#include <string>
mglmx	22:e4153ca757dd	5	#include <iostream>
mglmx	35:cfcfeccb959e	6	#include <vector>
mglmx	62:6fc0c683cb02	7	#include <fstream>
mglmx	62:6fc0c683cb02	8	#include <sstream>
mglmx	35:cfcfeccb959e	9
mglmx	35:cfcfeccb959e	10	using namespace std;
mglmx	18:aa43bb62e60f	11
carlosperales95	65:8fd4046c6540	12
carlosperales95	7:e2b8461d4f05	13	/****PINS AND DECLARATIONS*****/
carlosperales95	7:e2b8461d4f05	14
carlosperales95	25:a42a1ed4d8e9	15	//------PINS
mglmx	21:e6f1649add39	16
mglmx	21:e6f1649add39	17	//SWITCHES p5 - p8
mglmx	21:e6f1649add39	18	DigitalIn switch1(p5);
mglmx	21:e6f1649add39	19	DigitalIn switch2(p6);
mglmx	21:e6f1649add39	20	DigitalIn switch3(p7);
mglmx	21:e6f1649add39	21	DigitalIn switch4(p8);
carlosperales95	7:e2b8461d4f05	22
carlosperales95	10:2088b1935a93	23	//RAIL SENSORS - INT0,INT1
carlosperales95	7:e2b8461d4f05	24	//INT0 - p9
mglmx	18:aa43bb62e60f	25	InterruptIn int0(p9);
carlosperales95	7:e2b8461d4f05	26	//INT1 - p10
mglmx	18:aa43bb62e60f	27	InterruptIn int1(p10);
carlosperales95	7:e2b8461d4f05	28
carlosperales95	75:bae748a7905f	29	///p11 - EMPTY
carlosperales95	75:bae748a7905f	30	///p12 - EMPTY
carlosperales95	12:e914ca5cd44b	31
carlosperales95	7:e2b8461d4f05	32	//M0 - p13
mglmx	35:cfcfeccb959e	33	DigitalIn d21stat(p13); //Sensor right of the station
carlosperales95	7:e2b8461d4f05	34	//M1 - p14
mglmx	35:cfcfeccb959e	35	DigitalIn d22stat(p14); //Sensor left of the station
carlosperales95	7:e2b8461d4f05	36	//M2 - p15
carlosperales95	29:559eb2164488	37	DigitalIn station(p15); //Sensor in the middle of the station
carlosperales95	12:e914ca5cd44b	38
carlosperales95	75:bae748a7905f	39	//p16 - EMPTY
carlosperales95	75:bae748a7905f	40
carlosperales95	50:ee4398ee44be	41	//ENABLE - p17
carlosperales95	50:ee4398ee44be	42	DigitalOut enable(p17);
carlosperales95	12:e914ca5cd44b	43
carlosperales95	11:021210c59a95	44	//BUZZER - p18
carlosperales95	11:021210c59a95	45	DigitalOut buzz(p18); // buzz=0 doesn't beep, buzz=1 beeps
carlosperales95	7:e2b8461d4f05	46
carlosperales95	7:e2b8461d4f05	47	//POTENTIOMETER - p19
carlosperales95	13:dbf1ead12cee	48	AnalogIn pot(p19); //Gives float value pot.read(). Convert analog input to V with f*3.3
carlosperales95	7:e2b8461d4f05	49
carlosperales95	7:e2b8461d4f05	50	//DAT - p20
mglmx	3:fe7010b693a0	51	DigitalOut Track(p20); //Digital output bit used to drive track power via H-bridge
carlosperales95	7:e2b8461d4f05	52
carlosperales95	7:e2b8461d4f05	53	//LCD SCREEN - p21, p22, p23, p24, p25, p26
carlosperales95	11:021210c59a95	54	TextLCD lcd(p22,p21,p23,p24,p25,p26); // RS, E, A4, A5, A6, A7 // ldc.cls() to clear and printf(String up to 16char)
carlosperales95	7:e2b8461d4f05	55
carlosperales95	75:bae748a7905f	56	//p27 - EMPTY
carlosperales95	75:bae748a7905f	57
carlosperales95	75:bae748a7905f	58	//12C - p28
mglmx	33:24ce12dec157	59	I2C i2c(p28,p27);
carlosperales95	7:e2b8461d4f05	60
carlosperales95	7:e2b8461d4f05	61	//LED1 - p29
mglmx	3:fe7010b693a0	62	DigitalOut redled(p29);
carlosperales95	7:e2b8461d4f05	63	//LED2 - p30
mglmx	3:fe7010b693a0	64	DigitalOut greenled(p30);
carlosperales95	7:e2b8461d4f05	65
carlosperales95	24:1d71dd8778c4	66	//MBED LEDS
mglmx	18:aa43bb62e60f	67	DigitalOut led1(LED1);
mglmx	18:aa43bb62e60f	68	DigitalOut led2(LED2);
mglmx	18:aa43bb62e60f	69	DigitalOut led3(LED3);
carlosperales95	69:609c69e91b4e	70	DigitalOut led4(LED4);
mglmx	16:2a2da0e67793	71
mglmx	16:2a2da0e67793	72	//MCP
mglmx	16:2a2da0e67793	73	MCP23017 *mcp;
carlosperales95	12:e914ca5cd44b	74
carlosperales95	65:8fd4046c6540	75
carlosperales95	74:dbbc528f2b18	76	//------DEFINITIONS
carlosperales95	25:a42a1ed4d8e9	77
carlosperales95	74:dbbc528f2b18	78	//......SENSOR DEFS
carlosperales95	45:d589318238bf	79	//Definition of D sensors, will be interpreted as ints for the program's logic
mglmx	34:c9ab2a987734	80	#define D0 0
mglmx	34:c9ab2a987734	81	#define D1 1
mglmx	34:c9ab2a987734	82	#define D2 2
mglmx	34:c9ab2a987734	83	#define D3 3
mglmx	34:c9ab2a987734	84	#define D4 4
mglmx	34:c9ab2a987734	85	#define D5 5
mglmx	34:c9ab2a987734	86	#define D6 6
mglmx	34:c9ab2a987734	87	#define D7 7
mglmx	34:c9ab2a987734	88	#define D8 8
mglmx	34:c9ab2a987734	89	#define D9 9
mglmx	34:c9ab2a987734	90	#define D10 10
mglmx	34:c9ab2a987734	91	#define D11 11
mglmx	34:c9ab2a987734	92	#define D12 12
mglmx	34:c9ab2a987734	93	#define D13 13
mglmx	34:c9ab2a987734	94	#define D21 14
mglmx	34:c9ab2a987734	95	#define D22 15
mglmx	34:c9ab2a987734	96
carlosperales95	74:dbbc528f2b18	97	//......SPEED DEFS
carlosperales95	65:8fd4046c6540	98	//Definition of the different train speeds, will be interpreted as ints for the program's logic
mglmx	57:ee5da8a011e0	99	#define STOP 0
mglmx	57:ee5da8a011e0	100	#define SLOW 1
mglmx	57:ee5da8a011e0	101	#define MEDIUM 2
mglmx	57:ee5da8a011e0	102	#define FAST 3
mglmx	57:ee5da8a011e0	103	#define FULL 4
mglmx	57:ee5da8a011e0	104	#define R_MEDIUM 5
mglmx	55:aa3baa01f43d	105
mglmx	55:aa3baa01f43d	106
carlosperales95	74:dbbc528f2b18	107	//--------DCC SEND COMMANDS
mglmx	62:6fc0c683cb02	108
carlosperales95	74:dbbc528f2b18	109	//01DCSSSS for speed, D is direction (fwd=1 and rev=0), C is speed(SSSSC) LSB
carlosperales95	74:dbbc528f2b18	110	const unsigned int DCCinst_forward = 0x68; //forward half speed
carlosperales95	74:dbbc528f2b18	111	const unsigned int DCCinst_forward_slow = 0x66; //forward slow speed (step 9)
carlosperales95	74:dbbc528f2b18	112	const unsigned int DCCinst_forward_fast = 0x6C; //Forward fast speed (step 22)
carlosperales95	74:dbbc528f2b18	113	const unsigned int DCCinst_forward_full = 0x6F; //Forward full speed
carlosperales95	74:dbbc528f2b18	114	const unsigned int DCCinst_reverse = 0x48; //reverse half speed
carlosperales95	74:dbbc528f2b18	115	const unsigned int DCCinst_stop = 0x50; //stop the train
carlosperales95	25:a42a1ed4d8e9	116
carlosperales95	74:dbbc528f2b18	117	//100DDDDD for basic headlight functions
carlosperales95	74:dbbc528f2b18	118	const unsigned int DCC_func_lighton = 0x90; //F0 turns on headlight function
carlosperales95	74:dbbc528f2b18	119	const unsigned int DCC_func_dimlight = 0x91; //F0 + F1 dims headlight
mglmx	35:cfcfeccb959e	120
carlosperales95	45:d589318238bf	121
carlosperales95	74:dbbc528f2b18	122	//.....SWITCH COMMAND VARS
carlosperales95	74:dbbc528f2b18	123
carlosperales95	74:dbbc528f2b18	124	const unsigned int SWBaddress = 0x06; //Address for switch box
carlosperales95	74:dbbc528f2b18	125
carlosperales95	74:dbbc528f2b18	126	//100DDDDD where DDDDD is the switch command and 100 is constant:
carlosperales95	74:dbbc528f2b18	127
carlosperales95	74:dbbc528f2b18	128	//00001(F1 active)-00010(F2 active)-00100(F3 active)-01000(F4 active)
carlosperales95	74:dbbc528f2b18	129	//Example - 111111 0 00000101 0 10000000 0 10000101 1 - idle
carlosperales95	74:dbbc528f2b18	130	const unsigned int SWBidle = 0x80; //IDLE - Flip last activated SW.
carlosperales95	74:dbbc528f2b18	131	const unsigned int SWBflip_1 = 0x81; //Flip SW1
carlosperales95	74:dbbc528f2b18	132	const unsigned int SWBflip_2 = 0x82; //Flip SW2
carlosperales95	74:dbbc528f2b18	133	const unsigned int SWBflip_3 = 0x84; //Flip SW3
carlosperales95	74:dbbc528f2b18	134	const unsigned int SWBflip_4 = 0x88; //Flip SW4
carlosperales95	74:dbbc528f2b18	135
carlosperales95	74:dbbc528f2b18	136
carlosperales95	74:dbbc528f2b18	137	//.....DCC TRAIN COMMAND VARS
carlosperales95	74:dbbc528f2b18	138
carlosperales95	74:dbbc528f2b18	139	//typical out of box default engine DCC address is 3 (at least for Bachmann trains)
carlosperales95	74:dbbc528f2b18	140	//Note: A DCC controller can reprogram the address whenever needed
carlosperales95	74:dbbc528f2b18	141	const unsigned int DCCaddressDR = 0x01; //Address for train 1 DARK-RED
carlosperales95	74:dbbc528f2b18	142	const unsigned int DCCaddressLR = 0x03; //Address for train 3 LIGHT-RED
mglmx	41:4fa6aa29d1ed	143
carlosperales95	45:d589318238bf	144
carlosperales95	45:d589318238bf	145	/**
carlosperales95	45:d589318238bf	146	*
mglmx	57:ee5da8a011e0	147	*Method to send DCC commands to train and switches.
mglmx	57:ee5da8a011e0	148	*
mglmx	57:ee5da8a011e0	149	*@address - (HEX)Address where the commands will be sent
mglmx	57:ee5da8a011e0	150	*@inst - (HEX)Number of instruction that will be commanded
mglmx	57:ee5da8a011e0	151	*@repeat_count - Number of times the command will be sent
mglmx	57:ee5da8a011e0	152	*
mglmx	57:ee5da8a011e0	153	**/
mglmx	57:ee5da8a011e0	154	void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count){
mglmx	57:ee5da8a011e0	155
mglmx	57:ee5da8a011e0	156	unsigned __int64 command = 0x0000000000000000; // __int64 is the 64-bit integer type
mglmx	57:ee5da8a011e0	157	unsigned __int64 temp_command = 0x0000000000000000;
mglmx	57:ee5da8a011e0	158	unsigned __int64 prefix = 0x3FFF; // 14 "1" bits needed at start
mglmx	57:ee5da8a011e0	159	unsigned int error = 0x00; //error byte
mglmx	57:ee5da8a011e0	160
mglmx	57:ee5da8a011e0	161	//calculate error detection byte with xor
mglmx	57:ee5da8a011e0	162	error = address ^ inst;
mglmx	57:ee5da8a011e0	163
mglmx	57:ee5da8a011e0	164	//combine packet bits in basic DCC format
mglmx	57:ee5da8a011e0	165	command = (prefix<<28)\|(address<<19)\|(inst<<10)\|((error)<<1)\|0x01;
mglmx	57:ee5da8a011e0	166	//printf("\n\r %llx \n\r",command);
mglmx	57:ee5da8a011e0	167
mglmx	57:ee5da8a011e0	168	int i=0;
mglmx	57:ee5da8a011e0	169	//repeat DCC command lots of times
mglmx	57:ee5da8a011e0	170	while(i < repeat_count) {
mglmx	57:ee5da8a011e0	171
mglmx	57:ee5da8a011e0	172	temp_command = command;
mglmx	57:ee5da8a011e0	173	//loops through packet bits encoding and sending out digital pulses for a DCC command
mglmx	57:ee5da8a011e0	174	for (int j=0; j<64; j++) {
mglmx	57:ee5da8a011e0	175
mglmx	57:ee5da8a011e0	176	if((temp_command&0x8000000000000000)==0) {
mglmx	57:ee5da8a011e0	177	//test packet bit
mglmx	57:ee5da8a011e0	178	//send data for a "0" bit
mglmx	57:ee5da8a011e0	179	Track=0;
mglmx	57:ee5da8a011e0	180	wait_us(100);
mglmx	57:ee5da8a011e0	181	Track=1;
mglmx	57:ee5da8a011e0	182	wait_us(100);
mglmx	57:ee5da8a011e0	183	//printf("0011");
mglmx	57:ee5da8a011e0	184	}else{
mglmx	57:ee5da8a011e0	185
mglmx	57:ee5da8a011e0	186	//send data for a "1"bit
mglmx	57:ee5da8a011e0	187	Track=0;
mglmx	57:ee5da8a011e0	188	wait_us(58);
mglmx	57:ee5da8a011e0	189	Track=1;
mglmx	57:ee5da8a011e0	190	wait_us(58);
mglmx	57:ee5da8a011e0	191	//printf("01");
mglmx	57:ee5da8a011e0	192	}
mglmx	57:ee5da8a011e0	193	// next bit in packet
mglmx	57:ee5da8a011e0	194	temp_command = temp_command<<1;
mglmx	57:ee5da8a011e0	195	}
mglmx	57:ee5da8a011e0	196	i++;
mglmx	57:ee5da8a011e0	197	}
mglmx	57:ee5da8a011e0	198	}
mglmx	57:ee5da8a011e0	199
carlosperales95	74:dbbc528f2b18	200
carlosperales95	74:dbbc528f2b18	201
carlosperales95	74:dbbc528f2b18	202	//------CLASSES
carlosperales95	74:dbbc528f2b18	203
carlosperales95	74:dbbc528f2b18	204	//......POSITION CLASS
carlosperales95	74:dbbc528f2b18	205
carlosperales95	74:dbbc528f2b18	206	/**
carlosperales95	74:dbbc528f2b18	207	*
carlosperales95	74:dbbc528f2b18	208	*Position class.
carlosperales95	74:dbbc528f2b18	209	*
carlosperales95	74:dbbc528f2b18	210	*@position -
carlosperales95	74:dbbc528f2b18	211	*@previous_cw -
carlosperales95	74:dbbc528f2b18	212	*@previous_ccw -
carlosperales95	74:dbbc528f2b18	213	*
carlosperales95	74:dbbc528f2b18	214	*Position(int) -
carlosperales95	74:dbbc528f2b18	215	*
carlosperales95	74:dbbc528f2b18	216	*get_pos() -
carlosperales95	74:dbbc528f2b18	217	*get_prev_cw() -
carlosperales95	74:dbbc528f2b18	218	*get_ccw() -
carlosperales95	74:dbbc528f2b18	219	*add_prev_cw() -
carlosperales95	74:dbbc528f2b18	220	*add_ccw() -
carlosperales95	74:dbbc528f2b18	221	*
carlosperales95	74:dbbc528f2b18	222	**/
carlosperales95	74:dbbc528f2b18	223	class Position{
carlosperales95	74:dbbc528f2b18	224
carlosperales95	74:dbbc528f2b18	225	private:
carlosperales95	74:dbbc528f2b18	226
carlosperales95	74:dbbc528f2b18	227	int position;
carlosperales95	74:dbbc528f2b18	228	vector <int> previous_cw;
carlosperales95	74:dbbc528f2b18	229	vector <int> previous_ccw;
carlosperales95	74:dbbc528f2b18	230	public:
carlosperales95	74:dbbc528f2b18	231
carlosperales95	74:dbbc528f2b18	232	Position(int p){
carlosperales95	74:dbbc528f2b18	233	position = p;
carlosperales95	74:dbbc528f2b18	234	}
carlosperales95	74:dbbc528f2b18	235
carlosperales95	74:dbbc528f2b18	236	int get_pos(){
carlosperales95	74:dbbc528f2b18	237	return position;
carlosperales95	74:dbbc528f2b18	238	}
carlosperales95	74:dbbc528f2b18	239
carlosperales95	74:dbbc528f2b18	240	vector<int> get_next_cw(){
carlosperales95	74:dbbc528f2b18	241	return previous_ccw;
carlosperales95	74:dbbc528f2b18	242	}
carlosperales95	74:dbbc528f2b18	243
carlosperales95	74:dbbc528f2b18	244	vector<int> get_next_ccw(){
carlosperales95	74:dbbc528f2b18	245	return previous_cw;
carlosperales95	74:dbbc528f2b18	246	}
carlosperales95	74:dbbc528f2b18	247
carlosperales95	74:dbbc528f2b18	248	vector <int> get_prev_cw(){
carlosperales95	74:dbbc528f2b18	249	return previous_cw;
carlosperales95	74:dbbc528f2b18	250	}
carlosperales95	74:dbbc528f2b18	251
carlosperales95	74:dbbc528f2b18	252	vector <int> get_prev_ccw(){
carlosperales95	74:dbbc528f2b18	253	return previous_ccw;
carlosperales95	74:dbbc528f2b18	254	}
carlosperales95	74:dbbc528f2b18	255
carlosperales95	74:dbbc528f2b18	256	void add_prev_cw(int pos){
carlosperales95	74:dbbc528f2b18	257	previous_cw.push_back(pos);
carlosperales95	74:dbbc528f2b18	258	};
carlosperales95	74:dbbc528f2b18	259
carlosperales95	74:dbbc528f2b18	260	void add_prev_ccw(int pos){
carlosperales95	74:dbbc528f2b18	261	previous_ccw.push_back(pos);
carlosperales95	74:dbbc528f2b18	262	};
carlosperales95	74:dbbc528f2b18	263	};
carlosperales95	74:dbbc528f2b18	264
carlosperales95	74:dbbc528f2b18	265	//......INITS USED FOR CLASS TRAIN
carlosperales95	74:dbbc528f2b18	266
carlosperales95	74:dbbc528f2b18	267	//Creating a vector with all the positions.
carlosperales95	74:dbbc528f2b18	268	vector<Position> positions;
carlosperales95	74:dbbc528f2b18	269
mglmx	57:ee5da8a011e0	270	/**
mglmx	57:ee5da8a011e0	271	*Defining areas for train detection and collision logic.
mglmx	57:ee5da8a011e0	272	*area_A_arr/area_B_arr - Arrays that hold the Dsensors for each area, used to initialize the vectors.
mglmx	57:ee5da8a011e0	273	*area_A/area_B - Vectors that hold the different sensors of the corresponding areas of the track.
mglmx	57:ee5da8a011e0	274	**/
mglmx	57:ee5da8a011e0	275	int area_A_arr[] = {D21,D2,D22,D1,D0,D13,D12};
mglmx	57:ee5da8a011e0	276	int area_B_arr[] = {D6,D7,D8};
mglmx	57:ee5da8a011e0	277
mglmx	57:ee5da8a011e0	278	const vector<int> area_A(area_A_arr,area_A_arr + sizeof(area_A_arr) / sizeof(int));
mglmx	57:ee5da8a011e0	279	const vector<int> area_B(area_B_arr,area_B_arr + sizeof(area_B_arr) / sizeof(int));
mglmx	57:ee5da8a011e0	280
mglmx	57:ee5da8a011e0	281
carlosperales95	74:dbbc528f2b18	282	//......TRAIN CLASS
mglmx	62:6fc0c683cb02	283
mglmx	57:ee5da8a011e0	284	/**
mglmx	57:ee5da8a011e0	285	*
carlosperales95	45:d589318238bf	286	*Train class.
carlosperales95	45:d589318238bf	287	*
carlosperales95	45:d589318238bf	288	*@position -
carlosperales95	45:d589318238bf	289	*@going_cw -
carlosperales95	45:d589318238bf	290	*
carlosperales95	45:d589318238bf	291	*Train(int, bool) -
mglmx	62:6fc0c683cb02	292	*Train(bool) -
carlosperales95	58:b60db1092088	293	*
mglmx	62:6fc0c683cb02	294	*Vector get_next_sensors() -
carlosperales95	58:b60db1092088	295	*set_position(int) -
carlosperales95	58:b60db1092088	296	*set_goes_cw(bool) -
carlosperales95	45:d589318238bf	297	*Position get_position() -
carlosperales95	45:d589318238bf	298	*Int get_position_number() -
carlosperales95	45:d589318238bf	299	*Bool goes_cw() -
carlosperales95	45:d589318238bf	300	*
carlosperales95	45:d589318238bf	301	**/
mglmx	41:4fa6aa29d1ed	302	class Train{
carlosperales95	45:d589318238bf	303
mglmx	62:6fc0c683cb02	304	private:
mglmx	62:6fc0c683cb02	305
mglmx	57:ee5da8a011e0	306	unsigned int train_address; //stop the train
mglmx	41:4fa6aa29d1ed	307	Position *position;
mglmx	41:4fa6aa29d1ed	308	bool going_cw;
mglmx	57:ee5da8a011e0	309	int speed;
carlosperales95	45:d589318238bf	310
mglmx	41:4fa6aa29d1ed	311	public:
mglmx	41:4fa6aa29d1ed	312	Train(int pos, bool cw){
carlosperales95	45:d589318238bf	313
mglmx	41:4fa6aa29d1ed	314	position = &positions[pos];
mglmx	41:4fa6aa29d1ed	315	going_cw = cw;
mglmx	41:4fa6aa29d1ed	316	}
carlosperales95	45:d589318238bf	317
mglmx	57:ee5da8a011e0	318	/**
mglmx	62:6fc0c683cb02	319	* Contructor that takes the address of the train and the speed with default value MEDIUM.
mglmx	62:6fc0c683cb02	320	*/
mglmx	62:6fc0c683cb02	321	Train(unsigned int address, int s=MEDIUM){
mglmx	57:ee5da8a011e0	322	train_address = address;
mglmx	57:ee5da8a011e0	323	speed = s;
mglmx	57:ee5da8a011e0	324	}
mglmx	62:6fc0c683cb02	325
mglmx	62:6fc0c683cb02	326	Train(bool cw){ going_cw = cw; }
mglmx	62:6fc0c683cb02	327
mglmx	41:4fa6aa29d1ed	328	vector<int> get_next_sensors(){
mglmx	41:4fa6aa29d1ed	329
mglmx	41:4fa6aa29d1ed	330	//Checking direction
mglmx	62:6fc0c683cb02	331	if(going_cw){
mglmx	62:6fc0c683cb02	332
mglmx	62:6fc0c683cb02	333	return position->get_next_cw();
mglmx	62:6fc0c683cb02	334	}else{
mglmx	62:6fc0c683cb02	335
mglmx	62:6fc0c683cb02	336	return position->get_next_ccw();
mglmx	62:6fc0c683cb02	337	}
mglmx	41:4fa6aa29d1ed	338	}
mglmx	41:4fa6aa29d1ed	339
carlosperales95	74:dbbc528f2b18	340	vector<int> get_previous_sensors(){
carlosperales95	74:dbbc528f2b18	341
carlosperales95	74:dbbc528f2b18	342	//Checking direction
carlosperales95	74:dbbc528f2b18	343	if(going_cw){
carlosperales95	74:dbbc528f2b18	344
carlosperales95	74:dbbc528f2b18	345	return position->get_next_ccw();
carlosperales95	74:dbbc528f2b18	346	}else{
carlosperales95	74:dbbc528f2b18	347
carlosperales95	74:dbbc528f2b18	348	return position->get_next_cw();
carlosperales95	74:dbbc528f2b18	349	}
carlosperales95	74:dbbc528f2b18	350	}
carlosperales95	74:dbbc528f2b18	351
mglmx	57:ee5da8a011e0	352	void set_speed(int s){
mglmx	57:ee5da8a011e0	353	speed = s;
mglmx	57:ee5da8a011e0	354	}
mglmx	57:ee5da8a011e0	355
mglmx	57:ee5da8a011e0	356	/**
mglmx	57:ee5da8a011e0	357	* Sends a DCC command to the train with the speed indicaed by the attribute speed
mglmx	57:ee5da8a011e0	358	* The number of times the command is sent can be indicated as an optional parameter. Default value is 1.
mglmx	57:ee5da8a011e0	359	*/
mglmx	62:6fc0c683cb02	360	void run(int times=1){
mglmx	57:ee5da8a011e0	361
mglmx	57:ee5da8a011e0	362	const unsigned int DCCinst_forward_slow = 0x66; //forward slow speed (step 9)
mglmx	57:ee5da8a011e0	363	const unsigned int DCCinst_forward_medium = 0x68; //forward half speed
mglmx	57:ee5da8a011e0	364	const unsigned int DCCinst_forward_fast = 0x6C; //Forward fast speed (step 22)
mglmx	57:ee5da8a011e0	365	const unsigned int DCCinst_forward_full = 0x6F; //Forward full speed
mglmx	57:ee5da8a011e0	366	const unsigned int DCCinst_reverse_medium = 0x48; //reverse half speed
mglmx	57:ee5da8a011e0	367	const unsigned int DCCinst_stop = 0x50; //stop the train
mglmx	57:ee5da8a011e0	368
mglmx	57:ee5da8a011e0	369	switch(speed){
mglmx	62:6fc0c683cb02	370	case STOP:
mglmx	57:ee5da8a011e0	371	DCC_send_command(train_address, DCCinst_stop,times);
mglmx	57:ee5da8a011e0	372	break;
mglmx	57:ee5da8a011e0	373	case SLOW:
mglmx	57:ee5da8a011e0	374	DCC_send_command(train_address, DCCinst_forward_slow,times);
mglmx	57:ee5da8a011e0	375	break;
mglmx	57:ee5da8a011e0	376	case MEDIUM:
mglmx	57:ee5da8a011e0	377	DCC_send_command(train_address, DCCinst_forward_medium,times);
mglmx	57:ee5da8a011e0	378	break;
mglmx	57:ee5da8a011e0	379	case FAST:
mglmx	57:ee5da8a011e0	380	DCC_send_command(train_address, DCCinst_forward_fast,times);
mglmx	57:ee5da8a011e0	381	break;
mglmx	57:ee5da8a011e0	382	case FULL:
mglmx	57:ee5da8a011e0	383	DCC_send_command(train_address, DCCinst_forward_full,times);
mglmx	57:ee5da8a011e0	384	break;
mglmx	57:ee5da8a011e0	385	case R_MEDIUM:
mglmx	57:ee5da8a011e0	386	DCC_send_command(train_address, DCCinst_reverse_medium,times);
mglmx	57:ee5da8a011e0	387	break;
mglmx	57:ee5da8a011e0	388	}
mglmx	57:ee5da8a011e0	389	}
mglmx	57:ee5da8a011e0	390
mglmx	41:4fa6aa29d1ed	391	void set_position(int pos){
carlosperales95	45:d589318238bf	392
mglmx	41:4fa6aa29d1ed	393	position = &positions[pos]; //Taking the new position from the positions vector
mglmx	41:4fa6aa29d1ed	394	}
mglmx	41:4fa6aa29d1ed	395
mglmx	41:4fa6aa29d1ed	396	void set_goes_cw(bool cw){
carlosperales95	45:d589318238bf	397
mglmx	41:4fa6aa29d1ed	398	going_cw = cw;
mglmx	41:4fa6aa29d1ed	399	}
mglmx	41:4fa6aa29d1ed	400
mglmx	41:4fa6aa29d1ed	401	Position get_position(){
carlosperales95	45:d589318238bf	402
mglmx	41:4fa6aa29d1ed	403	return *position;
mglmx	41:4fa6aa29d1ed	404	}
mglmx	41:4fa6aa29d1ed	405
mglmx	41:4fa6aa29d1ed	406	int get_position_number(){
carlosperales95	45:d589318238bf	407
mglmx	41:4fa6aa29d1ed	408	return position->get_pos();
mglmx	41:4fa6aa29d1ed	409	}
mglmx	41:4fa6aa29d1ed	410
mglmx	41:4fa6aa29d1ed	411	bool goes_cw(){
carlosperales95	45:d589318238bf	412
mglmx	41:4fa6aa29d1ed	413	return going_cw;
mglmx	41:4fa6aa29d1ed	414	}
mglmx	57:ee5da8a011e0	415
mglmx	62:6fc0c683cb02	416
mglmx	57:ee5da8a011e0	417	/**
mglmx	57:ee5da8a011e0	418	*
mglmx	57:ee5da8a011e0	419	*Checks if the element exists within the vector.
mglmx	57:ee5da8a011e0	420	*
mglmx	57:ee5da8a011e0	421	*@v - The vector (of ints) the method will go through.
mglmx	57:ee5da8a011e0	422	*@element - The element the method will look for.
mglmx	57:ee5da8a011e0	423	*
mglmx	57:ee5da8a011e0	424	**/
mglmx	57:ee5da8a011e0	425	bool in_vector(vector<int>v,int element){
mglmx	57:ee5da8a011e0	426
mglmx	57:ee5da8a011e0	427	bool exist = false;
mglmx	57:ee5da8a011e0	428
mglmx	57:ee5da8a011e0	429	for(int i=0; i< v.size(); i++){
mglmx	57:ee5da8a011e0	430
mglmx	57:ee5da8a011e0	431	if(v[i] == element){
mglmx	57:ee5da8a011e0	432
mglmx	57:ee5da8a011e0	433	exist = true;
mglmx	57:ee5da8a011e0	434	}
mglmx	57:ee5da8a011e0	435	}
mglmx	57:ee5da8a011e0	436	return exist;
mglmx	57:ee5da8a011e0	437	}
mglmx	57:ee5da8a011e0	438
mglmx	57:ee5da8a011e0	439	bool is_in_A(){
mglmx	62:6fc0c683cb02	440	return in_vector(area_A,get_position_number());
mglmx	57:ee5da8a011e0	441
mglmx	57:ee5da8a011e0	442	}
mglmx	57:ee5da8a011e0	443
mglmx	57:ee5da8a011e0	444	bool is_in_B(){
mglmx	57:ee5da8a011e0	445
mglmx	62:6fc0c683cb02	446	return in_vector(area_B,get_position_number());
mglmx	57:ee5da8a011e0	447	}
mglmx	41:4fa6aa29d1ed	448	};
mglmx	41:4fa6aa29d1ed	449
carlosperales95	45:d589318238bf	450
carlosperales95	74:dbbc528f2b18	451	//------GLOBAL VARS AND INITS
carlosperales95	74:dbbc528f2b18	452
carlosperales95	74:dbbc528f2b18	453	//......POSITIONS INIT
carlosperales95	74:dbbc528f2b18	454
carlosperales95	38:b9aba3715682	455	//Creation of all the positions. One for every sensor on the table - Position name(mapping)
mglmx	35:cfcfeccb959e	456	Position d0(D0);
mglmx	35:cfcfeccb959e	457	Position d1(D1);
mglmx	35:cfcfeccb959e	458	Position d2(D2);
mglmx	35:cfcfeccb959e	459	Position d3(D3);
mglmx	35:cfcfeccb959e	460	Position d4(D4);
mglmx	35:cfcfeccb959e	461	Position d5(D5);
mglmx	35:cfcfeccb959e	462	Position d6(D6);
mglmx	35:cfcfeccb959e	463	Position d7(D7);
mglmx	35:cfcfeccb959e	464	Position d8(D8);
mglmx	35:cfcfeccb959e	465	Position d9(D9);
mglmx	35:cfcfeccb959e	466	Position d10(D10);
mglmx	35:cfcfeccb959e	467	Position d11(D11);
mglmx	35:cfcfeccb959e	468	Position d12(D12);
mglmx	35:cfcfeccb959e	469	Position d13(D13);
mglmx	35:cfcfeccb959e	470	Position d21(D21);
mglmx	35:cfcfeccb959e	471	Position d22(D22);
mglmx	35:cfcfeccb959e	472
mglmx	43:346a1f4144cd	473
carlosperales95	74:dbbc528f2b18	474	//......TRAINS INIT
mglmx	57:ee5da8a011e0	475
carlosperales95	45:d589318238bf	476	/**
carlosperales95	45:d589318238bf	477	*Creation of 2 Train objects.
carlosperales95	45:d589318238bf	478	*Using boolean constructor because position initialization will be done after initializing all position vectors.
carlosperales95	45:d589318238bf	479	*DR_train = Dark Red train - LR_train = Light Red Train
carlosperales95	45:d589318238bf	480	**/
mglmx	66:45a90a945983	481	Train DR_train(DCCaddressDR,MEDIUM);
mglmx	57:ee5da8a011e0	482	Train LR_train(DCCaddressLR,MEDIUM);
mglmx	43:346a1f4144cd	483
carlosperales95	74:dbbc528f2b18	484
carlosperales95	74:dbbc528f2b18	485	//......FLAGS INIT
carlosperales95	74:dbbc528f2b18	486
carlosperales95	47:e992a129ef44	487	//possibility of an array having {dr_train, lr_train}? for reuse and modularity of functions
carlosperales95	47:e992a129ef44	488
carlosperales95	69:609c69e91b4e	489	int speedcheck = 0;
carlosperales95	65:8fd4046c6540	490
carlosperales95	74:dbbc528f2b18	491	bool station_stop = false;
carlosperales95	74:dbbc528f2b18	492
mglmx	62:6fc0c683cb02	493
mglmx	18:aa43bb62e60f	494
carlosperales95	38:b9aba3715682	495	//************** FUNCTIONS FOR DENVER TRAIN **************//
carlosperales95	38:b9aba3715682	496
carlosperales95	38:b9aba3715682	497
mglmx	22:e4153ca757dd	498	/**
carlosperales95	45:d589318238bf	499	*
carlosperales95	25:a42a1ed4d8e9	500	*Activates the buzzer for 0.5 seconds.
carlosperales95	45:d589318238bf	501	*
mglmx	22:e4153ca757dd	502	**/
mglmx	22:e4153ca757dd	503	void doBuzz(){
carlosperales95	25:a42a1ed4d8e9	504
mglmx	22:e4153ca757dd	505	buzz = 1;
mglmx	22:e4153ca757dd	506	wait(0.5);
mglmx	22:e4153ca757dd	507	buzz = 0;
mglmx	22:e4153ca757dd	508	}
mglmx	18:aa43bb62e60f	509
carlosperales95	45:d589318238bf	510
carlosperales95	45:d589318238bf	511	/**
carlosperales95	45:d589318238bf	512	*
carlosperales95	47:e992a129ef44	513	*Initializes every position's vectors (prev_cw and prev_ccw) with the corresponding sensors.
carlosperales95	47:e992a129ef44	514	*prev_cw - Sensors previous to the current in clockwise sense.
carlosperales95	47:e992a129ef44	515	*prev_ccw - Sensors previous to the current in counter-clockwise sense.
carlosperales95	45:d589318238bf	516	*
carlosperales95	45:d589318238bf	517	**/
mglmx	35:cfcfeccb959e	518	void init_positions(){
mglmx	36:9428c72bdd58	519
mglmx	35:cfcfeccb959e	520	d0.add_prev_cw(D1);
mglmx	35:cfcfeccb959e	521	d0.add_prev_ccw(D13);
mglmx	35:cfcfeccb959e	522
mglmx	35:cfcfeccb959e	523	d1.add_prev_cw(D22);
mglmx	35:cfcfeccb959e	524	d1.add_prev_ccw(D0);
mglmx	35:cfcfeccb959e	525
mglmx	35:cfcfeccb959e	526	d22.add_prev_cw(D2);
mglmx	35:cfcfeccb959e	527	d22.add_prev_ccw(D1);
mglmx	35:cfcfeccb959e	528
mglmx	35:cfcfeccb959e	529	d2.add_prev_cw(D21);
mglmx	35:cfcfeccb959e	530	d2.add_prev_ccw(D22);
mglmx	35:cfcfeccb959e	531
mglmx	35:cfcfeccb959e	532	d21.add_prev_cw(D3);
mglmx	35:cfcfeccb959e	533	d21.add_prev_cw(D4);
mglmx	35:cfcfeccb959e	534	d21.add_prev_ccw(D2);
mglmx	35:cfcfeccb959e	535
mglmx	35:cfcfeccb959e	536	d3.add_prev_cw(D9);
mglmx	35:cfcfeccb959e	537	d3.add_prev_ccw(D21);
mglmx	35:cfcfeccb959e	538
mglmx	35:cfcfeccb959e	539	d4.add_prev_cw(D6);
mglmx	35:cfcfeccb959e	540	d4.add_prev_ccw(D21);
mglmx	35:cfcfeccb959e	541
mglmx	35:cfcfeccb959e	542	d5.add_prev_cw(D6);
mglmx	35:cfcfeccb959e	543	d5.add_prev_ccw(D11);
mglmx	35:cfcfeccb959e	544
mglmx	35:cfcfeccb959e	545	d6.add_prev_cw(D7);
mglmx	35:cfcfeccb959e	546	d6.add_prev_ccw(D4);
mglmx	35:cfcfeccb959e	547	d6.add_prev_ccw(D5);
mglmx	35:cfcfeccb959e	548
mglmx	35:cfcfeccb959e	549	d7.add_prev_cw(D8);
mglmx	35:cfcfeccb959e	550	d7.add_prev_ccw(D6);
mglmx	35:cfcfeccb959e	551
mglmx	36:9428c72bdd58	552	d8.add_prev_cw(D9);
mglmx	36:9428c72bdd58	553	d8.add_prev_cw(D10);
mglmx	36:9428c72bdd58	554	d8.add_prev_ccw(D7);
mglmx	36:9428c72bdd58	555
mglmx	35:cfcfeccb959e	556	d9.add_prev_cw(D3);
mglmx	35:cfcfeccb959e	557	d9.add_prev_ccw(D8);
mglmx	35:cfcfeccb959e	558
mglmx	35:cfcfeccb959e	559	d10.add_prev_cw(D12);
mglmx	35:cfcfeccb959e	560	d10.add_prev_ccw(D8);
mglmx	35:cfcfeccb959e	561
mglmx	35:cfcfeccb959e	562	d11.add_prev_cw(D12);
mglmx	35:cfcfeccb959e	563	d11.add_prev_ccw(D5);
mglmx	35:cfcfeccb959e	564
mglmx	35:cfcfeccb959e	565	d12.add_prev_cw(D13);
mglmx	35:cfcfeccb959e	566	d12.add_prev_ccw(D10);
mglmx	35:cfcfeccb959e	567	d12.add_prev_ccw(D11);
mglmx	35:cfcfeccb959e	568
mglmx	35:cfcfeccb959e	569	d13.add_prev_cw(D0);
mglmx	35:cfcfeccb959e	570	d13.add_prev_ccw(D12);
mglmx	36:9428c72bdd58	571
mglmx	36:9428c72bdd58	572	//Initialize array with positions
mglmx	36:9428c72bdd58	573	positions.push_back(d0);
mglmx	36:9428c72bdd58	574	positions.push_back(d1);
mglmx	36:9428c72bdd58	575	positions.push_back(d2);
mglmx	36:9428c72bdd58	576	positions.push_back(d3);
mglmx	36:9428c72bdd58	577	positions.push_back(d4);
mglmx	36:9428c72bdd58	578	positions.push_back(d5);
mglmx	36:9428c72bdd58	579	positions.push_back(d6);
mglmx	36:9428c72bdd58	580	positions.push_back(d7);
mglmx	36:9428c72bdd58	581	positions.push_back(d8);
mglmx	36:9428c72bdd58	582	positions.push_back(d9);
mglmx	36:9428c72bdd58	583	positions.push_back(d10);
mglmx	36:9428c72bdd58	584	positions.push_back(d11);
mglmx	36:9428c72bdd58	585	positions.push_back(d12);
mglmx	36:9428c72bdd58	586	positions.push_back(d13);
mglmx	36:9428c72bdd58	587	positions.push_back(d21);
mglmx	36:9428c72bdd58	588	positions.push_back(d22);
mglmx	35:cfcfeccb959e	589	}
carlosperales95	24:1d71dd8778c4	590
carlosperales95	45:d589318238bf	591
mglmx	22:e4153ca757dd	592	/**
carlosperales95	24:1d71dd8778c4	593	*
carlosperales95	28:71bd4c83c05f	594	*Here we initialize the mcp that will be used to manage the interrupts.
carlosperales95	25:a42a1ed4d8e9	595	*
carlosperales95	24:1d71dd8778c4	596	**/
mglmx	18:aa43bb62e60f	597	void initialize_mcp(){
mglmx	33:24ce12dec157	598	mcp = new MCP23017(i2c,0x40); //Connect to SCL - p28 and SDA - p27 and MPC I2C address 0x40
mglmx	18:aa43bb62e60f	599
mglmx	33:24ce12dec157	600	mcp->_write(IODIRA, (unsigned char )0xff);
mglmx	33:24ce12dec157	601	mcp->_write(IODIRB, (unsigned char )0xff);
mglmx	33:24ce12dec157	602	mcp->_write(IPOLA, (unsigned char )0x00);
mglmx	33:24ce12dec157	603	mcp->_write(IPOLB, (unsigned char )0x00);
mglmx	33:24ce12dec157	604	mcp->_write(DEFVALA, (unsigned char )0xff);
mglmx	33:24ce12dec157	605	mcp->_write(DEFVALB, (unsigned char )0xff);
mglmx	33:24ce12dec157	606	mcp->_write(INTCONA, (unsigned char )0xff);
mglmx	33:24ce12dec157	607	mcp->_write(INTCONB, (unsigned char )0xff);
mglmx	33:24ce12dec157	608	mcp->_write(IOCONA, (unsigned char )0x2);
mglmx	33:24ce12dec157	609	mcp->_write(IOCONB, (unsigned char )0x2);
mglmx	33:24ce12dec157	610	mcp->_write(GPPUA, (unsigned char )0xff);
mglmx	33:24ce12dec157	611	mcp->_write(GPPUB, (unsigned char )0xff);
mglmx	33:24ce12dec157	612
mglmx	18:aa43bb62e60f	613	}
mglmx	18:aa43bb62e60f	614
carlosperales95	45:d589318238bf	615
carlosperales95	24:1d71dd8778c4	616	/**
carlosperales95	24:1d71dd8778c4	617	*
carlosperales95	37:bb15bea420a3	618	*Returns the number of the sensor where the train was detected.
carlosperales95	37:bb15bea420a3	619	*
carlosperales95	47:e992a129ef44	620	*@number -
carlosperales95	47:e992a129ef44	621	*@interrupt -
carlosperales95	47:e992a129ef44	622	*
carlosperales95	37:bb15bea420a3	623	**/
mglmx	34:c9ab2a987734	624	int get_sensor(unsigned int number,int interrupt){
carlosperales95	45:d589318238bf	625
mglmx	34:c9ab2a987734	626	int sensor = -1;
mglmx	34:c9ab2a987734	627
carlosperales95	37:bb15bea420a3	628	for(int i=0; i<8; i++){
mglmx	34:c9ab2a987734	629
carlosperales95	45:d589318238bf	630	if(~number & 1<<i){
carlosperales95	45:d589318238bf	631
mglmx	34:c9ab2a987734	632	sensor = i;
mglmx	34:c9ab2a987734	633	}
mglmx	34:c9ab2a987734	634	}
mglmx	34:c9ab2a987734	635
mglmx	34:c9ab2a987734	636	if(interrupt == 1){
carlosperales95	45:d589318238bf	637
carlosperales95	37:bb15bea420a3	638	sensor+= 8; // Sensors caught by interreupt1 are identified from 8 to 15.
mglmx	34:c9ab2a987734	639	}
carlosperales95	45:d589318238bf	640
mglmx	34:c9ab2a987734	641	return sensor;
mglmx	34:c9ab2a987734	642	}
mglmx	34:c9ab2a987734	643
carlosperales95	65:8fd4046c6540	644
mglmx	62:6fc0c683cb02	645	/**
mglmx	62:6fc0c683cb02	646	*
mglmx	62:6fc0c683cb02	647	*Method to flip the switches
mglmx	62:6fc0c683cb02	648	*
mglmx	62:6fc0c683cb02	649	*@switchId - (1-4)The ID of the switch we want to flip
mglmx	62:6fc0c683cb02	650	*@times - The number of times we want to send the command
mglmx	62:6fc0c683cb02	651	*@activate - True if the switch is going to be activated. False if it needs to go back to rest position.
mglmx	62:6fc0c683cb02	652	*
mglmx	62:6fc0c683cb02	653	**/
mglmx	62:6fc0c683cb02	654	void flip_switch(int switchId, int times, bool activate=true){
mglmx	62:6fc0c683cb02	655
mglmx	62:6fc0c683cb02	656	unsigned int SWBflip = SWBidle; //IDLE - Flip last activated SW.
mglmx	62:6fc0c683cb02	657
mglmx	62:6fc0c683cb02	658	switch(switchId){
mglmx	62:6fc0c683cb02	659
mglmx	62:6fc0c683cb02	660	case 1:
mglmx	62:6fc0c683cb02	661	SWBflip = SWBflip_1; //FLIP SW1
mglmx	62:6fc0c683cb02	662	break;
mglmx	62:6fc0c683cb02	663
mglmx	62:6fc0c683cb02	664	case 2:
mglmx	62:6fc0c683cb02	665	SWBflip = SWBflip_2; //FLIP SW2
mglmx	62:6fc0c683cb02	666	break;
mglmx	62:6fc0c683cb02	667
mglmx	62:6fc0c683cb02	668	case 3:
mglmx	66:45a90a945983	669	SWBflip = SWBidle; //FLIP SW3
mglmx	62:6fc0c683cb02	670	break;
mglmx	62:6fc0c683cb02	671
mglmx	62:6fc0c683cb02	672	case 4:
mglmx	62:6fc0c683cb02	673	SWBflip = SWBflip_4; //FLIP SW4
mglmx	62:6fc0c683cb02	674	break;
mglmx	62:6fc0c683cb02	675
mglmx	62:6fc0c683cb02	676	default:
mglmx	62:6fc0c683cb02	677	break;
mglmx	62:6fc0c683cb02	678	}
mglmx	62:6fc0c683cb02	679
carlosperales95	71:9a303d90c9b8	680	//Security measure not to burn the switch.
carlosperales95	71:9a303d90c9b8	681	DCC_send_command(SWBaddress,SWBflip,times); //Activating switch
mglmx	66:45a90a945983	682
mglmx	62:6fc0c683cb02	683	if(!activate){
mglmx	66:45a90a945983	684	if(switchId == 3){
mglmx	66:45a90a945983	685	DCC_send_command(SWBaddress,SWBflip_3,times); // Exception for switch 3
mglmx	66:45a90a945983	686	}else{
mglmx	66:45a90a945983	687	DCC_send_command(SWBaddress,SWBidle,times);
mglmx	66:45a90a945983	688	}
mglmx	62:6fc0c683cb02	689	}
mglmx	66:45a90a945983	690
mglmx	62:6fc0c683cb02	691	}
mglmx	62:6fc0c683cb02	692
carlosperales95	71:9a303d90c9b8	693
mglmx	62:6fc0c683cb02	694	/**
mglmx	62:6fc0c683cb02	695	* Action to do when NAC is detected
mglmx	62:6fc0c683cb02	696	* Booster is disabled and the buzz is activated
mglmx	62:6fc0c683cb02	697	*
mglmx	62:6fc0c683cb02	698	*/
mglmx	62:6fc0c683cb02	699	void NAC_action(){
mglmx	62:6fc0c683cb02	700	enable = 0;
mglmx	62:6fc0c683cb02	701	doBuzz();
mglmx	62:6fc0c683cb02	702	}
mglmx	62:6fc0c683cb02	703
mglmx	62:6fc0c683cb02	704
carlosperales95	45:d589318238bf	705
carlosperales95	42:b445252a772a	706	/**
carlosperales95	71:9a303d90c9b8	707	*
mglmx	57:ee5da8a011e0	708	*This method will check if there is a non-avoidable frontal collision(NAFC).
carlosperales95	47:e992a129ef44	709	*A NAFC will happen if:
carlosperales95	47:e992a129ef44	710	*
carlosperales95	47:e992a129ef44	711	*Both trains in area A or B with different direction
carlosperales95	47:e992a129ef44	712	*Trains in (D11 and D5) or (D9 and D3) with same direction
carlosperales95	47:e992a129ef44	713	*
carlosperales95	71:9a303d90c9b8	714	**/
mglmx	57:ee5da8a011e0	715	bool check_NAC(){
carlosperales95	45:d589318238bf	716
mglmx	43:346a1f4144cd	717	bool NAC = false;
mglmx	43:346a1f4144cd	718
mglmx	57:ee5da8a011e0	719	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
carlosperales95	45:d589318238bf	720
mglmx	43:346a1f4144cd	721	if(DR_train.goes_cw() ^ LR_train.goes_cw()){ //XOR: They must have different values to be true (Different direction)
mglmx	66:45a90a945983	722	lcd.cls();
mglmx	66:45a90a945983	723	lcd.printf("NAC Both area A or B");
mglmx	43:346a1f4144cd	724	NAC = true;
mglmx	43:346a1f4144cd	725	}
mglmx	67:7ff2425141ce	726	}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
carlosperales95	45:d589318238bf	727
mglmx	43:346a1f4144cd	728	if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction)
mglmx	66:45a90a945983	729	lcd.cls();
mglmx	67:7ff2425141ce	730	lcd.printf("NAC D11 and D5");
mglmx	43:346a1f4144cd	731	NAC = true;
mglmx	43:346a1f4144cd	732	}
mglmx	67:7ff2425141ce	733	}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
carlosperales95	45:d589318238bf	734
mglmx	43:346a1f4144cd	735	if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction)
mglmx	66:45a90a945983	736	lcd.cls();
mglmx	67:7ff2425141ce	737	lcd.printf("NAC D9 and D3");
mglmx	43:346a1f4144cd	738	NAC = true;
mglmx	43:346a1f4144cd	739	}
mglmx	43:346a1f4144cd	740	}
carlosperales95	45:d589318238bf	741	return NAC;
mglmx	43:346a1f4144cd	742	}
mglmx	43:346a1f4144cd	743
carlosperales95	71:9a303d90c9b8	744
mglmx	62:6fc0c683cb02	745	/**
mglmx	62:6fc0c683cb02	746	* Switch_n switch that needs to switch
carlosperales95	63:86f8dfa007ac	747	* cont_sensor sensor that when activated the stopped train continues
mglmx	62:6fc0c683cb02	748	* switch_sensor sensor where the switch should be activated
mglmx	66:45a90a945983	749	*/
mglmx	62:6fc0c683cb02	750	void AFC_action(int switch_n, int cont_sensor, int switch_sensor, Train stop_train, Train cont_train ){
carlosperales95	71:9a303d90c9b8	751
mglmx	62:6fc0c683cb02	752	bool send_pack_switch = false;
carlosperales95	71:9a303d90c9b8	753
mglmx	66:45a90a945983	754	if(switch_n == 3){
mglmx	66:45a90a945983	755	DCC_send_command(SWBaddress,SWBflip_3,15); //Activating switchç
mglmx	66:45a90a945983	756	}
mglmx	66:45a90a945983	757
mglmx	62:6fc0c683cb02	758	while(cont_train->get_position_number() != cont_sensor){
carlosperales95	71:9a303d90c9b8	759
mglmx	62:6fc0c683cb02	760	if(cont_train->get_position_number() == switch_sensor){
carlosperales95	71:9a303d90c9b8	761
mglmx	62:6fc0c683cb02	762	send_pack_switch = true;
mglmx	62:6fc0c683cb02	763	}
mglmx	62:6fc0c683cb02	764	stop_train->set_speed(STOP);
mglmx	62:6fc0c683cb02	765	stop_train->run(); //Stopping train on sensor D4 or D10
mglmx	62:6fc0c683cb02	766	cont_train->run();
mglmx	62:6fc0c683cb02	767
mglmx	62:6fc0c683cb02	768	if(send_pack_switch){
carlosperales95	71:9a303d90c9b8	769
mglmx	62:6fc0c683cb02	770	lcd.cls();
mglmx	62:6fc0c683cb02	771	lcd.printf("Switching SW%d",switch_n);
mglmx	62:6fc0c683cb02	772	flip_switch(switch_n,5);
mglmx	62:6fc0c683cb02	773	}
mglmx	55:aa3baa01f43d	774	}
mglmx	62:6fc0c683cb02	775
mglmx	66:45a90a945983	776	if(switch_n == 3){
carlosperales95	71:9a303d90c9b8	777
mglmx	66:45a90a945983	778	DCC_send_command(SWBaddress,SWBflip_3,15); //Activating switch
mglmx	66:45a90a945983	779	}else{
carlosperales95	71:9a303d90c9b8	780
mglmx	66:45a90a945983	781	flip_switch(5,5); //Send IDLE command
mglmx	66:45a90a945983	782	}
mglmx	66:45a90a945983	783	stop_train->set_speed(MEDIUM);
mglmx	55:aa3baa01f43d	784	}
carlosperales95	45:d589318238bf	785
carlosperales95	71:9a303d90c9b8	786
carlosperales95	68:9859a3ff9298	787	/**
carlosperales95	71:9a303d90c9b8	788	*
carlosperales95	68:9859a3ff9298	789	* Switch_n switch that needs to switch
carlosperales95	68:9859a3ff9298	790	* cont_sensor sensor that when activated the stopped train continues
carlosperales95	68:9859a3ff9298	791	* switch_sensor sensor where the switch should be activated
carlosperales95	71:9a303d90c9b8	792	*
carlosperales95	68:9859a3ff9298	793	*/
carlosperales95	68:9859a3ff9298	794	void ALC_action(int cont_sensor, Train stop_train, Train cont_train ){
carlosperales95	71:9a303d90c9b8	795
carlosperales95	68:9859a3ff9298	796	while(cont_train->get_position_number() != cont_sensor){
carlosperales95	68:9859a3ff9298	797
carlosperales95	68:9859a3ff9298	798	stop_train->set_speed(STOP);
carlosperales95	68:9859a3ff9298	799	stop_train->run(); //Stopping train on sensor D4 or D10
carlosperales95	68:9859a3ff9298	800	cont_train->run();
carlosperales95	68:9859a3ff9298	801	}
carlosperales95	68:9859a3ff9298	802	stop_train->set_speed(MEDIUM);
carlosperales95	68:9859a3ff9298	803	}
mglmx	66:45a90a945983	804
carlosperales95	58:b60db1092088	805
mglmx	43:346a1f4144cd	806	/**
carlosperales95	47:e992a129ef44	807	*
mglmx	55:aa3baa01f43d	808	*The function will check if there is an Avoidable Frontal Collision (AFC).
carlosperales95	47:e992a129ef44	809	*AFC will occur if:
carlosperales95	47:e992a129ef44	810	*
carlosperales95	47:e992a129ef44	811	*Train in area A(ccw) and train in D4(cw)
carlosperales95	47:e992a129ef44	812	*Train in area A(cw) and train in D10(ccw)
carlosperales95	47:e992a129ef44	813	*Train in area B(cw) and train in D4(ccw)
mglmx	62:6fc0c683cb02	814	*Train in area B(ccw) and train in D10(ccw)
mglmx	62:6fc0c683cb02	815	*
mglmx	62:6fc0c683cb02	816	*stop_train is the train that is going to stop in sensors D4 or D10 until the other train passes
mglmx	62:6fc0c683cb02	817	*cont_train is the train that won't stop and will do the switch
carlosperales95	47:e992a129ef44	818	*
mglmx	66:45a90a945983	819	**/
mglmx	62:6fc0c683cb02	820	bool check_AFC(Train stop_train, Train cont_train){ //TODO - Add same for LR train
carlosperales95	73:1bf7f6a38fde	821
mglmx	62:6fc0c683cb02	822	bool detected_AFC = false;
mglmx	62:6fc0c683cb02	823	if( stop_train->get_position_number() == D4){
carlosperales95	45:d589318238bf	824
mglmx	62:6fc0c683cb02	825	if(stop_train->goes_cw()){
carlosperales95	45:d589318238bf	826
mglmx	62:6fc0c683cb02	827	if(cont_train->is_in_A() && !cont_train->goes_cw()){
mglmx	62:6fc0c683cb02	828
carlosperales95	71:9a303d90c9b8	829	detected_AFC = true;
mglmx	62:6fc0c683cb02	830	lcd.cls();
mglmx	62:6fc0c683cb02	831	lcd.printf("AFC!!! STOP D4 SW2 CONT D3");
carlosperales95	45:d589318238bf	832
mglmx	62:6fc0c683cb02	833	AFC_action(2,D3,D2,stop_train,cont_train);
mglmx	62:6fc0c683cb02	834	//Activate switch2
mglmx	62:6fc0c683cb02	835	//When cont_train is at D3 stop_train continues
mglmx	43:346a1f4144cd	836	}
mglmx	43:346a1f4144cd	837	}else{ //DR goes ccw
carlosperales95	45:d589318238bf	838
mglmx	62:6fc0c683cb02	839	if(cont_train->is_in_B() && cont_train->goes_cw()){
carlosperales95	45:d589318238bf	840
carlosperales95	71:9a303d90c9b8	841	detected_AFC = true;
mglmx	62:6fc0c683cb02	842	lcd.cls();
mglmx	62:6fc0c683cb02	843	lcd.printf("AFC!!! STOP D4 SW3 CONT D5");
mglmx	62:6fc0c683cb02	844
mglmx	62:6fc0c683cb02	845	AFC_action(3,D5,D6,stop_train,cont_train);
mglmx	43:346a1f4144cd	846	//DR_train stops
mglmx	43:346a1f4144cd	847	//Activate switch3
mglmx	43:346a1f4144cd	848	//When LR is at D5 DR continues
mglmx	43:346a1f4144cd	849	}
mglmx	43:346a1f4144cd	850	}
mglmx	62:6fc0c683cb02	851	}else if(stop_train->get_position_number() == D10){
carlosperales95	45:d589318238bf	852
mglmx	62:6fc0c683cb02	853	if(stop_train->goes_cw()){
carlosperales95	45:d589318238bf	854
mglmx	62:6fc0c683cb02	855	if(cont_train->is_in_B() && !cont_train->goes_cw()){
carlosperales95	71:9a303d90c9b8	856
carlosperales95	71:9a303d90c9b8	857	detected_AFC = true;
mglmx	62:6fc0c683cb02	858	lcd.cls();
mglmx	62:6fc0c683cb02	859	lcd.printf("AFC!!! STOP D10 SW4 CONT D9");
carlosperales95	45:d589318238bf	860
mglmx	62:6fc0c683cb02	861	AFC_action(4,D9,D8,stop_train,cont_train);
mglmx	43:346a1f4144cd	862	//DR train stops
mglmx	43:346a1f4144cd	863	//Activate switch4
mglmx	43:346a1f4144cd	864	//When LR is at D9 DR continues
mglmx	43:346a1f4144cd	865	}
mglmx	43:346a1f4144cd	866	}else{
carlosperales95	45:d589318238bf	867
mglmx	62:6fc0c683cb02	868	if(cont_train->is_in_A() && cont_train->goes_cw()){
carlosperales95	71:9a303d90c9b8	869
carlosperales95	71:9a303d90c9b8	870	detected_AFC = true;
mglmx	62:6fc0c683cb02	871	lcd.cls();
mglmx	62:6fc0c683cb02	872	lcd.printf("AFC!!! STOP D10 SW1 CONT D11");
mglmx	62:6fc0c683cb02	873	AFC_action(1,D11,D12,stop_train,cont_train);
mglmx	43:346a1f4144cd	874	//DR train stops
mglmx	43:346a1f4144cd	875	//Activate switch1
mglmx	62:6fc0c683cb02	876	//When LR is at D11 DR continues
mglmx	43:346a1f4144cd	877	}
mglmx	43:346a1f4144cd	878	}
carlosperales95	68:9859a3ff9298	879	}else if(stop_train->get_position_number() == D9){
carlosperales95	68:9859a3ff9298	880
carlosperales95	68:9859a3ff9298	881	if(stop_train->goes_cw()){
carlosperales95	68:9859a3ff9298	882
carlosperales95	68:9859a3ff9298	883	if(cont_train->is_in_B() && !cont_train->goes_cw()){
carlosperales95	68:9859a3ff9298	884
carlosperales95	71:9a303d90c9b8	885	detected_AFC = true;
carlosperales95	68:9859a3ff9298	886	lcd.cls();
carlosperales95	68:9859a3ff9298	887	lcd.printf("AFC!!! STOP D9 CONT D8");
carlosperales95	68:9859a3ff9298	888	AFC_action(5, D10, D4, stop_train, cont_train);
carlosperales95	68:9859a3ff9298	889	//train in 9 stops
carlosperales95	71:9a303d90c9b8	890	//when cont_train is at d10 stop train continues
carlosperales95	68:9859a3ff9298	891	}
carlosperales95	68:9859a3ff9298	892	}
carlosperales95	68:9859a3ff9298	893	}else if(stop_train->get_position_number() == D11){
carlosperales95	68:9859a3ff9298	894
carlosperales95	68:9859a3ff9298	895	if(!stop_train->goes_cw()){
carlosperales95	68:9859a3ff9298	896
carlosperales95	68:9859a3ff9298	897	if(cont_train->is_in_A() && cont_train->goes_cw()){
carlosperales95	68:9859a3ff9298	898
carlosperales95	71:9a303d90c9b8	899	detected_AFC = true;
carlosperales95	68:9859a3ff9298	900	lcd.cls();
carlosperales95	68:9859a3ff9298	901	lcd.printf("AFC!!! STOP D11 CONT D12");
carlosperales95	68:9859a3ff9298	902	AFC_action(5, D10, D4, stop_train, cont_train);
carlosperales95	68:9859a3ff9298	903	//train in 11 stops
carlosperales95	68:9859a3ff9298	904	//when cont_train is at d10 stop train continues
carlosperales95	68:9859a3ff9298	905	}
carlosperales95	68:9859a3ff9298	906	}
carlosperales95	68:9859a3ff9298	907	}else if(stop_train->get_position_number() == D3){
carlosperales95	68:9859a3ff9298	908
carlosperales95	68:9859a3ff9298	909	if(stop_train->goes_cw()){
carlosperales95	68:9859a3ff9298	910
carlosperales95	68:9859a3ff9298	911	if(cont_train->is_in_A() && !cont_train->goes_cw()){
carlosperales95	68:9859a3ff9298	912
carlosperales95	71:9a303d90c9b8	913	detected_AFC = true;
carlosperales95	68:9859a3ff9298	914	lcd.cls();
carlosperales95	68:9859a3ff9298	915	lcd.printf("AFC!!! STOP D3 CONT D14");
carlosperales95	68:9859a3ff9298	916	AFC_action(5, D4, D10, stop_train, cont_train);
carlosperales95	68:9859a3ff9298	917	//train in 3 stops
carlosperales95	68:9859a3ff9298	918	//when cont_train is at d4 stop train continues
carlosperales95	68:9859a3ff9298	919	}
carlosperales95	68:9859a3ff9298	920	}
carlosperales95	68:9859a3ff9298	921
carlosperales95	68:9859a3ff9298	922	}else if(stop_train->get_position_number() == D5){
carlosperales95	68:9859a3ff9298	923
carlosperales95	68:9859a3ff9298	924	if(!stop_train->goes_cw()){
carlosperales95	68:9859a3ff9298	925
carlosperales95	68:9859a3ff9298	926	if(cont_train->is_in_B() && cont_train->goes_cw()){
carlosperales95	68:9859a3ff9298	927
carlosperales95	71:9a303d90c9b8	928	detected_AFC = true;
carlosperales95	68:9859a3ff9298	929	lcd.cls();
carlosperales95	68:9859a3ff9298	930	lcd.printf("AFC!!! STOP D5 CONT D6");
carlosperales95	68:9859a3ff9298	931	AFC_action(5, D4, D10, stop_train, cont_train);
carlosperales95	68:9859a3ff9298	932	//train in 5 stops
carlosperales95	68:9859a3ff9298	933	//when cont_train is at d4 stop train continues
carlosperales95	68:9859a3ff9298	934	}
carlosperales95	68:9859a3ff9298	935	}
mglmx	43:346a1f4144cd	936	}
mglmx	62:6fc0c683cb02	937	return detected_AFC;
mglmx	66:45a90a945983	938	}
mglmx	43:346a1f4144cd	939
carlosperales95	45:d589318238bf	940
carlosperales95	45:d589318238bf	941	/**
carlosperales95	45:d589318238bf	942	*
carlosperales95	63:86f8dfa007ac	943	*
carlosperales95	63:86f8dfa007ac	944	*
carlosperales95	68:9859a3ff9298	945	**/
carlosperales95	63:86f8dfa007ac	946	bool check_ALC(Train stop_train, Train cont_train){ //TODO - Add same for LR train
carlosperales95	64:be1015efd974	947
carlosperales95	63:86f8dfa007ac	948	bool detected_ALC = false;
carlosperales95	71:9a303d90c9b8	949
carlosperales95	63:86f8dfa007ac	950	if( stop_train->get_position_number() == D4){
carlosperales95	63:86f8dfa007ac	951
carlosperales95	63:86f8dfa007ac	952	if(stop_train->goes_cw()){
carlosperales95	63:86f8dfa007ac	953
carlosperales95	63:86f8dfa007ac	954	if(cont_train->get_position_number() == D3 && cont_train->goes_cw()){
carlosperales95	63:86f8dfa007ac	955
carlosperales95	71:9a303d90c9b8	956	detected_ALC = true;
carlosperales95	63:86f8dfa007ac	957	lcd.cls();
carlosperales95	63:86f8dfa007ac	958	lcd.printf("ALC!!! STOP D4 CONT D3");
carlosperales95	68:9859a3ff9298	959	ALC_action(2, stop_train, cont_train );
carlosperales95	63:86f8dfa007ac	960	//When cont_train is at D22 stop_train continues
carlosperales95	63:86f8dfa007ac	961	}
carlosperales95	63:86f8dfa007ac	962	}else{ //DR goes ccw
carlosperales95	63:86f8dfa007ac	963
carlosperales95	63:86f8dfa007ac	964	if(cont_train->get_position_number() == D5 && !cont_train->goes_cw()){
carlosperales95	63:86f8dfa007ac	965
carlosperales95	71:9a303d90c9b8	966	detected_ALC = true;
carlosperales95	63:86f8dfa007ac	967	lcd.cls();
carlosperales95	68:9859a3ff9298	968	lcd.printf("ALC!!! STOP D4 SW3 CONT D5");
carlosperales95	68:9859a3ff9298	969	ALC_action(7, stop_train, cont_train );
carlosperales95	63:86f8dfa007ac	970	//Train stops
carlosperales95	63:86f8dfa007ac	971	//When CONT is at D6 DR continues
carlosperales95	63:86f8dfa007ac	972	}
carlosperales95	63:86f8dfa007ac	973	}
carlosperales95	63:86f8dfa007ac	974	}else if(stop_train->get_position_number() == D10){
carlosperales95	63:86f8dfa007ac	975
carlosperales95	63:86f8dfa007ac	976	if(stop_train->goes_cw()){
carlosperales95	63:86f8dfa007ac	977
carlosperales95	64:be1015efd974	978	if(cont_train->get_position_number() == D9 && cont_train->goes_cw()){
carlosperales95	63:86f8dfa007ac	979
carlosperales95	71:9a303d90c9b8	980	detected_ALC = true;
carlosperales95	63:86f8dfa007ac	981	lcd.cls();
carlosperales95	68:9859a3ff9298	982	lcd.printf("ALC!!! STOP D10 CONT D9");
carlosperales95	68:9859a3ff9298	983	ALC_action(8, stop_train, cont_train );
carlosperales95	63:86f8dfa007ac	984	//D10 train stops
carlosperales95	63:86f8dfa007ac	985	//When CONT is at D8, D10 continues
carlosperales95	63:86f8dfa007ac	986	}
carlosperales95	63:86f8dfa007ac	987	}else{
carlosperales95	63:86f8dfa007ac	988
carlosperales95	64:be1015efd974	989	if(cont_train->get_position_number() == D11 && !cont_train->goes_cw()){
carlosperales95	63:86f8dfa007ac	990
carlosperales95	71:9a303d90c9b8	991	detected_ALC = true;
carlosperales95	63:86f8dfa007ac	992	lcd.cls();
carlosperales95	68:9859a3ff9298	993	lcd.printf("ALC!!! STOP D10 CONT D11");
carlosperales95	68:9859a3ff9298	994	ALC_action(12, stop_train, cont_train );
carlosperales95	63:86f8dfa007ac	995	//D10 train stops
carlosperales95	63:86f8dfa007ac	996	//When CONT is at D12, D10 continues
carlosperales95	63:86f8dfa007ac	997	}
carlosperales95	63:86f8dfa007ac	998	}
carlosperales95	63:86f8dfa007ac	999	}
carlosperales95	64:be1015efd974	1000	return detected_ALC;
carlosperales95	63:86f8dfa007ac	1001	}
carlosperales95	64:be1015efd974	1002
carlosperales95	64:be1015efd974	1003
carlosperales95	63:86f8dfa007ac	1004	/**
carlosperales95	63:86f8dfa007ac	1005	*
carlosperales95	74:dbbc528f2b18	1006	*
carlosperales95	74:dbbc528f2b18	1007	*
carlosperales95	74:dbbc528f2b18	1008	**/
carlosperales95	74:dbbc528f2b18	1009	void stay_at_distance(Train* train_front, Train* train_back){
carlosperales95	74:dbbc528f2b18	1010
carlosperales95	74:dbbc528f2b18	1011	led1 = 0;
carlosperales95	74:dbbc528f2b18	1012	led2 = 0;
carlosperales95	74:dbbc528f2b18	1013	led3 = 0;
carlosperales95	74:dbbc528f2b18	1014
carlosperales95	74:dbbc528f2b18	1015	for(int i=0; i<train_back->get_next_sensors().size(); i++){
carlosperales95	74:dbbc528f2b18	1016
carlosperales95	74:dbbc528f2b18	1017	for(int j=0; j<train_front->get_previous_sensors().size(); j++){
carlosperales95	74:dbbc528f2b18	1018
carlosperales95	74:dbbc528f2b18	1019	while(train_back->get_next_sensors()[i] == train_front->get_previous_sensors()[j]){
carlosperales95	74:dbbc528f2b18	1020
carlosperales95	75:bae748a7905f	1021	//lcd.cls();
carlosperales95	75:bae748a7905f	1022	//lcd.printf("TOO CLOSE! S %d", train_back->get_next_sensors()[i]);
carlosperales95	75:bae748a7905f	1023	led1 = 1;
carlosperales95	74:dbbc528f2b18	1024	led3 = 1;
carlosperales95	74:dbbc528f2b18	1025	//stop back train
carlosperales95	74:dbbc528f2b18	1026	train_back->set_speed(STOP);
carlosperales95	75:bae748a7905f	1027	train_back->run();
carlosperales95	75:bae748a7905f	1028	train_front->run();
carlosperales95	75:bae748a7905f	1029	led1 = 0;
carlosperales95	75:bae748a7905f	1030	led2 = 0;
carlosperales95	75:bae748a7905f	1031	led3 = 0;
carlosperales95	75:bae748a7905f	1032	}
carlosperales95	75:bae748a7905f	1033	led1 = 1;
carlosperales95	75:bae748a7905f	1034	led2 = 1;
carlosperales95	75:bae748a7905f	1035	led3 = 1;
carlosperales95	75:bae748a7905f	1036	train_back->set_speed(MEDIUM);
carlosperales95	75:bae748a7905f	1037	train_back->run();
carlosperales95	74:dbbc528f2b18	1038	}
carlosperales95	74:dbbc528f2b18	1039	}
carlosperales95	74:dbbc528f2b18	1040	led1 = 0;
carlosperales95	74:dbbc528f2b18	1041	led2 = 0;
carlosperales95	74:dbbc528f2b18	1042	led3 = 0;
carlosperales95	74:dbbc528f2b18	1043	}
carlosperales95	74:dbbc528f2b18	1044
carlosperales95	74:dbbc528f2b18	1045
carlosperales95	74:dbbc528f2b18	1046
carlosperales95	74:dbbc528f2b18	1047	/**
carlosperales95	74:dbbc528f2b18	1048	*
carlosperales95	47:e992a129ef44	1049	*The method check_position will check if any of the trains is in any of the areas.
carlosperales95	47:e992a129ef44	1050	*It will go through all the area vectors (A,B) and call the function in_vector to check inside the vectors.
carlosperales95	45:d589318238bf	1051	*
carlosperales95	45:d589318238bf	1052	**/
mglmx	57:ee5da8a011e0	1053	void check_position(){
carlosperales95	74:dbbc528f2b18	1054
carlosperales95	75:bae748a7905f	1055	//stay_at_distance(&DR_train,&LR_train);
carlosperales95	75:bae748a7905f	1056	//stay_at_distance(&LR_train,&DR_train);
carlosperales95	74:dbbc528f2b18	1057
carlosperales95	74:dbbc528f2b18	1058	if(check_NAC()){ NAC_action(); }
carlosperales95	71:9a303d90c9b8	1059
carlosperales95	68:9859a3ff9298	1060	check_AFC(&DR_train,&LR_train);
carlosperales95	68:9859a3ff9298	1061	check_AFC(&LR_train,&DR_train);
carlosperales95	68:9859a3ff9298	1062	check_ALC(&LR_train,&DR_train);
carlosperales95	68:9859a3ff9298	1063	check_ALC(&DR_train,&LR_train);
mglmx	43:346a1f4144cd	1064	}
mglmx	43:346a1f4144cd	1065
carlosperales95	45:d589318238bf	1066
carlosperales95	45:d589318238bf	1067	/**
carlosperales95	45:d589318238bf	1068	*
carlosperales95	72:a1bd95af46b8	1069	*
carlosperales95	72:a1bd95af46b8	1070	*
carlosperales95	72:a1bd95af46b8	1071	**/
carlosperales95	72:a1bd95af46b8	1072	void printPos(int sensor){
carlosperales95	72:a1bd95af46b8	1073
carlosperales95	72:a1bd95af46b8	1074	string DR_dir,LR_dir;
carlosperales95	72:a1bd95af46b8	1075
carlosperales95	72:a1bd95af46b8	1076	if(DR_train.goes_cw()){ DR_dir = "cw";}
carlosperales95	72:a1bd95af46b8	1077	else{DR_dir = "ccw";}
carlosperales95	72:a1bd95af46b8	1078
carlosperales95	72:a1bd95af46b8	1079	if(LR_train.goes_cw()){LR_dir = "cw";}
carlosperales95	72:a1bd95af46b8	1080	else{LR_dir = "ccw";}
carlosperales95	72:a1bd95af46b8	1081
carlosperales95	72:a1bd95af46b8	1082	lcd.cls();
carlosperales95	72:a1bd95af46b8	1083	lcd.printf("S:D%d DR%d(",sensor,DR_train.get_position_number());
carlosperales95	72:a1bd95af46b8	1084
carlosperales95	72:a1bd95af46b8	1085	for(int i=0; i<DR_train.get_next_sensors().size(); i++){
carlosperales95	72:a1bd95af46b8	1086
carlosperales95	72:a1bd95af46b8	1087	lcd.printf("%d,",DR_train.get_next_sensors()[i]);
carlosperales95	72:a1bd95af46b8	1088	}
carlosperales95	72:a1bd95af46b8	1089
carlosperales95	72:a1bd95af46b8	1090	lcd.printf(")%s LR%d(",DR_dir,LR_train.get_position_number());
carlosperales95	72:a1bd95af46b8	1091
carlosperales95	72:a1bd95af46b8	1092	for(int i=0; i<LR_train.get_next_sensors().size(); i++){
carlosperales95	72:a1bd95af46b8	1093
carlosperales95	72:a1bd95af46b8	1094	lcd.printf("%d,",LR_train.get_next_sensors()[i]);
carlosperales95	72:a1bd95af46b8	1095	}
carlosperales95	72:a1bd95af46b8	1096	lcd.printf(")%s",LR_dir);
carlosperales95	72:a1bd95af46b8	1097	}
carlosperales95	72:a1bd95af46b8	1098
carlosperales95	72:a1bd95af46b8	1099
carlosperales95	72:a1bd95af46b8	1100	/**
carlosperales95	72:a1bd95af46b8	1101	*
carlosperales95	47:e992a129ef44	1102	*Description
carlosperales95	45:d589318238bf	1103	*
carlosperales95	47:e992a129ef44	1104	*@sensor -
carlosperales95	45:d589318238bf	1105	*
carlosperales95	45:d589318238bf	1106	**/
mglmx	35:cfcfeccb959e	1107	void update_train_pos(int sensor){
carlosperales95	70:594fe5c1ad5c	1108
carlosperales95	71:9a303d90c9b8	1109	//bool found_DR = false;
carlosperales95	71:9a303d90c9b8	1110	//bool found_LR = false;
mglmx	49:880c0b9c9c64	1111
carlosperales95	71:9a303d90c9b8	1112
mglmx	55:aa3baa01f43d	1113	if(sensor == DR_train.get_position_number() \|\| sensor == LR_train.get_position_number()){
carlosperales95	71:9a303d90c9b8	1114	redled = 1;
carlosperales95	71:9a303d90c9b8	1115	}else{
carlosperales95	71:9a303d90c9b8	1116
carlosperales95	71:9a303d90c9b8	1117	redled = 0;
carlosperales95	72:a1bd95af46b8	1118	printPos(sensor);
mglmx	55:aa3baa01f43d	1119
mglmx	52:c08495446f87	1120	//Checking next sensors for DR train
mglmx	55:aa3baa01f43d	1121	for(int i=0; i<DR_train.get_next_sensors().size(); i++){
mglmx	55:aa3baa01f43d	1122
mglmx	55:aa3baa01f43d	1123	if(DR_train.get_next_sensors()[i] == sensor){ //If the sensor is one expected to visit by the train we update the position
carlosperales95	37:bb15bea420a3	1124
carlosperales95	71:9a303d90c9b8	1125	//found_DR = true;
mglmx	55:aa3baa01f43d	1126
mglmx	55:aa3baa01f43d	1127	if(DR_train.goes_cw()){
mglmx	55:aa3baa01f43d	1128	if(DR_train.get_position_number() == D5 \|\| DR_train.get_position_number() == D11){
mglmx	55:aa3baa01f43d	1129
mglmx	55:aa3baa01f43d	1130	DR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
mglmx	55:aa3baa01f43d	1131	}
mglmx	55:aa3baa01f43d	1132	}else{
mglmx	55:aa3baa01f43d	1133
mglmx	55:aa3baa01f43d	1134	if(DR_train.get_position_number() == D9 \|\| DR_train.get_position_number() == D3){
mglmx	55:aa3baa01f43d	1135
mglmx	55:aa3baa01f43d	1136	DR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
mglmx	55:aa3baa01f43d	1137	}
mglmx	55:aa3baa01f43d	1138	}
mglmx	55:aa3baa01f43d	1139
mglmx	55:aa3baa01f43d	1140	DR_train.set_position(sensor);
mglmx	55:aa3baa01f43d	1141
mglmx	55:aa3baa01f43d	1142	}
mglmx	55:aa3baa01f43d	1143	}
mglmx	55:aa3baa01f43d	1144
mglmx	55:aa3baa01f43d	1145	//Checking next sensors for LR train
mglmx	55:aa3baa01f43d	1146	for(int i=0; i<LR_train.get_next_sensors().size(); i++){
mglmx	55:aa3baa01f43d	1147
mglmx	55:aa3baa01f43d	1148	if(LR_train.get_next_sensors()[i] == sensor){
carlosperales95	45:d589318238bf	1149
carlosperales95	71:9a303d90c9b8	1150	//found_LR = true;
mglmx	55:aa3baa01f43d	1151
mglmx	55:aa3baa01f43d	1152	if(LR_train.goes_cw()){
carlosperales95	45:d589318238bf	1153
mglmx	55:aa3baa01f43d	1154	if(LR_train.get_position_number() == D5 \|\| LR_train.get_position_number() == D11){
mglmx	55:aa3baa01f43d	1155	LR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
mglmx	55:aa3baa01f43d	1156	}
mglmx	55:aa3baa01f43d	1157	}else{
mglmx	55:aa3baa01f43d	1158
mglmx	55:aa3baa01f43d	1159	if(LR_train.get_position_number() == D9 \|\| LR_train.get_position_number() == D3 ){
mglmx	55:aa3baa01f43d	1160
mglmx	55:aa3baa01f43d	1161	LR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
mglmx	55:aa3baa01f43d	1162	}
mglmx	41:4fa6aa29d1ed	1163	}
carlosperales95	74:dbbc528f2b18	1164	LR_train.set_position(sensor);
carlosperales95	74:dbbc528f2b18	1165
carlosperales95	74:dbbc528f2b18	1166	if(sensor == D2 && station_stop){
carlosperales95	74:dbbc528f2b18	1167
carlosperales95	74:dbbc528f2b18	1168	DR_train.set_speed(STOP);
carlosperales95	74:dbbc528f2b18	1169	LR_train.set_speed(STOP);
carlosperales95	74:dbbc528f2b18	1170	}
mglmx	55:aa3baa01f43d	1171	}
mglmx	55:aa3baa01f43d	1172	}
carlosperales95	71:9a303d90c9b8	1173	}
carlosperales95	71:9a303d90c9b8	1174	}
carlosperales95	71:9a303d90c9b8	1175
carlosperales95	71:9a303d90c9b8	1176
carlosperales95	71:9a303d90c9b8	1177	/**
carlosperales95	71:9a303d90c9b8	1178	*
carlosperales95	25:a42a1ed4d8e9	1179	*Method to catch interrupts 0
carlosperales95	25:a42a1ed4d8e9	1180	*
carlosperales95	24:1d71dd8778c4	1181	**/
mglmx	33:24ce12dec157	1182	void on_int0_change(){
carlosperales95	25:a42a1ed4d8e9	1183
mglmx	33:24ce12dec157	1184	wait_us(2000);
mglmx	33:24ce12dec157	1185	int sensor_data = mcp->_read(INTCAPA);
mglmx	34:c9ab2a987734	1186	int sensor = get_sensor(sensor_data,0);
mglmx	35:cfcfeccb959e	1187	update_train_pos(sensor);
mglmx	16:2a2da0e67793	1188	}
mglmx	16:2a2da0e67793	1189
mglmx	35:cfcfeccb959e	1190
carlosperales95	24:1d71dd8778c4	1191	/**
carlosperales95	24:1d71dd8778c4	1192	*
carlosperales95	25:a42a1ed4d8e9	1193	*Method to catch interrupts 1
carlosperales95	25:a42a1ed4d8e9	1194	*
carlosperales95	24:1d71dd8778c4	1195	**/
mglmx	33:24ce12dec157	1196	void on_int1_change(){
carlosperales95	25:a42a1ed4d8e9	1197
mglmx	33:24ce12dec157	1198	wait_us(2000);
mglmx	33:24ce12dec157	1199	int sensor_data = mcp->_read(INTCAPB);
mglmx	34:c9ab2a987734	1200	int sensor = get_sensor(sensor_data,1);
mglmx	35:cfcfeccb959e	1201	update_train_pos(sensor);
mglmx	16:2a2da0e67793	1202	}
mglmx	26:5c966a0a3e8e	1203
carlosperales95	71:9a303d90c9b8	1204
carlosperales95	45:d589318238bf	1205	/**
carlosperales95	45:d589318238bf	1206	*
carlosperales95	47:e992a129ef44	1207	*Clear current interrupts
carlosperales95	45:d589318238bf	1208	*
carlosperales95	45:d589318238bf	1209	**/
mglmx	62:6fc0c683cb02	1210	void init() {
mglmx	26:5c966a0a3e8e	1211
mglmx	33:24ce12dec157	1212	mcp->_read(GPIOA);
mglmx	33:24ce12dec157	1213	mcp->_read(GPIOB); // Register callbacks
mglmx	33:24ce12dec157	1214	int0.fall(&on_int0_change);
mglmx	33:24ce12dec157	1215	int1.fall(&on_int1_change); // Enable interrupts on MCP
mglmx	33:24ce12dec157	1216	mcp->_write(GPINTENA, (unsigned char )0xff);
mglmx	33:24ce12dec157	1217	mcp->_write(GPINTENB, (unsigned char )0xff); // Ready to go!
mglmx	62:6fc0c683cb02	1218	}
mglmx	62:6fc0c683cb02	1219
carlosperales95	11:021210c59a95	1220
carlosperales95	24:1d71dd8778c4	1221	/**
carlosperales95	24:1d71dd8778c4	1222	*
carlosperales95	29:559eb2164488	1223	*Checks if any of the switches of the box has been activated.
carlosperales95	29:559eb2164488	1224	*Calls necessary function and displays LCD text.
carlosperales95	25:a42a1ed4d8e9	1225	*
carlosperales95	24:1d71dd8778c4	1226	**/
mglmx	21:e6f1649add39	1227	void checkSwitch(){
carlosperales95	24:1d71dd8778c4	1228
mglmx	22:e4153ca757dd	1229	if(switch1 == 1){
carlosperales95	24:1d71dd8778c4	1230
carlosperales95	69:609c69e91b4e	1231	//lcd.cls();
carlosperales95	69:609c69e91b4e	1232	//lcd.printf("TRAIN NOW WILL STOP AT STATION");
carlosperales95	74:dbbc528f2b18	1233	station_stop = true;
carlosperales95	74:dbbc528f2b18	1234	}else{
carlosperales95	74:dbbc528f2b18	1235
carlosperales95	74:dbbc528f2b18	1236	station_stop = false;
carlosperales95	74:dbbc528f2b18	1237	LR_train.set_speed(MEDIUM);
carlosperales95	74:dbbc528f2b18	1238	DR_train.set_speed(MEDIUM);
carlosperales95	74:dbbc528f2b18	1239	}
carlosperales95	74:dbbc528f2b18	1240	if(switch2 == 1){
carlosperales95	24:1d71dd8778c4	1241
carlosperales95	69:609c69e91b4e	1242	//lcd.cls();
carlosperales95	69:609c69e91b4e	1243	//lcd.printf("SPEEDCHECKMODE");
carlosperales95	69:609c69e91b4e	1244	//change to speedcheckmode
carlosperales95	69:609c69e91b4e	1245	speedcheck = 1;
carlosperales95	69:609c69e91b4e	1246	}else{
carlosperales95	69:609c69e91b4e	1247
carlosperales95	69:609c69e91b4e	1248	//lcd.cls();
carlosperales95	69:609c69e91b4e	1249	//lcd.printf("NORMAL MODE");
carlosperales95	69:609c69e91b4e	1250	speedcheck = 0;
carlosperales95	69:609c69e91b4e	1251	}if(switch3 == 0){
carlosperales95	24:1d71dd8778c4	1252
carlosperales95	69:609c69e91b4e	1253	if(speedcheck>0){
carlosperales95	69:609c69e91b4e	1254
carlosperales95	69:609c69e91b4e	1255	//lcd.cls();
carlosperales95	69:609c69e91b4e	1256	//lcd.printf("SPEED TRAIN DARK RED");
carlosperales95	69:609c69e91b4e	1257	speedcheck = 2;
carlosperales95	69:609c69e91b4e	1258	}
carlosperales95	24:1d71dd8778c4	1259	}else if(switch4 == 0){
carlosperales95	69:609c69e91b4e	1260
carlosperales95	69:609c69e91b4e	1261	if(speedcheck>0){
carlosperales95	69:609c69e91b4e	1262
carlosperales95	69:609c69e91b4e	1263	//lcd.cls();
carlosperales95	69:609c69e91b4e	1264	//lcd.printf("SPEED TRAIN LIGHT RED");
carlosperales95	69:609c69e91b4e	1265	speedcheck = 3;
carlosperales95	69:609c69e91b4e	1266	}
carlosperales95	24:1d71dd8778c4	1267	}
mglmx	22:e4153ca757dd	1268	}
mglmx	22:e4153ca757dd	1269
carlosperales95	45:d589318238bf	1270
mglmx	55:aa3baa01f43d	1271	/**
carlosperales95	71:9a303d90c9b8	1272	*
mglmx	55:aa3baa01f43d	1273	* Returns a sensor number depending on how many times switch3 flips.
mglmx	55:aa3baa01f43d	1274	* When pressing switch4 it confirms the switch
mglmx	55:aa3baa01f43d	1275	* Init_sensor is the value where we start counting.
mglmx	55:aa3baa01f43d	1276	* string train is the name of the train that will be prited with the sensor
carlosperales95	71:9a303d90c9b8	1277	*
mglmx	55:aa3baa01f43d	1278	*/
mglmx	55:aa3baa01f43d	1279	int select_sensor(int init_sensor, string train){
mglmx	55:aa3baa01f43d	1280
mglmx	55:aa3baa01f43d	1281	lcd.cls();
mglmx	57:ee5da8a011e0	1282	lcd.printf("%s SENSOR D%d",train,init_sensor);
mglmx	55:aa3baa01f43d	1283
mglmx	55:aa3baa01f43d	1284	int sensor = init_sensor;
mglmx	52:c08495446f87	1285	bool changed = false;
mglmx	52:c08495446f87	1286	bool exit = false;
mglmx	52:c08495446f87	1287
mglmx	52:c08495446f87	1288	while(!exit){
carlosperales95	71:9a303d90c9b8	1289
mglmx	52:c08495446f87	1290	if(switch3 == 0){
carlosperales95	71:9a303d90c9b8	1291
mglmx	52:c08495446f87	1292	if(changed){
carlosperales95	71:9a303d90c9b8	1293
mglmx	52:c08495446f87	1294	sensor++;
mglmx	52:c08495446f87	1295	sensor=sensor%15; //Only sensors from 0 to 15.
mglmx	52:c08495446f87	1296	changed=false;
mglmx	55:aa3baa01f43d	1297	lcd.cls();
mglmx	55:aa3baa01f43d	1298	lcd.printf("%s: D%d",train,sensor);
mglmx	52:c08495446f87	1299	}
carlosperales95	71:9a303d90c9b8	1300	}else{
mglmx	62:6fc0c683cb02	1301
mglmx	52:c08495446f87	1302	changed = true;
mglmx	55:aa3baa01f43d	1303	wait(0.2);
mglmx	52:c08495446f87	1304	}
mglmx	52:c08495446f87	1305	if(switch4 == 0){
carlosperales95	71:9a303d90c9b8	1306
mglmx	52:c08495446f87	1307	exit = true;
mglmx	55:aa3baa01f43d	1308	wait(0.2);
mglmx	52:c08495446f87	1309	}
mglmx	52:c08495446f87	1310	}
mglmx	52:c08495446f87	1311	return sensor;
mglmx	52:c08495446f87	1312	}
carlosperales95	45:d589318238bf	1313
carlosperales95	69:609c69e91b4e	1314
mglmx	55:aa3baa01f43d	1315	/**
mglmx	55:aa3baa01f43d	1316	* Returns a boolean representing the direction. Everytimew switch3 is 0 it changes the direction.
mglmx	55:aa3baa01f43d	1317	* When switch4 is 0 the selection is confirmed.
mglmx	55:aa3baa01f43d	1318	* Init_going_cw is the initial direction.
mglmx	55:aa3baa01f43d	1319	* Train is the string with the name of the train that will be printed next to the direction
mglmx	55:aa3baa01f43d	1320	*/
mglmx	55:aa3baa01f43d	1321	bool select_direction(bool init_going_cw,string train){
mglmx	55:aa3baa01f43d	1322
mglmx	57:ee5da8a011e0	1323	string dir_string;
mglmx	57:ee5da8a011e0	1324
carlosperales95	71:9a303d90c9b8	1325	if(init_going_cw){dir_string = "cw";}
carlosperales95	71:9a303d90c9b8	1326	else{dir_string = "ccw";}
mglmx	57:ee5da8a011e0	1327
mglmx	55:aa3baa01f43d	1328	lcd.cls();
mglmx	57:ee5da8a011e0	1329	lcd.printf("%s DIRECTION %s ",train,dir_string);
mglmx	57:ee5da8a011e0	1330
mglmx	55:aa3baa01f43d	1331	bool exit = false;
mglmx	55:aa3baa01f43d	1332	bool going_cw = init_going_cw;
mglmx	55:aa3baa01f43d	1333	bool changed = false;
mglmx	55:aa3baa01f43d	1334
mglmx	55:aa3baa01f43d	1335	while(!exit){
carlosperales95	71:9a303d90c9b8	1336
mglmx	55:aa3baa01f43d	1337	if(switch3 == 0){
carlosperales95	71:9a303d90c9b8	1338
mglmx	55:aa3baa01f43d	1339	if(changed){
carlosperales95	71:9a303d90c9b8	1340
mglmx	55:aa3baa01f43d	1341	going_cw = !going_cw;
mglmx	55:aa3baa01f43d	1342	changed = false;
mglmx	55:aa3baa01f43d	1343	string dir;
carlosperales95	71:9a303d90c9b8	1344
carlosperales95	71:9a303d90c9b8	1345	if(going_cw){dir = "cw";}
carlosperales95	71:9a303d90c9b8	1346	else{dir = "ccw";}
carlosperales95	71:9a303d90c9b8	1347
mglmx	55:aa3baa01f43d	1348	lcd.cls();
mglmx	55:aa3baa01f43d	1349	lcd.printf("%s: %s",train,dir);
mglmx	55:aa3baa01f43d	1350	}
mglmx	55:aa3baa01f43d	1351	}else{
carlosperales95	71:9a303d90c9b8	1352
mglmx	55:aa3baa01f43d	1353	changed = true;
mglmx	55:aa3baa01f43d	1354	wait(0.2);
mglmx	55:aa3baa01f43d	1355	}
mglmx	55:aa3baa01f43d	1356	if(switch4 == 0){
carlosperales95	71:9a303d90c9b8	1357
mglmx	55:aa3baa01f43d	1358	exit = true;
mglmx	55:aa3baa01f43d	1359	wait(0.2);
mglmx	55:aa3baa01f43d	1360	}
mglmx	55:aa3baa01f43d	1361	}
mglmx	55:aa3baa01f43d	1362	return going_cw;
mglmx	55:aa3baa01f43d	1363	}
mglmx	55:aa3baa01f43d	1364
carlosperales95	69:609c69e91b4e	1365
carlosperales95	69:609c69e91b4e	1366	/**
carlosperales95	69:609c69e91b4e	1367	*
carlosperales95	69:609c69e91b4e	1368	*
carlosperales95	69:609c69e91b4e	1369	*
carlosperales95	69:609c69e91b4e	1370	**/
carlosperales95	69:609c69e91b4e	1371	void adjustSpeed(Train* train){
carlosperales95	69:609c69e91b4e	1372
carlosperales95	69:609c69e91b4e	1373	float f = pot.read();
carlosperales95	69:609c69e91b4e	1374	float vin = f * 3.3;
carlosperales95	69:609c69e91b4e	1375
carlosperales95	69:609c69e91b4e	1376	if(0<=vin && vin<0.60){
carlosperales95	69:609c69e91b4e	1377
carlosperales95	69:609c69e91b4e	1378	//speed = slow
carlosperales95	69:609c69e91b4e	1379	train->set_speed(SLOW);
carlosperales95	69:609c69e91b4e	1380	}else if(0.60<vin && vin<1.20){
carlosperales95	69:609c69e91b4e	1381
carlosperales95	69:609c69e91b4e	1382	//speed medium
carlosperales95	69:609c69e91b4e	1383	train->set_speed(MEDIUM);
carlosperales95	69:609c69e91b4e	1384	}else if(1.20<vin && vin<2.20){
carlosperales95	69:609c69e91b4e	1385
carlosperales95	69:609c69e91b4e	1386	//speed fast
carlosperales95	69:609c69e91b4e	1387	train->set_speed(FAST);
carlosperales95	69:609c69e91b4e	1388	}else if(2.20<vin && vin<3.20){
carlosperales95	69:609c69e91b4e	1389
carlosperales95	69:609c69e91b4e	1390	//speed full
carlosperales95	69:609c69e91b4e	1391	train->set_speed(FULL);
carlosperales95	69:609c69e91b4e	1392	}
carlosperales95	69:609c69e91b4e	1393	}
carlosperales95	69:609c69e91b4e	1394
carlosperales95	71:9a303d90c9b8	1395	/**
carlosperales95	71:9a303d90c9b8	1396	*
carlosperales95	71:9a303d90c9b8	1397	*
carlosperales95	71:9a303d90c9b8	1398	*
carlosperales95	71:9a303d90c9b8	1399	**/
carlosperales95	69:609c69e91b4e	1400	void switch_toSpeed(){
carlosperales95	69:609c69e91b4e	1401
carlosperales95	69:609c69e91b4e	1402	switch(speedcheck){
carlosperales95	73:1bf7f6a38fde	1403
carlosperales95	73:1bf7f6a38fde	1404	case 1:
carlosperales95	73:1bf7f6a38fde	1405	adjustSpeed(&DR_train);
carlosperales95	73:1bf7f6a38fde	1406	adjustSpeed(&LR_train);
carlosperales95	73:1bf7f6a38fde	1407
carlosperales95	69:609c69e91b4e	1408	case 2:
carlosperales95	69:609c69e91b4e	1409	adjustSpeed(&DR_train);
carlosperales95	69:609c69e91b4e	1410
carlosperales95	69:609c69e91b4e	1411	case 3:
carlosperales95	69:609c69e91b4e	1412	adjustSpeed(&LR_train);
carlosperales95	69:609c69e91b4e	1413
carlosperales95	69:609c69e91b4e	1414	}
carlosperales95	69:609c69e91b4e	1415	}
carlosperales95	69:609c69e91b4e	1416
carlosperales95	71:9a303d90c9b8	1417
carlosperales95	71:9a303d90c9b8	1418
carlosperales95	11:021210c59a95	1419	//************** MAIN PROGRAM FOR DENVER TRAIN **************//
carlosperales95	11:021210c59a95	1420
carlosperales95	45:d589318238bf	1421
mglmx	1:0ab26889af9b	1422	int main()
mglmx	1:0ab26889af9b	1423	{
carlosperales95	25:a42a1ed4d8e9	1424
carlosperales95	56:fe999806787d	1425	enable = 0;
carlosperales95	25:a42a1ed4d8e9	1426
carlosperales95	25:a42a1ed4d8e9	1427	//Led routine to start main program
mglmx	22:e4153ca757dd	1428	wait(0.2);
mglmx	2:f580707c44fa	1429	led1 = 1;
mglmx	16:2a2da0e67793	1430
mglmx	55:aa3baa01f43d	1431	init_positions();
carlosperales95	25:a42a1ed4d8e9	1432	initialize_mcp(); //mcp initialization for interrupts before train running
mglmx	33:24ce12dec157	1433	init();
mglmx	57:ee5da8a011e0	1434
carlosperales95	75:bae748a7905f	1435	int DR_init_sensor = select_sensor(D4,"DR");
carlosperales95	75:bae748a7905f	1436	bool DR_init_dir = select_direction(false,"DR");
mglmx	55:aa3baa01f43d	1437
mglmx	55:aa3baa01f43d	1438	wait(0.5);
mglmx	55:aa3baa01f43d	1439
carlosperales95	75:bae748a7905f	1440	int LR_init_sensor = select_sensor(D10,"LR");
carlosperales95	75:bae748a7905f	1441	bool LR_init_dir = select_direction(false,"LR");
mglmx	55:aa3baa01f43d	1442
mglmx	55:aa3baa01f43d	1443	DR_train.set_position(DR_init_sensor);
mglmx	55:aa3baa01f43d	1444	DR_train.set_goes_cw(DR_init_dir);
mglmx	55:aa3baa01f43d	1445
mglmx	55:aa3baa01f43d	1446	LR_train.set_position(LR_init_sensor);
mglmx	55:aa3baa01f43d	1447	LR_train.set_goes_cw(LR_init_dir);
mglmx	55:aa3baa01f43d	1448
mglmx	55:aa3baa01f43d	1449	string DR_print_dir, LR_print_dir;
mglmx	55:aa3baa01f43d	1450
carlosperales95	71:9a303d90c9b8	1451	if(DR_train.goes_cw()){DR_print_dir = "cw";}
carlosperales95	71:9a303d90c9b8	1452	else{DR_print_dir = "ccw";}
mglmx	55:aa3baa01f43d	1453
carlosperales95	71:9a303d90c9b8	1454	if(LR_train.goes_cw()){LR_print_dir = "cw";}
carlosperales95	71:9a303d90c9b8	1455	else{LR_print_dir = "ccw";}
mglmx	55:aa3baa01f43d	1456
mglmx	55:aa3baa01f43d	1457	lcd.cls();
mglmx	55:aa3baa01f43d	1458	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);
mglmx	55:aa3baa01f43d	1459
mglmx	55:aa3baa01f43d	1460	wait(2);
mglmx	55:aa3baa01f43d	1461
carlosperales95	71:9a303d90c9b8	1462	//LED2+LED3 Shows start of route + LCD notif
carlosperales95	71:9a303d90c9b8	1463	led2 = 1; // Entering the while
carlosperales95	71:9a303d90c9b8	1464	wait(0.4);
mglmx	22:e4153ca757dd	1465	led3 = 1; // Entering the while
carlosperales95	71:9a303d90c9b8	1466	wait(0.4);
carlosperales95	71:9a303d90c9b8	1467
mglmx	22:e4153ca757dd	1468	lcd.cls();
mglmx	22:e4153ca757dd	1469	lcd.printf("Ready to start");
mglmx	55:aa3baa01f43d	1470	wait(1);
carlosperales95	19:ff21ba3a4dc5	1471
carlosperales95	56:fe999806787d	1472	enable = 1;
carlosperales95	56:fe999806787d	1473
carlosperales95	71:9a303d90c9b8	1474	led1 = 0;
carlosperales95	71:9a303d90c9b8	1475	led2 = 0;
carlosperales95	71:9a303d90c9b8	1476	led3 = 0;
carlosperales95	71:9a303d90c9b8	1477
carlosperales95	71:9a303d90c9b8	1478	//Demo for stopping at the station
carlosperales95	71:9a303d90c9b8	1479	while(1) {
carlosperales95	75:bae748a7905f	1480
carlosperales95	75:bae748a7905f	1481	DR_train.run();
carlosperales95	75:bae748a7905f	1482	LR_train.run();
carlosperales95	75:bae748a7905f	1483	check_position();
carlosperales95	75:bae748a7905f	1484	//checkSwitch(); //Checks for switch commands everytime.
carlosperales95	75:bae748a7905f	1485	//switch_toSpeed();
carlosperales95	74:dbbc528f2b18	1486
carlosperales95	72:a1bd95af46b8	1487	}
carlosperales95	71:9a303d90c9b8	1488	}
carlosperales95	71:9a303d90c9b8	1489
carlosperales95	71:9a303d90c9b8	1490
carlosperales95	71:9a303d90c9b8	1491	//********************SAVED CODE CHUNKS**************************//
carlosperales95	71:9a303d90c9b8	1492
carlosperales95	71:9a303d90c9b8	1493
carlosperales95	71:9a303d90c9b8	1494	/**Flip switch-idle routine
carlosperales95	71:9a303d90c9b8	1495
mglmx	66:45a90a945983	1496	flip_switch(5,15);//Send IDLE command at the beginning
mglmx	66:45a90a945983	1497	flip_switch(1,40);
mglmx	66:45a90a945983	1498	wait(0.5);
mglmx	66:45a90a945983	1499	flip_switch(5,15);
mglmx	66:45a90a945983	1500	flip_switch(2,15);
mglmx	66:45a90a945983	1501	wait(0.5);
mglmx	66:45a90a945983	1502	flip_switch(5,15);
mglmx	66:45a90a945983	1503	flip_switch(3,40);
mglmx	66:45a90a945983	1504	wait(0.5);
mglmx	66:45a90a945983	1505	flip_switch(5,15);
mglmx	66:45a90a945983	1506	flip_switch(4,15);
mglmx	66:45a90a945983	1507	wait(0.5);
mglmx	66:45a90a945983	1508	flip_switch(5,15);
mglmx	66:45a90a945983	1509
carlosperales95	71:9a303d90c9b8	1510	**/
carlosperales95	69:609c69e91b4e	1511
carlosperales95	71:9a303d90c9b8	1512	/**Code for train to stop at station
carlosperales95	71:9a303d90c9b8	1513
carlosperales95	71:9a303d90c9b8	1514	if(station == 1){ //If train is on the sensor at the middle of the station it stops and displays LCD text.
carlosperales95	14:7bb998edd819	1515
mglmx	4:50879dfb82d5	1516	lcd.cls();
carlosperales95	29:559eb2164488	1517	lcd.printf("All aboard\n mind the gap");
mglmx	32:e5b732fb8e65	1518	DCC_send_command(DCCaddressDR,DCCinst_stop,400);
carlosperales95	29:559eb2164488	1519	lcd.cls();
mglmx	4:50879dfb82d5	1520
mglmx	22:e4153ca757dd	1521	}else{
mglmx	57:ee5da8a011e0	1522	DR_train.run();
mglmx	57:ee5da8a011e0	1523	LR_train.run();
mglmx	55:aa3baa01f43d	1524
carlosperales95	25:a42a1ed4d8e9	1525	}
carlosperales95	71:9a303d90c9b8	1526
carlosperales95	71:9a303d90c9b8	1527	**/
carlosperales95	71:9a303d90c9b8	1528
carlosperales95	71:9a303d90c9b8	1529
carlosperales95	71:9a303d90c9b8	1530	/**Train light routine to start running
carlosperales95	71:9a303d90c9b8	1531
carlosperales95	71:9a303d90c9b8	1532
carlosperales95	71:9a303d90c9b8	1533	DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // turn light on full
carlosperales95	71:9a303d90c9b8	1534	DCC_send_command(DCCaddressDR,DCC_func_dimlight,400); // dim light
carlosperales95	71:9a303d90c9b8	1535	DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // light full again
carlosperales95	71:9a303d90c9b8	1536
carlosperales95	71:9a303d90c9b8	1537
carlosperales95	71:9a303d90c9b8	1538	**/
carlosperales95	71:9a303d90c9b8	1539
carlosperales95	71:9a303d90c9b8	1540	/**Print if train is found and updated in update_Train_pos();
carlosperales95	71:9a303d90c9b8	1541	if(found_DR){
carlosperales95	71:9a303d90c9b8	1542
carlosperales95	71:9a303d90c9b8	1543	//doBuzz();
carlosperales95	71:9a303d90c9b8	1544	lcd.cls();
carlosperales95	71:9a303d90c9b8	1545	lcd.printf("DR is at D%d",DR_train.get_position_number());
carlosperales95	71:9a303d90c9b8	1546	}
carlosperales95	71:9a303d90c9b8	1547
carlosperales95	71:9a303d90c9b8	1548	if(found_LR){
carlosperales95	71:9a303d90c9b8	1549
carlosperales95	71:9a303d90c9b8	1550	lcd.cls();
carlosperales95	71:9a303d90c9b8	1551	lcd.printf("LR is at D%d",LR_train.get_position_number());
carlosperales95	71:9a303d90c9b8	1552	}
carlosperales95	71:9a303d90c9b8	1553
carlosperales95	71:9a303d90c9b8	1554	if(!found_DR && !found_LR){
carlosperales95	71:9a303d90c9b8	1555
carlosperales95	71:9a303d90c9b8	1556	lcd.cls();
carlosperales95	71:9a303d90c9b8	1557	lcd.printf("No train before :(");
carlosperales95	71:9a303d90c9b8	1558	}
carlosperales95	71:9a303d90c9b8	1559	**/
carlosperales95	71:9a303d90c9b8	1560
carlosperales95	71:9a303d90c9b8	1561
carlosperales95	71:9a303d90c9b8	1562	/**Print sensor interrupts in int0 and int1 functions
carlosperales95	71:9a303d90c9b8	1563
carlosperales95	71:9a303d90c9b8	1564	//lcd.cls();
carlosperales95	71:9a303d90c9b8	1565	//lcd.printf("int0 0x%x \n Sensor: %d",sensor_data,sensor);
carlosperales95	71:9a303d90c9b8	1566
carlosperales95	71:9a303d90c9b8	1567	**/
carlosperales95	71:9a303d90c9b8	1568
carlosperales95	71:9a303d90c9b8	1569	/**Print the potentiometer value
carlosperales95	71:9a303d90c9b8	1570
carlosperales95	71:9a303d90c9b8	1571	//lcd.cls();
carlosperales95	71:9a303d90c9b8	1572	// lcd.printf("vin: %.4f",vin);
carlosperales95	71:9a303d90c9b8	1573
carlosperales95	71:9a303d90c9b8	1574	**/
carlosperales95	71:9a303d90c9b8	1575
carlosperales95	71:9a303d90c9b8	1576	/** Chunk 1 of comments from main
carlosperales95	71:9a303d90c9b8	1577
carlosperales95	71:9a303d90c9b8	1578	//RISE FOR INTERRUPTS?? NOT WORKING ATM
carlosperales95	71:9a303d90c9b8	1579	//int0.rise(&interrupt0);
carlosperales95	71:9a303d90c9b8	1580	//int1.rise(&interrupt1);
carlosperales95	71:9a303d90c9b8	1581
carlosperales95	71:9a303d90c9b8	1582	//Read and display potentiometer
carlosperales95	71:9a303d90c9b8	1583	//float f = pot.read();
carlosperales95	71:9a303d90c9b8	1584	//float vin = f * 3.3;
carlosperales95	71:9a303d90c9b8	1585	//lcd.printf("vin: %.4f",vin);
carlosperales95	71:9a303d90c9b8	1586
carlosperales95	71:9a303d90c9b8	1587	//0xFFFC //1111111111111100
carlosperales95	71:9a303d90c9b8	1588
carlosperales95	71:9a303d90c9b8	1589	**/

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	06 Jun 2018
Imports:	3
Forks:	0
Commits:	76
Dependents:	0
Dependencies:	2
Followers:	4

main.cpp@75:bae748a7905f, 2018-06-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning