Project Systems Testing team 4
/
train_control
version for testing not implemented: delta loop safety, power surge, calibration mode
main.cpp@1:1648ce825c31, 2019-06-27 (annotated)
- Committer:
- KoenKahlman
- Date:
- Thu Jun 27 07:50:27 2019 +0000
- Revision:
- 1:1648ce825c31
- Parent:
- 0:9c82986d7cb9
- Child:
- 2:bb96eba66e78
version for testing; ; unimplemented: deltaloop safety, accounting for power surges, switch safety timers
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
KoenKahlman | 0:9c82986d7cb9 | 1 | #include "mbed.h" |
KoenKahlman | 0:9c82986d7cb9 | 2 | #include <string> |
KoenKahlman | 0:9c82986d7cb9 | 3 | #include <stdarg.h> |
KoenKahlman | 0:9c82986d7cb9 | 4 | #include "TextLCD.h" |
KoenKahlman | 0:9c82986d7cb9 | 5 | #include "MCP23017.h" |
KoenKahlman | 0:9c82986d7cb9 | 6 | #include "math.h" |
KoenKahlman | 0:9c82986d7cb9 | 7 | #include <list> |
KoenKahlman | 0:9c82986d7cb9 | 8 | |
KoenKahlman | 1:1648ce825c31 | 9 | const bool test = false; |
KoenKahlman | 0:9c82986d7cb9 | 10 | int iter; |
KoenKahlman | 0:9c82986d7cb9 | 11 | std::list<int> passedMarkers; |
KoenKahlman | 0:9c82986d7cb9 | 12 | std::list<int>::iterator it; |
KoenKahlman | 1:1648ce825c31 | 13 | int speedsetting; |
KoenKahlman | 0:9c82986d7cb9 | 14 | |
KoenKahlman | 1:1648ce825c31 | 15 | //BOX CONNECTIONS |
KoenKahlman | 0:9c82986d7cb9 | 16 | DigitalIn button1 (p8); |
KoenKahlman | 0:9c82986d7cb9 | 17 | DigitalIn button2 (p9); |
KoenKahlman | 0:9c82986d7cb9 | 18 | DigitalIn button3 (p10); |
KoenKahlman | 0:9c82986d7cb9 | 19 | DigitalIn button4 (p30); |
KoenKahlman | 0:9c82986d7cb9 | 20 | AnalogIn potentio (p15); // configures pin15 for analog input. Creates object Ain. It is the potentiometer. |
KoenKahlman | 0:9c82986d7cb9 | 21 | TextLCD lcd(p22, p21, p23, p24, p25, p26); // configures the LCD pins. Creates object lcd |
KoenKahlman | 0:9c82986d7cb9 | 22 | DigitalOut led1(LED1); |
KoenKahlman | 0:9c82986d7cb9 | 23 | DigitalOut led2(LED2); |
KoenKahlman | 0:9c82986d7cb9 | 24 | DigitalOut led3(LED3); |
KoenKahlman | 1:1648ce825c31 | 25 | DigitalOut led4(LED4); |
KoenKahlman | 1:1648ce825c31 | 26 | DigitalOut boxled1 (p19); |
KoenKahlman | 1:1648ce825c31 | 27 | DigitalOut boxled2 (p20); |
KoenKahlman | 1:1648ce825c31 | 28 | DigitalOut buzzer (p29); |
KoenKahlman | 0:9c82986d7cb9 | 29 | Serial pc(USBTX, USBRX); // Initialise the serial connection for terminal output |
KoenKahlman | 0:9c82986d7cb9 | 30 | InterruptIn int0(p13); |
KoenKahlman | 0:9c82986d7cb9 | 31 | InterruptIn int1(p14); |
KoenKahlman | 0:9c82986d7cb9 | 32 | I2C i2c(p28, p27); |
KoenKahlman | 0:9c82986d7cb9 | 33 | MCP23017 *mcp; //hall detectors |
KoenKahlman | 0:9c82986d7cb9 | 34 | DigitalOut data(p16); // configures pin16 for digital output. Creates object train. This signal should go to the booster |
KoenKahlman | 0:9c82986d7cb9 | 35 | DigitalOut enable(p17); //configures pin 17 for digital output. creates object enable, this enables the train track or disables it in case of emergency |
KoenKahlman | 0:9c82986d7cb9 | 36 | |
KoenKahlman | 1:1648ce825c31 | 37 | //CONSTANTS |
KoenKahlman | 1:1648ce825c31 | 38 | const unsigned int DCCaddress_train1 = 0x01; |
KoenKahlman | 1:1648ce825c31 | 39 | const unsigned int DCCaddress_train2 = 0x02; |
KoenKahlman | 1:1648ce825c31 | 40 | const unsigned int DCCaddress_decoder = 0x06; |
KoenKahlman | 1:1648ce825c31 | 41 | const unsigned int DCCinst_stop = 0x70; //stop 01 1 1 0000 |
KoenKahlman | 1:1648ce825c31 | 42 | const unsigned int DCCinst_estop = 0x71; //01 1 1 0001 |
KoenKahlman | 1:1648ce825c31 | 43 | const unsigned int DCCswitchneutral = 0b10000000; |
KoenKahlman | 1:1648ce825c31 | 44 | const unsigned int DCCswitch1 = 0b10000001; |
KoenKahlman | 1:1648ce825c31 | 45 | const unsigned int DCCswitch2 = 0b10000010; |
KoenKahlman | 1:1648ce825c31 | 46 | const unsigned int DCCswitch3 = 0b10000100; |
KoenKahlman | 1:1648ce825c31 | 47 | const unsigned int DCCswitch4 = 0b10001000; |
KoenKahlman | 1:1648ce825c31 | 48 | const double stationWaitTime = 5.; |
KoenKahlman | 1:1648ce825c31 | 49 | const double switchsafety = 4.; //contant time limit between uses of same switch |
KoenKahlman | 1:1648ce825c31 | 50 | const int switchwait = 1500; //time inbetween different switch commands |
KoenKahlman | 1:1648ce825c31 | 51 | |
KoenKahlman | 0:9c82986d7cb9 | 52 | //interrupt flags |
KoenKahlman | 0:9c82986d7cb9 | 53 | bool int0flag; |
KoenKahlman | 0:9c82986d7cb9 | 54 | bool int1flag; |
KoenKahlman | 0:9c82986d7cb9 | 55 | |
KoenKahlman | 0:9c82986d7cb9 | 56 | //global variables for train1 |
KoenKahlman | 0:9c82986d7cb9 | 57 | bool dir1; |
KoenKahlman | 0:9c82986d7cb9 | 58 | bool light1; |
KoenKahlman | 0:9c82986d7cb9 | 59 | int speed1; |
KoenKahlman | 0:9c82986d7cb9 | 60 | int prev1; |
KoenKahlman | 0:9c82986d7cb9 | 61 | int curr1; |
KoenKahlman | 0:9c82986d7cb9 | 62 | int next1; |
KoenKahlman | 1:1648ce825c31 | 63 | Timer station1; |
KoenKahlman | 1:1648ce825c31 | 64 | bool boarding1; |
KoenKahlman | 1:1648ce825c31 | 65 | bool command1; |
KoenKahlman | 1:1648ce825c31 | 66 | |
KoenKahlman | 1:1648ce825c31 | 67 | bool collision; |
KoenKahlman | 0:9c82986d7cb9 | 68 | |
KoenKahlman | 0:9c82986d7cb9 | 69 | //global variables for train2 |
KoenKahlman | 0:9c82986d7cb9 | 70 | bool dir2; |
KoenKahlman | 0:9c82986d7cb9 | 71 | bool light2; |
KoenKahlman | 0:9c82986d7cb9 | 72 | int speed2; |
KoenKahlman | 0:9c82986d7cb9 | 73 | int prev2; |
KoenKahlman | 0:9c82986d7cb9 | 74 | int curr2; |
KoenKahlman | 0:9c82986d7cb9 | 75 | int next2; |
KoenKahlman | 1:1648ce825c31 | 76 | Timer station2; |
KoenKahlman | 1:1648ce825c31 | 77 | bool boarding2; |
KoenKahlman | 1:1648ce825c31 | 78 | bool command2; |
KoenKahlman | 0:9c82986d7cb9 | 79 | |
KoenKahlman | 0:9c82986d7cb9 | 80 | //global variables for switches |
KoenKahlman | 0:9c82986d7cb9 | 81 | std::time_t switch1action; |
KoenKahlman | 0:9c82986d7cb9 | 82 | std::time_t switch2action; |
KoenKahlman | 0:9c82986d7cb9 | 83 | std::time_t switch3action; |
KoenKahlman | 0:9c82986d7cb9 | 84 | std::time_t switch4action; |
KoenKahlman | 0:9c82986d7cb9 | 85 | bool switch1; |
KoenKahlman | 0:9c82986d7cb9 | 86 | bool switch2; |
KoenKahlman | 0:9c82986d7cb9 | 87 | bool switch3; |
KoenKahlman | 0:9c82986d7cb9 | 88 | bool switch4; |
KoenKahlman | 0:9c82986d7cb9 | 89 | |
KoenKahlman | 0:9c82986d7cb9 | 90 | //FUNCTION PROTOTYPES |
KoenKahlman | 0:9c82986d7cb9 | 91 | void init_mcp(void); |
KoenKahlman | 0:9c82986d7cb9 | 92 | void on_int0_change(void); |
KoenKahlman | 0:9c82986d7cb9 | 93 | void on_int1_change(void); |
KoenKahlman | 0:9c82986d7cb9 | 94 | std::list<int> circuit0markers(int sensor_data); |
KoenKahlman | 0:9c82986d7cb9 | 95 | std::list<int> circuit1markers(int sensor_data); |
KoenKahlman | 0:9c82986d7cb9 | 96 | |
KoenKahlman | 0:9c82986d7cb9 | 97 | void displayTerminalStatus(void); |
KoenKahlman | 0:9c82986d7cb9 | 98 | void displayLCDstatus(void); |
KoenKahlman | 0:9c82986d7cb9 | 99 | |
KoenKahlman | 0:9c82986d7cb9 | 100 | void init(void); |
KoenKahlman | 0:9c82986d7cb9 | 101 | void initSwitches(void); |
KoenKahlman | 0:9c82986d7cb9 | 102 | void startupWait(void); |
KoenKahlman | 0:9c82986d7cb9 | 103 | void emergencyStop(void); |
KoenKahlman | 0:9c82986d7cb9 | 104 | void handleInt0(void); |
KoenKahlman | 0:9c82986d7cb9 | 105 | void handleInt1(void); |
KoenKahlman | 0:9c82986d7cb9 | 106 | void trainCommand(void); |
KoenKahlman | 1:1648ce825c31 | 107 | void updateTrainStatus(int marker); |
KoenKahlman | 1:1648ce825c31 | 108 | int predictNextMarker(int prev, int curr); |
KoenKahlman | 0:9c82986d7cb9 | 109 | void iterAction(void); |
KoenKahlman | 0:9c82986d7cb9 | 110 | |
KoenKahlman | 0:9c82986d7cb9 | 111 | void changeSwitch1(bool); |
KoenKahlman | 0:9c82986d7cb9 | 112 | void changeSwitch2(bool); |
KoenKahlman | 0:9c82986d7cb9 | 113 | void changeSwitch3(bool); |
KoenKahlman | 0:9c82986d7cb9 | 114 | void changeSwitch4(bool); |
KoenKahlman | 1:1648ce825c31 | 115 | void updateTrainPredictions(void); |
KoenKahlman | 0:9c82986d7cb9 | 116 | |
KoenKahlman | 0:9c82986d7cb9 | 117 | unsigned int trainInstruction(bool direction, bool light, int speed); |
KoenKahlman | 1:1648ce825c31 | 118 | void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count); |
KoenKahlman | 0:9c82986d7cb9 | 119 | |
KoenKahlman | 1:1648ce825c31 | 120 | void driveTrain(int train); |
KoenKahlman | 1:1648ce825c31 | 121 | void reverseTrain(int train); |
KoenKahlman | 1:1648ce825c31 | 122 | void stopTrain(int train); |
KoenKahlman | 0:9c82986d7cb9 | 123 | |
KoenKahlman | 1:1648ce825c31 | 124 | void atStation2(int train); |
KoenKahlman | 1:1648ce825c31 | 125 | void atStation12(int train); |
KoenKahlman | 1:1648ce825c31 | 126 | void atSwitch3(int train); |
KoenKahlman | 1:1648ce825c31 | 127 | void atSwitch4(int train); |
KoenKahlman | 1:1648ce825c31 | 128 | void headCollision(void); |
KoenKahlman | 1:1648ce825c31 | 129 | void sideCollision(void); |
KoenKahlman | 1:1648ce825c31 | 130 | void xCollision(void); |
KoenKahlman | 1:1648ce825c31 | 131 | bool isXCollision(void); |
KoenKahlman | 1:1648ce825c31 | 132 | void tailCollision(int backTrain); |
KoenKahlman | 1:1648ce825c31 | 133 | void atStation(int train); |
KoenKahlman | 1:1648ce825c31 | 134 | //void loopDeltaCrossing(int train); |
KoenKahlman | 0:9c82986d7cb9 | 135 | |
KoenKahlman | 0:9c82986d7cb9 | 136 | int main() { |
KoenKahlman | 1:1648ce825c31 | 137 | buzzer = 0; |
KoenKahlman | 0:9c82986d7cb9 | 138 | wait_ms(1000); //safety wait when starting (power surge) |
KoenKahlman | 1:1648ce825c31 | 139 | enable = false; //track off while doing startup |
KoenKahlman | 0:9c82986d7cb9 | 140 | while(1){ |
KoenKahlman | 0:9c82986d7cb9 | 141 | init(); // initialize all the things to starting values |
KoenKahlman | 0:9c82986d7cb9 | 142 | startupWait(); //wait for user to press button 3 to begin |
KoenKahlman | 0:9c82986d7cb9 | 143 | enable = true; |
KoenKahlman | 1:1648ce825c31 | 144 | initSwitches(); //put switches in starting position |
KoenKahlman | 1:1648ce825c31 | 145 | wait_ms(2000); //power surge safety |
KoenKahlman | 0:9c82986d7cb9 | 146 | |
KoenKahlman | 0:9c82986d7cb9 | 147 | if(!test){ |
KoenKahlman | 0:9c82986d7cb9 | 148 | iter = 0; //iter count |
KoenKahlman | 0:9c82986d7cb9 | 149 | while(enable){ |
KoenKahlman | 0:9c82986d7cb9 | 150 | if(!button4){ |
KoenKahlman | 0:9c82986d7cb9 | 151 | emergencyStop(); |
KoenKahlman | 0:9c82986d7cb9 | 152 | break; |
KoenKahlman | 1:1648ce825c31 | 153 | } |
KoenKahlman | 1:1648ce825c31 | 154 | |
KoenKahlman | 0:9c82986d7cb9 | 155 | if(int0flag){ |
KoenKahlman | 0:9c82986d7cb9 | 156 | handleInt0(); //store passed circuit 0 markers in the list passed markers |
KoenKahlman | 0:9c82986d7cb9 | 157 | } |
KoenKahlman | 0:9c82986d7cb9 | 158 | |
KoenKahlman | 0:9c82986d7cb9 | 159 | if(int1flag){ |
KoenKahlman | 0:9c82986d7cb9 | 160 | handleInt1(); //same for circuit 1 |
KoenKahlman | 0:9c82986d7cb9 | 161 | } |
KoenKahlman | 0:9c82986d7cb9 | 162 | |
KoenKahlman | 1:1648ce825c31 | 163 | if(!passedMarkers.empty()){ |
KoenKahlman | 0:9c82986d7cb9 | 164 | for (it=passedMarkers.begin(); it!=passedMarkers.end(); ++it){ |
KoenKahlman | 1:1648ce825c31 | 165 | if(*it != next1 && *it != next2 && *it != curr1 && *it != curr2){ //TODO check if safe |
KoenKahlman | 1:1648ce825c31 | 166 | pc.printf("ERROR: Unexpected marker %d. Expected train1: %d or train2: %d. Sensor failure or unknown object on track. Aborting.\n\r", *it, next1, next2); |
KoenKahlman | 0:9c82986d7cb9 | 167 | enable = false; |
KoenKahlman | 1:1648ce825c31 | 168 | break; |
KoenKahlman | 1:1648ce825c31 | 169 | } |
KoenKahlman | 0:9c82986d7cb9 | 170 | updateTrainStatus(*it); |
KoenKahlman | 0:9c82986d7cb9 | 171 | } |
KoenKahlman | 0:9c82986d7cb9 | 172 | passedMarkers.clear(); |
KoenKahlman | 0:9c82986d7cb9 | 173 | } |
KoenKahlman | 0:9c82986d7cb9 | 174 | |
KoenKahlman | 0:9c82986d7cb9 | 175 | trainCommand(); //decides for a command to send to the track |
KoenKahlman | 0:9c82986d7cb9 | 176 | |
KoenKahlman | 1:1648ce825c31 | 177 | iterAction(); //every X iterations display some stuff |
KoenKahlman | 0:9c82986d7cb9 | 178 | iter++; |
KoenKahlman | 0:9c82986d7cb9 | 179 | } |
KoenKahlman | 0:9c82986d7cb9 | 180 | } else { |
KoenKahlman | 0:9c82986d7cb9 | 181 | //test scripts |
KoenKahlman | 0:9c82986d7cb9 | 182 | } |
KoenKahlman | 0:9c82986d7cb9 | 183 | } |
KoenKahlman | 0:9c82986d7cb9 | 184 | } |
KoenKahlman | 0:9c82986d7cb9 | 185 | |
KoenKahlman | 0:9c82986d7cb9 | 186 | //MAIN LOOP ROUTINES |
KoenKahlman | 1:1648ce825c31 | 187 | void init(){ |
KoenKahlman | 1:1648ce825c31 | 188 | command1 = false; |
KoenKahlman | 1:1648ce825c31 | 189 | command2 = false; |
KoenKahlman | 1:1648ce825c31 | 190 | collision = false; |
KoenKahlman | 1:1648ce825c31 | 191 | |
KoenKahlman | 0:9c82986d7cb9 | 192 | //initialize clock time |
KoenKahlman | 0:9c82986d7cb9 | 193 | set_time(1); |
KoenKahlman | 0:9c82986d7cb9 | 194 | switch1action = time(NULL); |
KoenKahlman | 0:9c82986d7cb9 | 195 | switch2action = time(NULL); |
KoenKahlman | 0:9c82986d7cb9 | 196 | switch3action = time(NULL); |
KoenKahlman | 0:9c82986d7cb9 | 197 | switch4action = time(NULL); |
KoenKahlman | 0:9c82986d7cb9 | 198 | |
KoenKahlman | 0:9c82986d7cb9 | 199 | led1 = 0; |
KoenKahlman | 0:9c82986d7cb9 | 200 | led2 = 0; |
KoenKahlman | 0:9c82986d7cb9 | 201 | led3 = 0; |
KoenKahlman | 1:1648ce825c31 | 202 | buzzer = 0; |
KoenKahlman | 1:1648ce825c31 | 203 | boxled1 = 0; |
KoenKahlman | 1:1648ce825c31 | 204 | boxled2 = 0; |
KoenKahlman | 1:1648ce825c31 | 205 | |
KoenKahlman | 1:1648ce825c31 | 206 | speedsetting = 7; |
KoenKahlman | 0:9c82986d7cb9 | 207 | |
KoenKahlman | 0:9c82986d7cb9 | 208 | //pc.baud(921600); //faster printing |
KoenKahlman | 0:9c82986d7cb9 | 209 | pc.baud(19200); |
KoenKahlman | 0:9c82986d7cb9 | 210 | |
KoenKahlman | 0:9c82986d7cb9 | 211 | //initialize mcp //(initialize interrupt flags) |
KoenKahlman | 0:9c82986d7cb9 | 212 | init_mcp(); |
KoenKahlman | 0:9c82986d7cb9 | 213 | |
KoenKahlman | 0:9c82986d7cb9 | 214 | //initialize train globals |
KoenKahlman | 0:9c82986d7cb9 | 215 | dir1 = true; |
KoenKahlman | 0:9c82986d7cb9 | 216 | light1 = true; |
KoenKahlman | 1:1648ce825c31 | 217 | speed1 = speedsetting; |
KoenKahlman | 1:1648ce825c31 | 218 | boarding1 = false; |
KoenKahlman | 1:1648ce825c31 | 219 | //train 1 starting position |
KoenKahlman | 1:1648ce825c31 | 220 | prev1 = 0; |
KoenKahlman | 1:1648ce825c31 | 221 | curr1 = 1; |
KoenKahlman | 1:1648ce825c31 | 222 | next1 = predictNextMarker(prev1, curr1); //2 |
KoenKahlman | 0:9c82986d7cb9 | 223 | |
KoenKahlman | 0:9c82986d7cb9 | 224 | //global variables for train2 |
KoenKahlman | 1:1648ce825c31 | 225 | dir2 = true; |
KoenKahlman | 0:9c82986d7cb9 | 226 | light2 = true; |
KoenKahlman | 1:1648ce825c31 | 227 | speed2 = speedsetting; |
KoenKahlman | 1:1648ce825c31 | 228 | boarding2 = false; |
KoenKahlman | 1:1648ce825c31 | 229 | //train 2 starting position |
KoenKahlman | 1:1648ce825c31 | 230 | prev2 = 4; |
KoenKahlman | 1:1648ce825c31 | 231 | curr2 = 6; |
KoenKahlman | 1:1648ce825c31 | 232 | next2 = predictNextMarker(prev2, curr2); //7 |
KoenKahlman | 0:9c82986d7cb9 | 233 | |
KoenKahlman | 1:1648ce825c31 | 234 | //initialize presumed switch positions |
KoenKahlman | 0:9c82986d7cb9 | 235 | switch1 = false; |
KoenKahlman | 0:9c82986d7cb9 | 236 | switch2 = false; |
KoenKahlman | 0:9c82986d7cb9 | 237 | switch3 = false; |
KoenKahlman | 0:9c82986d7cb9 | 238 | switch4 = false; |
KoenKahlman | 1:1648ce825c31 | 239 | } |
KoenKahlman | 1:1648ce825c31 | 240 | |
KoenKahlman | 1:1648ce825c31 | 241 | void startupWait(){ |
KoenKahlman | 1:1648ce825c31 | 242 | lcd.cls(); |
KoenKahlman | 1:1648ce825c31 | 243 | lcd.printf("Press START to start"); |
KoenKahlman | 1:1648ce825c31 | 244 | pc.printf("Press START to start\n\r"); |
KoenKahlman | 1:1648ce825c31 | 245 | while(button3){ |
KoenKahlman | 1:1648ce825c31 | 246 | //do nothing |
KoenKahlman | 1:1648ce825c31 | 247 | } |
KoenKahlman | 1:1648ce825c31 | 248 | pc.printf("Starting...\n\r"); |
KoenKahlman | 0:9c82986d7cb9 | 249 | } |
KoenKahlman | 0:9c82986d7cb9 | 250 | |
KoenKahlman | 0:9c82986d7cb9 | 251 | void initSwitches(){ |
KoenKahlman | 0:9c82986d7cb9 | 252 | wait_ms(switchwait); |
KoenKahlman | 1:1648ce825c31 | 253 | changeSwitch1(switch1); |
KoenKahlman | 0:9c82986d7cb9 | 254 | wait_ms(switchwait); |
KoenKahlman | 1:1648ce825c31 | 255 | changeSwitch2(switch2); |
KoenKahlman | 0:9c82986d7cb9 | 256 | wait_ms(switchwait); |
KoenKahlman | 1:1648ce825c31 | 257 | changeSwitch3(switch3); |
KoenKahlman | 0:9c82986d7cb9 | 258 | wait_ms(switchwait); |
KoenKahlman | 1:1648ce825c31 | 259 | changeSwitch4(switch4); |
KoenKahlman | 1:1648ce825c31 | 260 | wait_ms(500); |
KoenKahlman | 0:9c82986d7cb9 | 261 | } |
KoenKahlman | 0:9c82986d7cb9 | 262 | |
KoenKahlman | 1:1648ce825c31 | 263 | void emergencyStop(){ |
KoenKahlman | 1:1648ce825c31 | 264 | enable = false; |
KoenKahlman | 1:1648ce825c31 | 265 | pc.printf("Emergency stop! Press button 3 when ready to restart\n\r"); |
KoenKahlman | 1:1648ce825c31 | 266 | |
KoenKahlman | 1:1648ce825c31 | 267 | lcd.cls(); |
KoenKahlman | 1:1648ce825c31 | 268 | lcd.printf("Press START when ready to restart"); |
KoenKahlman | 1:1648ce825c31 | 269 | while(button3){ |
KoenKahlman | 1:1648ce825c31 | 270 | //do nothing |
KoenKahlman | 0:9c82986d7cb9 | 271 | } |
KoenKahlman | 0:9c82986d7cb9 | 272 | } |
KoenKahlman | 0:9c82986d7cb9 | 273 | |
KoenKahlman | 0:9c82986d7cb9 | 274 | void handleInt0(){ |
KoenKahlman | 0:9c82986d7cb9 | 275 | wait_us(2000); |
KoenKahlman | 0:9c82986d7cb9 | 276 | int sensor_data = mcp->_read(INTCAPA); |
KoenKahlman | 0:9c82986d7cb9 | 277 | int0flag = false; |
KoenKahlman | 0:9c82986d7cb9 | 278 | std::list<int> temp = circuit0markers(sensor_data); |
KoenKahlman | 0:9c82986d7cb9 | 279 | passedMarkers.splice(passedMarkers.end(), temp); |
KoenKahlman | 0:9c82986d7cb9 | 280 | } |
KoenKahlman | 0:9c82986d7cb9 | 281 | |
KoenKahlman | 0:9c82986d7cb9 | 282 | void handleInt1(){ |
KoenKahlman | 0:9c82986d7cb9 | 283 | wait_us(2000); |
KoenKahlman | 0:9c82986d7cb9 | 284 | int sensor_data = mcp->_read(INTCAPB); |
KoenKahlman | 0:9c82986d7cb9 | 285 | int1flag = false; |
KoenKahlman | 0:9c82986d7cb9 | 286 | std::list<int> temp = circuit1markers(sensor_data); |
KoenKahlman | 0:9c82986d7cb9 | 287 | passedMarkers.splice(passedMarkers.end(), temp); |
KoenKahlman | 0:9c82986d7cb9 | 288 | } |
KoenKahlman | 0:9c82986d7cb9 | 289 | |
KoenKahlman | 1:1648ce825c31 | 290 | void updateTrainStatus(int marker){ |
KoenKahlman | 1:1648ce825c31 | 291 | if(marker == 21){ |
KoenKahlman | 1:1648ce825c31 | 292 | return; //marker 21 is unused |
KoenKahlman | 1:1648ce825c31 | 293 | } |
KoenKahlman | 1:1648ce825c31 | 294 | |
KoenKahlman | 1:1648ce825c31 | 295 | if(marker == next1){ |
KoenKahlman | 1:1648ce825c31 | 296 | prev1 = curr1; |
KoenKahlman | 1:1648ce825c31 | 297 | curr1 = next1; |
KoenKahlman | 1:1648ce825c31 | 298 | next1 = predictNextMarker(prev1, curr1); |
KoenKahlman | 1:1648ce825c31 | 299 | command1 = false; |
KoenKahlman | 1:1648ce825c31 | 300 | collision = false; |
KoenKahlman | 1:1648ce825c31 | 301 | pc.printf("STATUS: Train 1 reached marker %d from marker %d. Predicted marker %d\n\r", curr1, prev1, next1); |
KoenKahlman | 1:1648ce825c31 | 302 | } else { //marker == next2 |
KoenKahlman | 1:1648ce825c31 | 303 | prev2 = curr2; |
KoenKahlman | 1:1648ce825c31 | 304 | curr2 = next2; |
KoenKahlman | 1:1648ce825c31 | 305 | next2 = predictNextMarker(prev2, curr2); |
KoenKahlman | 1:1648ce825c31 | 306 | command2 = false; |
KoenKahlman | 1:1648ce825c31 | 307 | collision = false; |
KoenKahlman | 1:1648ce825c31 | 308 | pc.printf("STATUS: Train 2 reached marker %d from marker %d. Predicted marker %d\n\r", curr2, prev2, next2); |
KoenKahlman | 1:1648ce825c31 | 309 | } |
KoenKahlman | 1:1648ce825c31 | 310 | } |
KoenKahlman | 1:1648ce825c31 | 311 | |
KoenKahlman | 1:1648ce825c31 | 312 | int predictNextMarker(int prev, int curr){ |
KoenKahlman | 1:1648ce825c31 | 313 | switch(prev){ |
KoenKahlman | 1:1648ce825c31 | 314 | case 0: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 315 | case 1: return 2; |
KoenKahlman | 1:1648ce825c31 | 316 | case 13: return 12; |
KoenKahlman | 1:1648ce825c31 | 317 | } |
KoenKahlman | 1:1648ce825c31 | 318 | case 1: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 319 | case 2: return switch2?3:4; |
KoenKahlman | 1:1648ce825c31 | 320 | case 0: return 13; |
KoenKahlman | 1:1648ce825c31 | 321 | } |
KoenKahlman | 1:1648ce825c31 | 322 | case 2: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 323 | case 3: return 9; |
KoenKahlman | 1:1648ce825c31 | 324 | case 4: return 6; |
KoenKahlman | 1:1648ce825c31 | 325 | case 1: return 0; |
KoenKahlman | 1:1648ce825c31 | 326 | } |
KoenKahlman | 1:1648ce825c31 | 327 | case 3: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 328 | case 9: return 8; |
KoenKahlman | 1:1648ce825c31 | 329 | case 2: return 1; |
KoenKahlman | 1:1648ce825c31 | 330 | } |
KoenKahlman | 1:1648ce825c31 | 331 | case 4: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 332 | case 6: return 7; |
KoenKahlman | 1:1648ce825c31 | 333 | case 2: return 1; |
KoenKahlman | 1:1648ce825c31 | 334 | } |
KoenKahlman | 1:1648ce825c31 | 335 | case 5: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 336 | case 6: return 7; |
KoenKahlman | 1:1648ce825c31 | 337 | case 11: return 12; |
KoenKahlman | 1:1648ce825c31 | 338 | } |
KoenKahlman | 1:1648ce825c31 | 339 | case 6: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 340 | case 4: return 2; |
KoenKahlman | 1:1648ce825c31 | 341 | case 5: return 11; |
KoenKahlman | 1:1648ce825c31 | 342 | case 7: return 8; |
KoenKahlman | 1:1648ce825c31 | 343 | } |
KoenKahlman | 1:1648ce825c31 | 344 | case 7: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 345 | case 6: return switch3?5:4; |
KoenKahlman | 1:1648ce825c31 | 346 | case 8: return switch4?9:10; |
KoenKahlman | 1:1648ce825c31 | 347 | } |
KoenKahlman | 1:1648ce825c31 | 348 | case 8: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 349 | case 7: return 6; |
KoenKahlman | 1:1648ce825c31 | 350 | case 9: return 3; |
KoenKahlman | 1:1648ce825c31 | 351 | case 10: return 12; |
KoenKahlman | 1:1648ce825c31 | 352 | } |
KoenKahlman | 1:1648ce825c31 | 353 | case 9: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 354 | case 3: return 2; |
KoenKahlman | 1:1648ce825c31 | 355 | case 8: return 7; |
KoenKahlman | 1:1648ce825c31 | 356 | } |
KoenKahlman | 1:1648ce825c31 | 357 | case 10: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 358 | case 8: return 7; |
KoenKahlman | 1:1648ce825c31 | 359 | case 12: return 13; |
KoenKahlman | 1:1648ce825c31 | 360 | } |
KoenKahlman | 1:1648ce825c31 | 361 | case 11: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 362 | case 5: return 6; |
KoenKahlman | 1:1648ce825c31 | 363 | case 12: return 13; |
KoenKahlman | 1:1648ce825c31 | 364 | } |
KoenKahlman | 1:1648ce825c31 | 365 | case 12: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 366 | case 10: return 8; |
KoenKahlman | 1:1648ce825c31 | 367 | case 11: return 5; |
KoenKahlman | 1:1648ce825c31 | 368 | case 13: return 0; |
KoenKahlman | 1:1648ce825c31 | 369 | } |
KoenKahlman | 1:1648ce825c31 | 370 | case 13: switch(curr){ |
KoenKahlman | 1:1648ce825c31 | 371 | case 0: return 1; |
KoenKahlman | 1:1648ce825c31 | 372 | case 12: return switch1?11:10; |
KoenKahlman | 1:1648ce825c31 | 373 | } |
KoenKahlman | 1:1648ce825c31 | 374 | } |
KoenKahlman | 1:1648ce825c31 | 375 | return 666; //TODO default |
KoenKahlman | 1:1648ce825c31 | 376 | } |
KoenKahlman | 1:1648ce825c31 | 377 | |
KoenKahlman | 1:1648ce825c31 | 378 | void iterAction(){ |
KoenKahlman | 1:1648ce825c31 | 379 | displayLCDstatus(); |
KoenKahlman | 1:1648ce825c31 | 380 | if(iter % 100 == 0){ |
KoenKahlman | 1:1648ce825c31 | 381 | displayTerminalStatus(); |
KoenKahlman | 1:1648ce825c31 | 382 | } |
KoenKahlman | 1:1648ce825c31 | 383 | } |
KoenKahlman | 1:1648ce825c31 | 384 | |
KoenKahlman | 1:1648ce825c31 | 385 | //TRAIN COMMAND |
KoenKahlman | 1:1648ce825c31 | 386 | void trainCommand(){ |
KoenKahlman | 1:1648ce825c31 | 387 | float Vin = potentio.read() * 3.3; |
KoenKahlman | 1:1648ce825c31 | 388 | speedsetting = floor(Vin/0.22); //0.22 = 3.3/15 |
KoenKahlman | 1:1648ce825c31 | 389 | |
KoenKahlman | 1:1648ce825c31 | 390 | if(boarding1 || boarding2){ |
KoenKahlman | 1:1648ce825c31 | 391 | if(station1.read() > stationWaitTime){ //timer works? TODO |
KoenKahlman | 1:1648ce825c31 | 392 | boarding1 = false; |
KoenKahlman | 1:1648ce825c31 | 393 | led1 = 0; |
KoenKahlman | 1:1648ce825c31 | 394 | station1.stop(); |
KoenKahlman | 1:1648ce825c31 | 395 | } |
KoenKahlman | 1:1648ce825c31 | 396 | |
KoenKahlman | 1:1648ce825c31 | 397 | if(station2.read() > stationWaitTime){ |
KoenKahlman | 1:1648ce825c31 | 398 | boarding2 = false; |
KoenKahlman | 1:1648ce825c31 | 399 | led2 = 0; |
KoenKahlman | 1:1648ce825c31 | 400 | station2.stop(); |
KoenKahlman | 1:1648ce825c31 | 401 | } |
KoenKahlman | 1:1648ce825c31 | 402 | } |
KoenKahlman | 1:1648ce825c31 | 403 | speed1 = boarding1?0:speedsetting; |
KoenKahlman | 1:1648ce825c31 | 404 | speed2 = boarding2?0:speedsetting; |
KoenKahlman | 1:1648ce825c31 | 405 | |
KoenKahlman | 1:1648ce825c31 | 406 | if(next1 == next2 && !collision){ |
KoenKahlman | 1:1648ce825c31 | 407 | headCollision(); |
KoenKahlman | 1:1648ce825c31 | 408 | collision = true; |
KoenKahlman | 1:1648ce825c31 | 409 | } else if ((next1 == curr2 || next2 == curr1) && !collision){ |
KoenKahlman | 1:1648ce825c31 | 410 | sideCollision(); |
KoenKahlman | 1:1648ce825c31 | 411 | collision = true; |
KoenKahlman | 1:1648ce825c31 | 412 | } else if (isXCollision() && !collision){ |
KoenKahlman | 1:1648ce825c31 | 413 | xCollision(); |
KoenKahlman | 1:1648ce825c31 | 414 | collision = true; |
KoenKahlman | 1:1648ce825c31 | 415 | } else if (next1 == prev2 && !command1){ |
KoenKahlman | 1:1648ce825c31 | 416 | tailCollision(1); |
KoenKahlman | 1:1648ce825c31 | 417 | command1 = true; |
KoenKahlman | 1:1648ce825c31 | 418 | } else if (next2 == prev1 && !command1){ |
KoenKahlman | 1:1648ce825c31 | 419 | tailCollision(2); |
KoenKahlman | 1:1648ce825c31 | 420 | command1 = true; |
KoenKahlman | 1:1648ce825c31 | 421 | } else if (curr1 == 2 && !command1){ |
KoenKahlman | 1:1648ce825c31 | 422 | atStation2(1); |
KoenKahlman | 1:1648ce825c31 | 423 | command1 = true; |
KoenKahlman | 1:1648ce825c31 | 424 | } else if (curr1 == 6 && !command1){ |
KoenKahlman | 1:1648ce825c31 | 425 | atSwitch3(1); |
KoenKahlman | 1:1648ce825c31 | 426 | command1 = true; |
KoenKahlman | 1:1648ce825c31 | 427 | } else if (curr1 == 8 && !command1){ |
KoenKahlman | 1:1648ce825c31 | 428 | atSwitch4(1); |
KoenKahlman | 1:1648ce825c31 | 429 | command1 = true; |
KoenKahlman | 1:1648ce825c31 | 430 | } else if (curr1 == 12 && !command1){ |
KoenKahlman | 1:1648ce825c31 | 431 | atStation12(1); |
KoenKahlman | 1:1648ce825c31 | 432 | command1 = true; |
KoenKahlman | 1:1648ce825c31 | 433 | } else if (curr2 == 2 && !command2){ |
KoenKahlman | 1:1648ce825c31 | 434 | atStation2(1); |
KoenKahlman | 1:1648ce825c31 | 435 | command2 = true; |
KoenKahlman | 1:1648ce825c31 | 436 | } else if (curr2 == 6 && !command2){ |
KoenKahlman | 1:1648ce825c31 | 437 | atSwitch3(2); |
KoenKahlman | 1:1648ce825c31 | 438 | command2 = true; |
KoenKahlman | 1:1648ce825c31 | 439 | } else if (curr2 == 8 && !command2){ |
KoenKahlman | 1:1648ce825c31 | 440 | atSwitch4(2); |
KoenKahlman | 1:1648ce825c31 | 441 | command2 = true; |
KoenKahlman | 1:1648ce825c31 | 442 | } else if (curr2 == 12 && !command2){ |
KoenKahlman | 1:1648ce825c31 | 443 | atStation12(2); |
KoenKahlman | 1:1648ce825c31 | 444 | command2 = true; |
KoenKahlman | 1:1648ce825c31 | 445 | } |
KoenKahlman | 1:1648ce825c31 | 446 | |
KoenKahlman | 1:1648ce825c31 | 447 | driveTrain(2); |
KoenKahlman | 1:1648ce825c31 | 448 | driveTrain(1); |
KoenKahlman | 1:1648ce825c31 | 449 | |
KoenKahlman | 1:1648ce825c31 | 450 | /* |
KoenKahlman | 1:1648ce825c31 | 451 | else if (!command1 && (curr1 == 9 && next1 == 3) || (curr1 == 5 && next1 == 11)){ |
KoenKahlman | 1:1648ce825c31 | 452 | loopDeltaCrossing(1); |
KoenKahlman | 1:1648ce825c31 | 453 | command1 = true; |
KoenKahlman | 1:1648ce825c31 | 454 | } else if (!command2 && (curr2 == 9 && next2 == 3) || (curr2 == 5 && next2 == 11)){ |
KoenKahlman | 1:1648ce825c31 | 455 | loopDeltaCrossing(2); |
KoenKahlman | 1:1648ce825c31 | 456 | command2 = true; |
KoenKahlman | 1:1648ce825c31 | 457 | } |
KoenKahlman | 1:1648ce825c31 | 458 | */ |
KoenKahlman | 1:1648ce825c31 | 459 | } |
KoenKahlman | 1:1648ce825c31 | 460 | |
KoenKahlman | 1:1648ce825c31 | 461 | bool isXCollision(){ |
KoenKahlman | 1:1648ce825c31 | 462 | return (curr1 == 3 && next1 == 9) && (next2 == 5 || next2 == 11) |
KoenKahlman | 1:1648ce825c31 | 463 | || (curr1 == 5 && next1 == 11) && (next2 == 3 || next2 == 9) |
KoenKahlman | 1:1648ce825c31 | 464 | || (curr1 == 9 && next1 == 3) && (next2 == 5 || next2 == 11) |
KoenKahlman | 1:1648ce825c31 | 465 | || (curr1 == 11 && next1 == 5) && (next2 == 3 || next2 == 9) |
KoenKahlman | 1:1648ce825c31 | 466 | || (curr2 == 3 && next2 == 9) && (next1 == 5 || next1 == 11) |
KoenKahlman | 1:1648ce825c31 | 467 | || (curr2 == 5 && next2 == 11) && (next1 == 3 || next1 == 9) |
KoenKahlman | 1:1648ce825c31 | 468 | || (curr2 == 9 && next2 == 3) && (next1 == 5 || next1 == 11) |
KoenKahlman | 1:1648ce825c31 | 469 | || (curr2 == 11 && next2 == 5) && (next1 == 3 || next1 == 9); |
KoenKahlman | 1:1648ce825c31 | 470 | } |
KoenKahlman | 1:1648ce825c31 | 471 | |
KoenKahlman | 1:1648ce825c31 | 472 | void stopTrain(int train){ |
KoenKahlman | 1:1648ce825c31 | 473 | if(train == 1){ |
KoenKahlman | 1:1648ce825c31 | 474 | DCC_send_command(DCCaddress_train1, DCCinst_estop, 10); |
KoenKahlman | 1:1648ce825c31 | 475 | pc.printf("STATUS: Train 1 stopped\n\r"); |
KoenKahlman | 1:1648ce825c31 | 476 | } else { //train == 2 |
KoenKahlman | 1:1648ce825c31 | 477 | DCC_send_command(DCCaddress_train2, DCCinst_estop, 10); |
KoenKahlman | 1:1648ce825c31 | 478 | pc.printf("STATUS: Train 2 stopped\n\r"); |
KoenKahlman | 1:1648ce825c31 | 479 | } |
KoenKahlman | 1:1648ce825c31 | 480 | } |
KoenKahlman | 1:1648ce825c31 | 481 | |
KoenKahlman | 1:1648ce825c31 | 482 | void reverseTrain(int train){ //always stop before reversing |
KoenKahlman | 1:1648ce825c31 | 483 | if(train == 1){ |
KoenKahlman | 1:1648ce825c31 | 484 | int stupid; |
KoenKahlman | 1:1648ce825c31 | 485 | dir1 = !dir1; |
KoenKahlman | 1:1648ce825c31 | 486 | stupid = next1; |
KoenKahlman | 1:1648ce825c31 | 487 | next1 = curr1; |
KoenKahlman | 1:1648ce825c31 | 488 | curr1 = stupid; |
KoenKahlman | 1:1648ce825c31 | 489 | prev1 = predictNextMarker(curr1, next1); |
KoenKahlman | 1:1648ce825c31 | 490 | pc.printf("STATUS: Train 1 reversed\n\r"); |
KoenKahlman | 1:1648ce825c31 | 491 | } else { //train == 2 |
KoenKahlman | 1:1648ce825c31 | 492 | int swap; |
KoenKahlman | 1:1648ce825c31 | 493 | dir2 = !dir2; |
KoenKahlman | 1:1648ce825c31 | 494 | swap = next2; |
KoenKahlman | 1:1648ce825c31 | 495 | next2 = curr2; |
KoenKahlman | 1:1648ce825c31 | 496 | curr2 = swap; |
KoenKahlman | 1:1648ce825c31 | 497 | prev2 = predictNextMarker(curr2, next2); |
KoenKahlman | 1:1648ce825c31 | 498 | pc.printf("STATUS: Train 2 reversed\n\r"); |
KoenKahlman | 1:1648ce825c31 | 499 | } |
KoenKahlman | 1:1648ce825c31 | 500 | } |
KoenKahlman | 1:1648ce825c31 | 501 | |
KoenKahlman | 1:1648ce825c31 | 502 | void driveTrain(int train){ |
KoenKahlman | 1:1648ce825c31 | 503 | if(train == 1){ |
KoenKahlman | 1:1648ce825c31 | 504 | DCC_send_command(DCCaddress_train1, trainInstruction(dir1, light1, speed1), 25); |
KoenKahlman | 1:1648ce825c31 | 505 | } else { //train == 2 |
KoenKahlman | 1:1648ce825c31 | 506 | DCC_send_command(DCCaddress_train2, trainInstruction(dir2, light2, speed2), 25); |
KoenKahlman | 1:1648ce825c31 | 507 | } |
KoenKahlman | 1:1648ce825c31 | 508 | } |
KoenKahlman | 1:1648ce825c31 | 509 | |
KoenKahlman | 1:1648ce825c31 | 510 | void atStation2(int train){ |
KoenKahlman | 1:1648ce825c31 | 511 | if(train == 1){ |
KoenKahlman | 1:1648ce825c31 | 512 | if(next1 != 22){ |
KoenKahlman | 1:1648ce825c31 | 513 | pc.printf("STATUS: train 1 reached switch 2\n\r"); |
KoenKahlman | 1:1648ce825c31 | 514 | stopTrain(1); |
KoenKahlman | 1:1648ce825c31 | 515 | if(curr2 == 6 || next2 == 6){ |
KoenKahlman | 1:1648ce825c31 | 516 | switch2 = true; |
KoenKahlman | 1:1648ce825c31 | 517 | } else if(curr2 == 9 || next2 == 9){ |
KoenKahlman | 1:1648ce825c31 | 518 | switch2 = false; |
KoenKahlman | 1:1648ce825c31 | 519 | } else { //other train far away |
KoenKahlman | 1:1648ce825c31 | 520 | switch2 = rand()%2; |
KoenKahlman | 1:1648ce825c31 | 521 | } |
KoenKahlman | 1:1648ce825c31 | 522 | changeSwitch2(switch2); |
KoenKahlman | 1:1648ce825c31 | 523 | } |
KoenKahlman | 1:1648ce825c31 | 524 | atStation(1); |
KoenKahlman | 1:1648ce825c31 | 525 | } else { //train == 2 |
KoenKahlman | 1:1648ce825c31 | 526 | if(next2 != 22){ |
KoenKahlman | 1:1648ce825c31 | 527 | pc.printf("STATUS: train 2 reached switch 2\n\r"); |
KoenKahlman | 1:1648ce825c31 | 528 | stopTrain(2); |
KoenKahlman | 1:1648ce825c31 | 529 | if(curr1 == 6 || next1 == 6){ |
KoenKahlman | 1:1648ce825c31 | 530 | //switch2 = true; |
KoenKahlman | 1:1648ce825c31 | 531 | } else if(curr1 == 9 || next1 == 9){ |
KoenKahlman | 1:1648ce825c31 | 532 | //switch2 = false; |
KoenKahlman | 1:1648ce825c31 | 533 | } else { //other train far away |
KoenKahlman | 1:1648ce825c31 | 534 | switch2 = rand()%2; |
KoenKahlman | 1:1648ce825c31 | 535 | } |
KoenKahlman | 1:1648ce825c31 | 536 | changeSwitch2(switch2); |
KoenKahlman | 1:1648ce825c31 | 537 | } |
KoenKahlman | 1:1648ce825c31 | 538 | atStation(2); |
KoenKahlman | 1:1648ce825c31 | 539 | } |
KoenKahlman | 1:1648ce825c31 | 540 | } |
KoenKahlman | 1:1648ce825c31 | 541 | |
KoenKahlman | 1:1648ce825c31 | 542 | void atStation12(int train){ |
KoenKahlman | 1:1648ce825c31 | 543 | if(train == 1){ |
KoenKahlman | 1:1648ce825c31 | 544 | if(next1 != 22){ |
KoenKahlman | 1:1648ce825c31 | 545 | pc.printf("STATUS: train 1 reached switch 1\n\r"); |
KoenKahlman | 1:1648ce825c31 | 546 | stopTrain(1); |
KoenKahlman | 1:1648ce825c31 | 547 | if(curr2 == 8 || next2 == 8){ |
KoenKahlman | 1:1648ce825c31 | 548 | //switch1 = true; |
KoenKahlman | 1:1648ce825c31 | 549 | } else if(curr2 == 5 || next2 == 5){ |
KoenKahlman | 1:1648ce825c31 | 550 | //switch1 = false; |
KoenKahlman | 1:1648ce825c31 | 551 | } else { //other train far away |
KoenKahlman | 1:1648ce825c31 | 552 | switch1 = rand()%2; |
KoenKahlman | 1:1648ce825c31 | 553 | } |
KoenKahlman | 1:1648ce825c31 | 554 | changeSwitch1(switch1); |
KoenKahlman | 1:1648ce825c31 | 555 | } |
KoenKahlman | 1:1648ce825c31 | 556 | atStation(1); |
KoenKahlman | 1:1648ce825c31 | 557 | } else { //train == 2 |
KoenKahlman | 1:1648ce825c31 | 558 | if(next2 != 22){ |
KoenKahlman | 1:1648ce825c31 | 559 | pc.printf("STATUS: train 2 reached switch 1\n\r"); |
KoenKahlman | 1:1648ce825c31 | 560 | stopTrain(2); |
KoenKahlman | 1:1648ce825c31 | 561 | if(curr1 == 8 || next1 == 8){ |
KoenKahlman | 1:1648ce825c31 | 562 | //switch1 = true; |
KoenKahlman | 1:1648ce825c31 | 563 | } else if(curr1 == 5 || next1 == 5){ |
KoenKahlman | 1:1648ce825c31 | 564 | //switch1 = false; |
KoenKahlman | 1:1648ce825c31 | 565 | } else { //other train far away |
KoenKahlman | 1:1648ce825c31 | 566 | switch1 = rand()%2; |
KoenKahlman | 1:1648ce825c31 | 567 | } |
KoenKahlman | 1:1648ce825c31 | 568 | changeSwitch1(switch1); |
KoenKahlman | 1:1648ce825c31 | 569 | } |
KoenKahlman | 1:1648ce825c31 | 570 | atStation(2); |
KoenKahlman | 1:1648ce825c31 | 571 | } |
KoenKahlman | 1:1648ce825c31 | 572 | } |
KoenKahlman | 1:1648ce825c31 | 573 | |
KoenKahlman | 1:1648ce825c31 | 574 | void atStation(int train){ |
KoenKahlman | 1:1648ce825c31 | 575 | if(train == 1){ |
KoenKahlman | 1:1648ce825c31 | 576 | pc.printf("STATUS: Train 1 reached station!\n\r"); |
KoenKahlman | 1:1648ce825c31 | 577 | boxled1 = 1; |
KoenKahlman | 1:1648ce825c31 | 578 | speed1 = 0; |
KoenKahlman | 1:1648ce825c31 | 579 | station1.start(); |
KoenKahlman | 1:1648ce825c31 | 580 | boarding1 = true; |
KoenKahlman | 1:1648ce825c31 | 581 | } else { //train == 2 |
KoenKahlman | 1:1648ce825c31 | 582 | pc.printf("STATUS: Train 2 reached station!\n\r"); |
KoenKahlman | 1:1648ce825c31 | 583 | boxled2 = 1; |
KoenKahlman | 1:1648ce825c31 | 584 | speed2 = 0; |
KoenKahlman | 1:1648ce825c31 | 585 | station2.start(); |
KoenKahlman | 1:1648ce825c31 | 586 | boarding2 = true; |
KoenKahlman | 1:1648ce825c31 | 587 | } |
KoenKahlman | 1:1648ce825c31 | 588 | } |
KoenKahlman | 1:1648ce825c31 | 589 | |
KoenKahlman | 1:1648ce825c31 | 590 | void atSwitch3(int train){ |
KoenKahlman | 1:1648ce825c31 | 591 | if(train == 1){ |
KoenKahlman | 1:1648ce825c31 | 592 | if(next1 == 7){ return; } |
KoenKahlman | 1:1648ce825c31 | 593 | pc.printf("STATUS: train 1 reached switch 3\n\r"); |
KoenKahlman | 1:1648ce825c31 | 594 | stopTrain(1); |
KoenKahlman | 1:1648ce825c31 | 595 | if(curr2 == 2 || next2 == 2){ |
KoenKahlman | 1:1648ce825c31 | 596 | //switch3 = true; TODO complicated |
KoenKahlman | 1:1648ce825c31 | 597 | } else if(curr2 == 11 || next2 == 11){ |
KoenKahlman | 1:1648ce825c31 | 598 | //switch3 = false; |
KoenKahlman | 1:1648ce825c31 | 599 | } else { //other train far away |
KoenKahlman | 1:1648ce825c31 | 600 | switch3 = rand()%2; |
KoenKahlman | 1:1648ce825c31 | 601 | } |
KoenKahlman | 1:1648ce825c31 | 602 | changeSwitch3(switch3); |
KoenKahlman | 1:1648ce825c31 | 603 | } else { //train == 2 |
KoenKahlman | 1:1648ce825c31 | 604 | if(next2 == 7){ return; } |
KoenKahlman | 1:1648ce825c31 | 605 | pc.printf("STATUS: train 2 reached switch 3\n\r"); |
KoenKahlman | 1:1648ce825c31 | 606 | stopTrain(2); |
KoenKahlman | 1:1648ce825c31 | 607 | if(curr1 == 2 || next1 == 2){ |
KoenKahlman | 1:1648ce825c31 | 608 | //switch3 = true; |
KoenKahlman | 1:1648ce825c31 | 609 | } else if(curr1 == 11 || next1 == 11){ |
KoenKahlman | 1:1648ce825c31 | 610 | //switch3 = false; |
KoenKahlman | 1:1648ce825c31 | 611 | } else { //other train far away |
KoenKahlman | 1:1648ce825c31 | 612 | switch3 = rand()%2; |
KoenKahlman | 1:1648ce825c31 | 613 | } |
KoenKahlman | 1:1648ce825c31 | 614 | changeSwitch3(switch3); |
KoenKahlman | 1:1648ce825c31 | 615 | } |
KoenKahlman | 1:1648ce825c31 | 616 | } |
KoenKahlman | 1:1648ce825c31 | 617 | |
KoenKahlman | 1:1648ce825c31 | 618 | void atSwitch4(int train){ |
KoenKahlman | 1:1648ce825c31 | 619 | if(train == 1){ |
KoenKahlman | 1:1648ce825c31 | 620 | if(next1 == 7){ return; } |
KoenKahlman | 1:1648ce825c31 | 621 | pc.printf("STATUS: train 1 reached switch 4\n\r"); |
KoenKahlman | 1:1648ce825c31 | 622 | stopTrain(1); |
KoenKahlman | 1:1648ce825c31 | 623 | if(curr2 == 2 || next2 == 2){ |
KoenKahlman | 1:1648ce825c31 | 624 | //switch4 = true; |
KoenKahlman | 1:1648ce825c31 | 625 | } else if(curr2 == 11 || next2 == 11){ |
KoenKahlman | 1:1648ce825c31 | 626 | //switch4 = false; |
KoenKahlman | 1:1648ce825c31 | 627 | } else { //other train far away |
KoenKahlman | 1:1648ce825c31 | 628 | switch4 = rand()%2; |
KoenKahlman | 1:1648ce825c31 | 629 | } |
KoenKahlman | 1:1648ce825c31 | 630 | changeSwitch4(switch4); |
KoenKahlman | 1:1648ce825c31 | 631 | } else { //train == 2 |
KoenKahlman | 1:1648ce825c31 | 632 | if(next2 == 7){ return; } |
KoenKahlman | 1:1648ce825c31 | 633 | pc.printf("STATUS: train 2 reached switch 4\n\r"); |
KoenKahlman | 1:1648ce825c31 | 634 | stopTrain(2); |
KoenKahlman | 1:1648ce825c31 | 635 | if(curr1 == 2 || next1 == 2){ |
KoenKahlman | 1:1648ce825c31 | 636 | //switch4 = true; |
KoenKahlman | 1:1648ce825c31 | 637 | } else if(curr1 == 11 || next1 == 11){ |
KoenKahlman | 1:1648ce825c31 | 638 | //switch4 = false; |
KoenKahlman | 1:1648ce825c31 | 639 | } else { //other train far away |
KoenKahlman | 1:1648ce825c31 | 640 | switch4 = rand()%2; |
KoenKahlman | 1:1648ce825c31 | 641 | } |
KoenKahlman | 1:1648ce825c31 | 642 | changeSwitch4(switch4); |
KoenKahlman | 1:1648ce825c31 | 643 | } |
KoenKahlman | 1:1648ce825c31 | 644 | } |
KoenKahlman | 1:1648ce825c31 | 645 | |
KoenKahlman | 1:1648ce825c31 | 646 | void headCollision(void){ |
KoenKahlman | 1:1648ce825c31 | 647 | buzzer = 1; |
KoenKahlman | 1:1648ce825c31 | 648 | pc.printf("WARNING: inbound head collision\n\r"); |
KoenKahlman | 1:1648ce825c31 | 649 | stopTrain(1); |
KoenKahlman | 1:1648ce825c31 | 650 | stopTrain(2); |
KoenKahlman | 1:1648ce825c31 | 651 | reverseTrain(1); |
KoenKahlman | 1:1648ce825c31 | 652 | reverseTrain(2); |
KoenKahlman | 1:1648ce825c31 | 653 | buzzer = 0; |
KoenKahlman | 1:1648ce825c31 | 654 | } |
KoenKahlman | 1:1648ce825c31 | 655 | |
KoenKahlman | 1:1648ce825c31 | 656 | void sideCollision(void){ |
KoenKahlman | 1:1648ce825c31 | 657 | buzzer = 1; |
KoenKahlman | 1:1648ce825c31 | 658 | pc.printf("WARNING: possible side collision\n\r"); |
KoenKahlman | 1:1648ce825c31 | 659 | stopTrain(1); |
KoenKahlman | 1:1648ce825c31 | 660 | stopTrain(2); |
KoenKahlman | 1:1648ce825c31 | 661 | reverseTrain(1); |
KoenKahlman | 1:1648ce825c31 | 662 | reverseTrain(2); |
KoenKahlman | 1:1648ce825c31 | 663 | buzzer = 0; |
KoenKahlman | 1:1648ce825c31 | 664 | } |
KoenKahlman | 1:1648ce825c31 | 665 | |
KoenKahlman | 1:1648ce825c31 | 666 | void xCollision(void){ |
KoenKahlman | 1:1648ce825c31 | 667 | buzzer = 1; |
KoenKahlman | 1:1648ce825c31 | 668 | pc.printf("WARNING: possible cross collision\n\r"); |
KoenKahlman | 1:1648ce825c31 | 669 | stopTrain(1); |
KoenKahlman | 1:1648ce825c31 | 670 | stopTrain(2); |
KoenKahlman | 1:1648ce825c31 | 671 | reverseTrain(1); |
KoenKahlman | 1:1648ce825c31 | 672 | reverseTrain(2); |
KoenKahlman | 1:1648ce825c31 | 673 | buzzer = 0; |
KoenKahlman | 1:1648ce825c31 | 674 | } |
KoenKahlman | 1:1648ce825c31 | 675 | |
KoenKahlman | 1:1648ce825c31 | 676 | void tailCollision(int backTrain){ |
KoenKahlman | 1:1648ce825c31 | 677 | buzzer = 1; |
KoenKahlman | 1:1648ce825c31 | 678 | pc.printf("WARNING: possible tail collision\n\r"); |
KoenKahlman | 1:1648ce825c31 | 679 | stopTrain(backTrain); |
KoenKahlman | 1:1648ce825c31 | 680 | reverseTrain(backTrain); |
KoenKahlman | 1:1648ce825c31 | 681 | buzzer = 0; |
KoenKahlman | 1:1648ce825c31 | 682 | } |
KoenKahlman | 1:1648ce825c31 | 683 | /* |
KoenKahlman | 1:1648ce825c31 | 684 | void loopDeltaCrossing(int train){ |
KoenKahlman | 1:1648ce825c31 | 685 | buzzer = 1; |
KoenKahlman | 1:1648ce825c31 | 686 | pc.printf("WARNING: attempting to exit the loop\n\r"); |
KoenKahlman | 1:1648ce825c31 | 687 | if(train == 1){ |
KoenKahlman | 1:1648ce825c31 | 688 | if(curr2 != 13 && curr2 != 0 && curr2 != 1 && curr2 != 2){ |
KoenKahlman | 1:1648ce825c31 | 689 | stopTrain(1); |
KoenKahlman | 1:1648ce825c31 | 690 | reverseTrain(1); |
KoenKahlman | 1:1648ce825c31 | 691 | } |
KoenKahlman | 1:1648ce825c31 | 692 | //if train 2 in loop |
KoenKahlman | 1:1648ce825c31 | 693 | //stop, reverse |
KoenKahlman | 1:1648ce825c31 | 694 | } else { //train == 2 |
KoenKahlman | 1:1648ce825c31 | 695 | //if train 1 in loop |
KoenKahlman | 1:1648ce825c31 | 696 | //stop, reverse |
KoenKahlman | 1:1648ce825c31 | 697 | if(curr1 != 13 && curr1 != 0 && curr1 != 1 && curr1 != 2){ |
KoenKahlman | 1:1648ce825c31 | 698 | stopTrain(1); |
KoenKahlman | 1:1648ce825c31 | 699 | reverseTrain(1); |
KoenKahlman | 1:1648ce825c31 | 700 | } |
KoenKahlman | 1:1648ce825c31 | 701 | } |
KoenKahlman | 1:1648ce825c31 | 702 | buzzer = 0; |
KoenKahlman | 1:1648ce825c31 | 703 | } |
KoenKahlman | 1:1648ce825c31 | 704 | */ |
KoenKahlman | 0:9c82986d7cb9 | 705 | |
KoenKahlman | 0:9c82986d7cb9 | 706 | |
KoenKahlman | 0:9c82986d7cb9 | 707 | |
KoenKahlman | 0:9c82986d7cb9 | 708 | |
KoenKahlman | 0:9c82986d7cb9 | 709 | //TRACK COMMANDS |
KoenKahlman | 0:9c82986d7cb9 | 710 | //code from the tutorial, sends a message to the track through the train pin |
KoenKahlman | 0:9c82986d7cb9 | 711 | void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count){ |
KoenKahlman | 0:9c82986d7cb9 | 712 | switch(address){ |
KoenKahlman | 1:1648ce825c31 | 713 | case DCCaddress_train1: led1 = 1; break; |
KoenKahlman | 1:1648ce825c31 | 714 | case DCCaddress_train2: led2 = 1; break; |
KoenKahlman | 1:1648ce825c31 | 715 | case DCCaddress_decoder: led3 = 1; break; |
KoenKahlman | 0:9c82986d7cb9 | 716 | } |
KoenKahlman | 0:9c82986d7cb9 | 717 | |
KoenKahlman | 0:9c82986d7cb9 | 718 | unsigned __int64 command = 0x0000000000000000; // __int64 is the 64-bit integer type |
KoenKahlman | 0:9c82986d7cb9 | 719 | unsigned __int64 temp_command = 0x0000000000000000; |
KoenKahlman | 0:9c82986d7cb9 | 720 | unsigned __int64 prefix = 0x3FFF; // 14 "1" bits needed at start |
KoenKahlman | 0:9c82986d7cb9 | 721 | unsigned int error = 0x00; //error byte |
KoenKahlman | 0:9c82986d7cb9 | 722 | //calculate error detection byte with xor |
KoenKahlman | 0:9c82986d7cb9 | 723 | error = address ^ inst; |
KoenKahlman | 0:9c82986d7cb9 | 724 | //combine packet bits in basic DCC format |
KoenKahlman | 0:9c82986d7cb9 | 725 | command = (prefix<<28)|(address<<19)|(inst<<10)|((error)<<1)|0x01; |
KoenKahlman | 0:9c82986d7cb9 | 726 | //printf("\n\r %llx \n\r",command); |
KoenKahlman | 0:9c82986d7cb9 | 727 | int i=0; |
KoenKahlman | 0:9c82986d7cb9 | 728 | //repeat DCC command lots of times |
KoenKahlman | 0:9c82986d7cb9 | 729 | while(i < repeat_count) { |
KoenKahlman | 0:9c82986d7cb9 | 730 | temp_command = command; |
KoenKahlman | 0:9c82986d7cb9 | 731 | //loops through packet bits encoding and sending out digital pulses for a DCC command |
KoenKahlman | 0:9c82986d7cb9 | 732 | for (int j=0; j<64; j++) { |
KoenKahlman | 0:9c82986d7cb9 | 733 | if((temp_command&0x8000000000000000)==0) { //test packet bit |
KoenKahlman | 0:9c82986d7cb9 | 734 | //send data for a "0" bit |
KoenKahlman | 0:9c82986d7cb9 | 735 | data=0; |
KoenKahlman | 0:9c82986d7cb9 | 736 | wait_us(100); |
KoenKahlman | 0:9c82986d7cb9 | 737 | data=1; |
KoenKahlman | 0:9c82986d7cb9 | 738 | wait_us(100); |
KoenKahlman | 0:9c82986d7cb9 | 739 | //printf("0011"); |
KoenKahlman | 0:9c82986d7cb9 | 740 | } else { |
KoenKahlman | 0:9c82986d7cb9 | 741 | //send data for a "1"bit |
KoenKahlman | 0:9c82986d7cb9 | 742 | data=0; |
KoenKahlman | 0:9c82986d7cb9 | 743 | wait_us(58); |
KoenKahlman | 0:9c82986d7cb9 | 744 | data=1; |
KoenKahlman | 0:9c82986d7cb9 | 745 | wait_us(58); |
KoenKahlman | 0:9c82986d7cb9 | 746 | //printf("01"); |
KoenKahlman | 0:9c82986d7cb9 | 747 | } |
KoenKahlman | 0:9c82986d7cb9 | 748 | // next bit in packet |
KoenKahlman | 0:9c82986d7cb9 | 749 | temp_command = temp_command<<1; |
KoenKahlman | 0:9c82986d7cb9 | 750 | } |
KoenKahlman | 0:9c82986d7cb9 | 751 | i++; |
KoenKahlman | 0:9c82986d7cb9 | 752 | } |
KoenKahlman | 0:9c82986d7cb9 | 753 | |
KoenKahlman | 0:9c82986d7cb9 | 754 | led1 = 0; |
KoenKahlman | 0:9c82986d7cb9 | 755 | led2 = 0; |
KoenKahlman | 1:1648ce825c31 | 756 | led3 = 0; |
KoenKahlman | 0:9c82986d7cb9 | 757 | } |
KoenKahlman | 0:9c82986d7cb9 | 758 | |
KoenKahlman | 0:9c82986d7cb9 | 759 | void changeSwitch1(bool inwards){ |
KoenKahlman | 0:9c82986d7cb9 | 760 | if(inwards){ |
KoenKahlman | 0:9c82986d7cb9 | 761 | DCC_send_command(DCCaddress_decoder, DCCswitch1, 1); |
KoenKahlman | 1:1648ce825c31 | 762 | pc.printf("STATUS: Switch 1 changed inward\n\r"); |
KoenKahlman | 0:9c82986d7cb9 | 763 | } else { |
KoenKahlman | 0:9c82986d7cb9 | 764 | DCC_send_command(DCCaddress_decoder, DCCswitch1, 1); |
KoenKahlman | 0:9c82986d7cb9 | 765 | DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1); |
KoenKahlman | 1:1648ce825c31 | 766 | pc.printf("STATUS: Switch 1 changed outward\n\r"); |
KoenKahlman | 0:9c82986d7cb9 | 767 | } |
KoenKahlman | 1:1648ce825c31 | 768 | updateTrainPredictions(); |
KoenKahlman | 0:9c82986d7cb9 | 769 | } |
KoenKahlman | 0:9c82986d7cb9 | 770 | |
KoenKahlman | 0:9c82986d7cb9 | 771 | void changeSwitch2(bool inwards){ |
KoenKahlman | 0:9c82986d7cb9 | 772 | if(inwards){ |
KoenKahlman | 0:9c82986d7cb9 | 773 | DCC_send_command(DCCaddress_decoder, DCCswitch2, 1); |
KoenKahlman | 1:1648ce825c31 | 774 | pc.printf("STATUS: Switch 2 changed inward\n\r"); |
KoenKahlman | 0:9c82986d7cb9 | 775 | } else { |
KoenKahlman | 0:9c82986d7cb9 | 776 | DCC_send_command(DCCaddress_decoder, DCCswitch2, 1); |
KoenKahlman | 0:9c82986d7cb9 | 777 | DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1); |
KoenKahlman | 1:1648ce825c31 | 778 | pc.printf("STATUS: Switch 2 changed outward\n\r"); |
KoenKahlman | 1:1648ce825c31 | 779 | } |
KoenKahlman | 1:1648ce825c31 | 780 | updateTrainPredictions(); |
KoenKahlman | 0:9c82986d7cb9 | 781 | } |
KoenKahlman | 0:9c82986d7cb9 | 782 | |
KoenKahlman | 0:9c82986d7cb9 | 783 | void changeSwitch3(bool inwards){ |
KoenKahlman | 0:9c82986d7cb9 | 784 | if(inwards){ |
KoenKahlman | 0:9c82986d7cb9 | 785 | DCC_send_command(DCCaddress_decoder, DCCswitch3, 1); |
KoenKahlman | 0:9c82986d7cb9 | 786 | DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1); //switch 3 has reverse behavior |
KoenKahlman | 1:1648ce825c31 | 787 | pc.printf("STATUS: Switch 3 changed inward\n\r"); |
KoenKahlman | 0:9c82986d7cb9 | 788 | } else { |
KoenKahlman | 0:9c82986d7cb9 | 789 | DCC_send_command(DCCaddress_decoder, DCCswitch3, 1); |
KoenKahlman | 1:1648ce825c31 | 790 | pc.printf("STATUS: Switch 3 changed outward\n\r"); |
KoenKahlman | 1:1648ce825c31 | 791 | } |
KoenKahlman | 1:1648ce825c31 | 792 | updateTrainPredictions(); |
KoenKahlman | 0:9c82986d7cb9 | 793 | } |
KoenKahlman | 0:9c82986d7cb9 | 794 | |
KoenKahlman | 0:9c82986d7cb9 | 795 | void changeSwitch4(bool inwards){ |
KoenKahlman | 0:9c82986d7cb9 | 796 | if(inwards){ |
KoenKahlman | 0:9c82986d7cb9 | 797 | DCC_send_command(DCCaddress_decoder, DCCswitch4, 1); |
KoenKahlman | 1:1648ce825c31 | 798 | pc.printf("STATUS: Switch 4 changed inward\n\r"); |
KoenKahlman | 0:9c82986d7cb9 | 799 | } else { |
KoenKahlman | 0:9c82986d7cb9 | 800 | DCC_send_command(DCCaddress_decoder, DCCswitch4, 1); |
KoenKahlman | 0:9c82986d7cb9 | 801 | DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1); |
KoenKahlman | 1:1648ce825c31 | 802 | pc.printf("STATUS: Switch 4 changed outward\n\r"); |
KoenKahlman | 1:1648ce825c31 | 803 | } |
KoenKahlman | 1:1648ce825c31 | 804 | updateTrainPredictions(); |
KoenKahlman | 1:1648ce825c31 | 805 | } |
KoenKahlman | 1:1648ce825c31 | 806 | |
KoenKahlman | 1:1648ce825c31 | 807 | void updateTrainPredictions(){ |
KoenKahlman | 1:1648ce825c31 | 808 | next1 = predictNextMarker(prev1, curr1); |
KoenKahlman | 1:1648ce825c31 | 809 | next2 = predictNextMarker(prev2, curr2); |
KoenKahlman | 0:9c82986d7cb9 | 810 | } |
KoenKahlman | 0:9c82986d7cb9 | 811 | |
KoenKahlman | 0:9c82986d7cb9 | 812 | unsigned int trainInstruction(bool direction, bool light, int speed){ |
KoenKahlman | 0:9c82986d7cb9 | 813 | unsigned int result = 0x40; //0100 0000 |
KoenKahlman | 0:9c82986d7cb9 | 814 | if(direction){ |
KoenKahlman | 0:9c82986d7cb9 | 815 | result += 0x20; //0010 0000 |
KoenKahlman | 0:9c82986d7cb9 | 816 | } |
KoenKahlman | 0:9c82986d7cb9 | 817 | if(light){ |
KoenKahlman | 0:9c82986d7cb9 | 818 | result += 0x10; //0001 0000 |
KoenKahlman | 0:9c82986d7cb9 | 819 | } |
KoenKahlman | 0:9c82986d7cb9 | 820 | if(speed>0 && speed<15){ |
KoenKahlman | 0:9c82986d7cb9 | 821 | result += speed + 1; |
KoenKahlman | 0:9c82986d7cb9 | 822 | } |
KoenKahlman | 0:9c82986d7cb9 | 823 | return result; |
KoenKahlman | 0:9c82986d7cb9 | 824 | } |
KoenKahlman | 0:9c82986d7cb9 | 825 | |
KoenKahlman | 0:9c82986d7cb9 | 826 | //INTERPRET INTERRUPT SIGNALS |
KoenKahlman | 0:9c82986d7cb9 | 827 | //INTERRUPTS |
KoenKahlman | 0:9c82986d7cb9 | 828 | //code from the tutorial, initializes MCP and sets it up for interrupts |
KoenKahlman | 0:9c82986d7cb9 | 829 | void init_mcp() { |
KoenKahlman | 0:9c82986d7cb9 | 830 | // Initialisation of MCP registers, documentation on registers is available at |
KoenKahlman | 0:9c82986d7cb9 | 831 | //Niels/Abel/Robert/Natalia |
KoenKahlman | 0:9c82986d7cb9 | 832 | mcp = new MCP23017(i2c, 0x40); |
KoenKahlman | 0:9c82986d7cb9 | 833 | mcp->_write(IODIRA, (unsigned char )0xff); |
KoenKahlman | 0:9c82986d7cb9 | 834 | mcp->_write(IODIRB, (unsigned char )0xff); |
KoenKahlman | 0:9c82986d7cb9 | 835 | mcp->_write(IPOLA, (unsigned char )0x00); |
KoenKahlman | 0:9c82986d7cb9 | 836 | mcp->_write(IPOLB, (unsigned char )0x00); |
KoenKahlman | 0:9c82986d7cb9 | 837 | mcp->_write(DEFVALA, (unsigned char )0xff); |
KoenKahlman | 0:9c82986d7cb9 | 838 | mcp->_write(DEFVALB, (unsigned char )0xff); |
KoenKahlman | 0:9c82986d7cb9 | 839 | mcp->_write(INTCONA, (unsigned char )0xff); |
KoenKahlman | 0:9c82986d7cb9 | 840 | mcp->_write(INTCONB, (unsigned char )0xff); |
KoenKahlman | 0:9c82986d7cb9 | 841 | mcp->_write(IOCONA, (unsigned char )0x2); |
KoenKahlman | 0:9c82986d7cb9 | 842 | mcp->_write(IOCONB, (unsigned char )0x2); |
KoenKahlman | 0:9c82986d7cb9 | 843 | mcp->_write(GPPUA, (unsigned char )0xff); |
KoenKahlman | 0:9c82986d7cb9 | 844 | mcp->_write(GPPUB, (unsigned char )0xff); |
KoenKahlman | 0:9c82986d7cb9 | 845 | |
KoenKahlman | 0:9c82986d7cb9 | 846 | // Clear current interrupts |
KoenKahlman | 0:9c82986d7cb9 | 847 | mcp->_read(GPIOA); |
KoenKahlman | 0:9c82986d7cb9 | 848 | mcp->_read(GPIOB); |
KoenKahlman | 0:9c82986d7cb9 | 849 | // Register callbacks |
KoenKahlman | 0:9c82986d7cb9 | 850 | int0.fall(&on_int0_change); |
KoenKahlman | 0:9c82986d7cb9 | 851 | int1.fall(&on_int1_change); |
KoenKahlman | 0:9c82986d7cb9 | 852 | // Enable interrupts on the MCP |
KoenKahlman | 0:9c82986d7cb9 | 853 | mcp->_write(GPINTENA, (unsigned char )0xff); |
KoenKahlman | 0:9c82986d7cb9 | 854 | mcp->_write(GPINTENB, (unsigned char )0xff); |
KoenKahlman | 0:9c82986d7cb9 | 855 | |
KoenKahlman | 0:9c82986d7cb9 | 856 | int0flag = false; |
KoenKahlman | 0:9c82986d7cb9 | 857 | int1flag = false; |
KoenKahlman | 0:9c82986d7cb9 | 858 | } |
KoenKahlman | 0:9c82986d7cb9 | 859 | |
KoenKahlman | 0:9c82986d7cb9 | 860 | void on_int0_change() { |
KoenKahlman | 0:9c82986d7cb9 | 861 | int0flag = true; |
KoenKahlman | 0:9c82986d7cb9 | 862 | } |
KoenKahlman | 0:9c82986d7cb9 | 863 | |
KoenKahlman | 0:9c82986d7cb9 | 864 | void on_int1_change() { |
KoenKahlman | 0:9c82986d7cb9 | 865 | int1flag = true; |
KoenKahlman | 0:9c82986d7cb9 | 866 | } |
KoenKahlman | 0:9c82986d7cb9 | 867 | |
KoenKahlman | 0:9c82986d7cb9 | 868 | std::list<int> circuit0markers(int sensor_data){ |
KoenKahlman | 0:9c82986d7cb9 | 869 | std::list<int> result; |
KoenKahlman | 0:9c82986d7cb9 | 870 | sensor_data = ~sensor_data; |
KoenKahlman | 0:9c82986d7cb9 | 871 | if((sensor_data & 0b00000001) == 0b00000001){ |
KoenKahlman | 0:9c82986d7cb9 | 872 | result.push_back(0); |
KoenKahlman | 0:9c82986d7cb9 | 873 | } |
KoenKahlman | 0:9c82986d7cb9 | 874 | if((sensor_data & 0b00000010) == 0b00000010){ |
KoenKahlman | 0:9c82986d7cb9 | 875 | result.push_back(1); |
KoenKahlman | 0:9c82986d7cb9 | 876 | } |
KoenKahlman | 0:9c82986d7cb9 | 877 | if((sensor_data & 0b00000100) == 0b00000100){ |
KoenKahlman | 0:9c82986d7cb9 | 878 | result.push_back(2); |
KoenKahlman | 0:9c82986d7cb9 | 879 | } |
KoenKahlman | 0:9c82986d7cb9 | 880 | if((sensor_data & 0b00001000) == 0b00001000){ |
KoenKahlman | 0:9c82986d7cb9 | 881 | result.push_back(3); |
KoenKahlman | 0:9c82986d7cb9 | 882 | } |
KoenKahlman | 0:9c82986d7cb9 | 883 | if((sensor_data & 0b00010000) == 0b00010000){ |
KoenKahlman | 0:9c82986d7cb9 | 884 | result.push_back(4); |
KoenKahlman | 0:9c82986d7cb9 | 885 | } |
KoenKahlman | 0:9c82986d7cb9 | 886 | if((sensor_data & 0b00100000) == 0b00100000){ |
KoenKahlman | 0:9c82986d7cb9 | 887 | result.push_back(5); |
KoenKahlman | 0:9c82986d7cb9 | 888 | } |
KoenKahlman | 0:9c82986d7cb9 | 889 | if((sensor_data & 0b01000000) == 0b01000000){ |
KoenKahlman | 0:9c82986d7cb9 | 890 | result.push_back(6); |
KoenKahlman | 0:9c82986d7cb9 | 891 | } |
KoenKahlman | 0:9c82986d7cb9 | 892 | if((sensor_data & 0b10000000) == 0b10000000){ |
KoenKahlman | 0:9c82986d7cb9 | 893 | result.push_back(7); |
KoenKahlman | 0:9c82986d7cb9 | 894 | } |
KoenKahlman | 0:9c82986d7cb9 | 895 | return result; |
KoenKahlman | 0:9c82986d7cb9 | 896 | } |
KoenKahlman | 0:9c82986d7cb9 | 897 | |
KoenKahlman | 0:9c82986d7cb9 | 898 | std::list<int> circuit1markers(int sensor_data){ |
KoenKahlman | 0:9c82986d7cb9 | 899 | std::list<int> result; |
KoenKahlman | 0:9c82986d7cb9 | 900 | sensor_data = ~sensor_data; |
KoenKahlman | 0:9c82986d7cb9 | 901 | if((sensor_data & 0b00000001) == 0b00000001){ |
KoenKahlman | 0:9c82986d7cb9 | 902 | result.push_back(8); |
KoenKahlman | 0:9c82986d7cb9 | 903 | } |
KoenKahlman | 0:9c82986d7cb9 | 904 | if((sensor_data & 0b00000010) == 0b00000010){ |
KoenKahlman | 0:9c82986d7cb9 | 905 | result.push_back(9); |
KoenKahlman | 0:9c82986d7cb9 | 906 | } |
KoenKahlman | 0:9c82986d7cb9 | 907 | if((sensor_data & 0b00000100) == 0b00000100){ |
KoenKahlman | 0:9c82986d7cb9 | 908 | result.push_back(10); |
KoenKahlman | 0:9c82986d7cb9 | 909 | } |
KoenKahlman | 0:9c82986d7cb9 | 910 | if((sensor_data & 0b00001000) == 0b00001000){ |
KoenKahlman | 0:9c82986d7cb9 | 911 | result.push_back(11); |
KoenKahlman | 0:9c82986d7cb9 | 912 | } |
KoenKahlman | 0:9c82986d7cb9 | 913 | if((sensor_data & 0b00010000) == 0b00010000){ |
KoenKahlman | 0:9c82986d7cb9 | 914 | result.push_back(12); |
KoenKahlman | 0:9c82986d7cb9 | 915 | } |
KoenKahlman | 0:9c82986d7cb9 | 916 | if((sensor_data & 0b00100000) == 0b00100000){ |
KoenKahlman | 0:9c82986d7cb9 | 917 | result.push_back(13); |
KoenKahlman | 0:9c82986d7cb9 | 918 | } |
KoenKahlman | 0:9c82986d7cb9 | 919 | if((sensor_data & 0b01000000) == 0b01000000){ |
KoenKahlman | 1:1648ce825c31 | 920 | //result.push_back(21); do nothing; sensor 21 badly positioned |
KoenKahlman | 0:9c82986d7cb9 | 921 | } |
KoenKahlman | 0:9c82986d7cb9 | 922 | if((sensor_data & 0b10000000) == 0b10000000){ |
KoenKahlman | 1:1648ce825c31 | 923 | //result.push_back(22); do nothing; too close |
KoenKahlman | 0:9c82986d7cb9 | 924 | } |
KoenKahlman | 0:9c82986d7cb9 | 925 | return result; |
KoenKahlman | 0:9c82986d7cb9 | 926 | } |
KoenKahlman | 0:9c82986d7cb9 | 927 | |
KoenKahlman | 0:9c82986d7cb9 | 928 | //LOG OUTPUT |
KoenKahlman | 0:9c82986d7cb9 | 929 | void displayLCDstatus(){ |
KoenKahlman | 0:9c82986d7cb9 | 930 | lcd.cls(); |
KoenKahlman | 1:1648ce825c31 | 931 | lcd.printf("Train 1: %d \nTrain 2: %d", speed1, speed2); |
KoenKahlman | 0:9c82986d7cb9 | 932 | } |
KoenKahlman | 0:9c82986d7cb9 | 933 | |
KoenKahlman | 0:9c82986d7cb9 | 934 | void displayTerminalStatus(){ |
KoenKahlman | 0:9c82986d7cb9 | 935 | string dir1Text = dir1?"forward":"reverse"; |
KoenKahlman | 0:9c82986d7cb9 | 936 | string dir2Text = dir2?"forward":"reverse"; |
KoenKahlman | 0:9c82986d7cb9 | 937 | string light1Text = light1?"on, ":"off,"; |
KoenKahlman | 0:9c82986d7cb9 | 938 | string light2Text = light2?"on, ":"off,"; |
KoenKahlman | 1:1648ce825c31 | 939 | pc.printf("SYSTEM: Iteration %d\n\r", iter); |
KoenKahlman | 1:1648ce825c31 | 940 | pc.printf("Train 1: dir %s, light %s spd %d\n\rTrain 2: dir %s, light %s spd %d\n\r", dir1Text.c_str(), light1Text.c_str(), speed1, dir2Text.c_str(), light2Text.c_str(), speed2); |
KoenKahlman | 0:9c82986d7cb9 | 941 | } |