ESE519 / Mbed 2 deprecated elevatorLab_1

attempt 1

Dependencies:   mbed

main.cpp@5:d5f2ff7ca039, 2015-03-31 (annotated)

Committer:
psahay
Date:
Tue Mar 31 17:42:56 2015 +0000
Revision:
5:d5f2ff7ca039
Parent:
4:857d2663c894 
Final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jaredwil 0:fda0081e634b 1 #include "mbed.h"
jaredwil 0:fda0081e634b 2 #include <map>
jaredwil 0:fda0081e634b 3
jaredwil 0:fda0081e634b 4 BusOut leds(LED1,LED2,LED3,LED4);
psahay 5:d5f2ff7ca039 5
psahay 5:d5f2ff7ca039 6 //Driving motors
jaredwil 4:857d2663c894 7 PwmOut EN(p22);
jaredwil 4:857d2663c894 8 DigitalOut IN1(p30);
jaredwil 4:857d2663c894 9 DigitalOut IN2(p29);
jaredwil 1:54ae0e3ae1b7 10
jaredwil 1:54ae0e3ae1b7 11
jaredwil 1:54ae0e3ae1b7 12 //p23 LEFT p24 RIGHT
psahay 5:d5f2ff7ca039 13 PwmOut leftDoor(p23);
psahay 5:d5f2ff7ca039 14 PwmOut rightDoor(p24);
jaredwil 1:54ae0e3ae1b7 15
jaredwil 1:54ae0e3ae1b7 16 InterruptIn irIN(p16);
jaredwil 0:fda0081e634b 17
jaredwil 0:fda0081e634b 18 Serial pc(USBTX, USBRX);
jaredwil 0:fda0081e634b 19
jaredwil 0:fda0081e634b 20 //itizilize digital I/O for keypad
jaredwil 1:54ae0e3ae1b7 21 DigitalOut row1(p27);
jaredwil 1:54ae0e3ae1b7 22 DigitalOut row2(p28);
jaredwil 0:fda0081e634b 23 DigitalIn col1(p20);
jaredwil 0:fda0081e634b 24 DigitalIn col2(p19);
jaredwil 0:fda0081e634b 25 DigitalIn col3(p18);
jaredwil 0:fda0081e634b 26
jaredwil 0:fda0081e634b 27 //timer for row switch
jaredwil 0:fda0081e634b 28 Timer t1;
psahay 5:d5f2ff7ca039 29
psahay 5:d5f2ff7ca039 30 //Timer for floor detection
jaredwil 0:fda0081e634b 31 Timer irT;
psahay 5:d5f2ff7ca039 32
jaredwil 0:fda0081e634b 33 //init keypad map
jaredwil 0:fda0081e634b 34 map<int, int> keypadMap;
jaredwil 0:fda0081e634b 35
psahay 5:d5f2ff7ca039 36
jaredwil 3:f45ab902c0ad 37 int curFloor; //initialize at 2
jaredwil 1:54ae0e3ae1b7 38 int floorDesired = 1; //init floor buffer
jaredwil 3:f45ab902c0ad 39 bool floors[5] = {false, false, false,false,false};
jaredwil 1:54ae0e3ae1b7 40
jaredwil 3:f45ab902c0ad 41 int open = 0;
jaredwil 1:54ae0e3ae1b7 42 int count = 0;
jaredwil 3:f45ab902c0ad 43 int nextFloor = 1;
jaredwil 1:54ae0e3ae1b7 44
jaredwil 1:54ae0e3ae1b7 45 //direction 1:up; 2: down;
jaredwil 1:54ae0e3ae1b7 46 int direction = 0;
jaredwil 1:54ae0e3ae1b7 47
jaredwil 1:54ae0e3ae1b7 48 float first = 10000.0;
jaredwil 1:54ae0e3ae1b7 49 float second = 5000.0;
jaredwil 1:54ae0e3ae1b7 50 float third = 3333.0;
jaredwil 1:54ae0e3ae1b7 51 float fourth = 2500.0;
jaredwil 1:54ae0e3ae1b7 52 float fifth = 2000.0;
jaredwil 1:54ae0e3ae1b7 53
jaredwil 3:f45ab902c0ad 54 float tol = 150.0;
jaredwil 1:54ae0e3ae1b7 55
psahay 5:d5f2ff7ca039 56 void init(){
jaredwil 1:54ae0e3ae1b7 57 keypadMap[0x11]= 1;
jaredwil 1:54ae0e3ae1b7 58 keypadMap[0x12]= 2;
jaredwil 1:54ae0e3ae1b7 59 keypadMap[0x14]= 3;
jaredwil 1:54ae0e3ae1b7 60 keypadMap[0x21]= 4;
jaredwil 3:f45ab902c0ad 61 keypadMap[0x24]= 5;
jaredwil 0:fda0081e634b 62 }
jaredwil 0:fda0081e634b 63
psahay 5:d5f2ff7ca039 64 //Elevator functions
jaredwil 1:54ae0e3ae1b7 65 void openDoors(){
jaredwil 1:54ae0e3ae1b7 66
jaredwil 1:54ae0e3ae1b7 67 leftDoor= 0.075; //init door closed
psahay 5:d5f2ff7ca039 68 rightDoor= 0.125; //itit door closed]
jaredwil 1:54ae0e3ae1b7 69
jaredwil 1:54ae0e3ae1b7 70 }
jaredwil 1:54ae0e3ae1b7 71 void closeDoors(){
jaredwil 1:54ae0e3ae1b7 72
jaredwil 1:54ae0e3ae1b7 73 leftDoor= 0.125; //init door closed
psahay 5:d5f2ff7ca039 74 rightDoor= 0.075; //itit door closed
jaredwil 1:54ae0e3ae1b7 75 }
jaredwil 1:54ae0e3ae1b7 76
psahay 5:d5f2ff7ca039 77 //Elevator movement
jaredwil 1:54ae0e3ae1b7 78 void up(){
jaredwil 3:f45ab902c0ad 79 EN = 0.6;
jaredwil 1:54ae0e3ae1b7 80 IN1 = 1;
jaredwil 1:54ae0e3ae1b7 81 IN2 = 0.0;
jaredwil 1:54ae0e3ae1b7 82 }
jaredwil 1:54ae0e3ae1b7 83 void down(){
jaredwil 3:f45ab902c0ad 84 EN = 0.6;
jaredwil 1:54ae0e3ae1b7 85 IN1 = 0.0;
jaredwil 1:54ae0e3ae1b7 86 IN2 = 1;
jaredwil 1:54ae0e3ae1b7 87 }
jaredwil 1:54ae0e3ae1b7 88
jaredwil 1:54ae0e3ae1b7 89 void stop(){
jaredwil 1:54ae0e3ae1b7 90 EN = 0.0;
jaredwil 1:54ae0e3ae1b7 91 IN1 = 0.0;
jaredwil 1:54ae0e3ae1b7 92 IN2 = 0.0;
jaredwil 1:54ae0e3ae1b7 93 }
jaredwil 1:54ae0e3ae1b7 94
psahay 5:d5f2ff7ca039 95 //Floor classification based on the time period
jaredwil 1:54ae0e3ae1b7 96 int getFloor(float period) {
jaredwil 1:54ae0e3ae1b7 97 if (first - tol < period && first + tol >= period)
jaredwil 1:54ae0e3ae1b7 98 return 1;
jaredwil 1:54ae0e3ae1b7 99 else if (second - tol < period && second + tol >= period)
jaredwil 1:54ae0e3ae1b7 100 return 2;
jaredwil 1:54ae0e3ae1b7 101 else if (third - tol < period && third + tol >= period)
jaredwil 1:54ae0e3ae1b7 102 return 3;
jaredwil 1:54ae0e3ae1b7 103 else if (fourth - tol < period && fourth + tol >= period)
jaredwil 1:54ae0e3ae1b7 104 return 4;
jaredwil 1:54ae0e3ae1b7 105 else if (fifth - tol < period && fifth + tol >= period)
jaredwil 1:54ae0e3ae1b7 106 return 5;
jaredwil 1:54ae0e3ae1b7 107 else return 0;
jaredwil 1:54ae0e3ae1b7 108 }
jaredwil 1:54ae0e3ae1b7 109
psahay 5:d5f2ff7ca039 110 //Interrupt In of IR wave
jaredwil 1:54ae0e3ae1b7 111 void irRise() {
jaredwil 1:54ae0e3ae1b7 112 irT.start();
jaredwil 1:54ae0e3ae1b7 113 }
jaredwil 1:54ae0e3ae1b7 114
psahay 5:d5f2ff7ca039 115 //Mecahnism to detect floors with aggregation
jaredwil 4:857d2663c894 116 int aggr;
jaredwil 4:857d2663c894 117 int temp;
jaredwil 4:857d2663c894 118 int prev;
jaredwil 4:857d2663c894 119
jaredwil 1:54ae0e3ae1b7 120 void irFall() {
jaredwil 1:54ae0e3ae1b7 121 irT.stop();
jaredwil 1:54ae0e3ae1b7 122 float irPeriod = 2 * irT.read_us();
jaredwil 1:54ae0e3ae1b7 123
jaredwil 1:54ae0e3ae1b7 124 temp = getFloor(irPeriod);
jaredwil 4:857d2663c894 125 if(temp != 0){
jaredwil 4:857d2663c894 126 if(temp == prev)
jaredwil 4:857d2663c894 127 aggr++;
jaredwil 4:857d2663c894 128 else aggr = 0;
jaredwil 4:857d2663c894 129 if(aggr == 15){
jaredwil 4:857d2663c894 130 curFloor = temp;
jaredwil 4:857d2663c894 131 aggr = 0;
jaredwil 4:857d2663c894 132 }
jaredwil 4:857d2663c894 133 prev = temp;
jaredwil 4:857d2663c894 134 }
jaredwil 1:54ae0e3ae1b7 135 irT.reset();
jaredwil 1:54ae0e3ae1b7 136 }
jaredwil 1:54ae0e3ae1b7 137
psahay 5:d5f2ff7ca039 138 //Getting next floor based on Elevator Algorithm
jaredwil 1:54ae0e3ae1b7 139 int getNextFloor() {
jaredwil 1:54ae0e3ae1b7 140 //up
jaredwil 1:54ae0e3ae1b7 141 if (direction == 1){
jaredwil 3:f45ab902c0ad 142 for (int i = curFloor; i< 5; ++i){
jaredwil 3:f45ab902c0ad 143 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 144 return i+1;
jaredwil 1:54ae0e3ae1b7 145 }
jaredwil 1:54ae0e3ae1b7 146 for (int i = curFloor - 2; i >=0 ; --i){
jaredwil 3:f45ab902c0ad 147 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 148 return i+1;
jaredwil 1:54ae0e3ae1b7 149 }
jaredwil 3:f45ab902c0ad 150 return curFloor;
jaredwil 1:54ae0e3ae1b7 151 }
jaredwil 1:54ae0e3ae1b7 152 //down
jaredwil 3:f45ab902c0ad 153 else if (direction == 2){
jaredwil 1:54ae0e3ae1b7 154 for (int i = curFloor - 2; i >=0 ; --i){
jaredwil 3:f45ab902c0ad 155
jaredwil 3:f45ab902c0ad 156 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 157 return i+1;
jaredwil 1:54ae0e3ae1b7 158 }
jaredwil 3:f45ab902c0ad 159 for (int i = curFloor; i< 5; ++i){
jaredwil 3:f45ab902c0ad 160 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 161 return i+1;
jaredwil 1:54ae0e3ae1b7 162 }
jaredwil 3:f45ab902c0ad 163 return curFloor;
jaredwil 1:54ae0e3ae1b7 164 }
jaredwil 1:54ae0e3ae1b7 165 else return floorDesired;
jaredwil 3:f45ab902c0ad 166
jaredwil 1:54ae0e3ae1b7 167 }
jaredwil 1:54ae0e3ae1b7 168
jaredwil 0:fda0081e634b 169 int main() {
jaredwil 0:fda0081e634b 170
jaredwil 0:fda0081e634b 171 init();
psahay 5:d5f2ff7ca039 172
jaredwil 0:fda0081e634b 173 //Initialize rows to 0 and keypad stuff
jaredwil 0:fda0081e634b 174 row1 = 0;
jaredwil 0:fda0081e634b 175 row2 = 0;
jaredwil 0:fda0081e634b 176 t1.start();
jaredwil 0:fda0081e634b 177 int cur_input = 0x00;
jaredwil 0:fda0081e634b 178 int prev_input = 0x00;
psahay 5:d5f2ff7ca039 179
jaredwil 1:54ae0e3ae1b7 180 //float irPeriod = 0;
jaredwil 1:54ae0e3ae1b7 181 irIN.rise(&irRise);
jaredwil 1:54ae0e3ae1b7 182 irIN.fall(&irFall);
jaredwil 0:fda0081e634b 183
jaredwil 0:fda0081e634b 184 //initialize servo and dc motor
jaredwil 1:54ae0e3ae1b7 185 float freq = 50; //set 50 HZ freq for PWM and motor
jaredwil 1:54ae0e3ae1b7 186 EN.period(1/freq);
jaredwil 1:54ae0e3ae1b7 187
jaredwil 1:54ae0e3ae1b7 188
jaredwil 1:54ae0e3ae1b7 189 leftDoor.period(1/freq);
jaredwil 1:54ae0e3ae1b7 190 leftDoor= 0.125; //init door closed
psahay 5:d5f2ff7ca039 191 rightDoor.period(1/freq);
psahay 5:d5f2ff7ca039 192 rightDoor= 0.075; //itit door closed
jaredwil 0:fda0081e634b 193
jaredwil 3:f45ab902c0ad 194 int reach = 0;
jaredwil 3:f45ab902c0ad 195
jaredwil 3:f45ab902c0ad 196 //go down until 1st floor detected as starting point
jaredwil 3:f45ab902c0ad 197 pc.printf("Starting elevator \r");
jaredwil 3:f45ab902c0ad 198
jaredwil 3:f45ab902c0ad 199 while(curFloor != 1)
jaredwil 3:f45ab902c0ad 200 {
jaredwil 3:f45ab902c0ad 201 down();
jaredwil 3:f45ab902c0ad 202 }
jaredwil 3:f45ab902c0ad 203
jaredwil 3:f45ab902c0ad 204 stop();
jaredwil 3:f45ab902c0ad 205
jaredwil 3:f45ab902c0ad 206
jaredwil 3:f45ab902c0ad 207
jaredwil 0:fda0081e634b 208 while(1) {
jaredwil 3:f45ab902c0ad 209
jaredwil 0:fda0081e634b 210
jaredwil 0:fda0081e634b 211 /////////////Below contains all code in order to detect current input from keypad////////////////////////////
jaredwil 0:fda0081e634b 212 if(prev_input == 0){
jaredwil 0:fda0081e634b 213
jaredwil 0:fda0081e634b 214 row1 = 0;
jaredwil 0:fda0081e634b 215 row2 = 0;
jaredwil 0:fda0081e634b 216 //Keep each pin on for 4ms
jaredwil 0:fda0081e634b 217 //"turn on" appropriat bit in map when on.
jaredwil 0:fda0081e634b 218 switch(t1.read_ms()%8){
jaredwil 0:fda0081e634b 219 case 0:
jaredwil 0:fda0081e634b 220 row1 = 1;
jaredwil 0:fda0081e634b 221 cur_input |= 0x10;
jaredwil 0:fda0081e634b 222 break;
jaredwil 0:fda0081e634b 223 case 4:
jaredwil 0:fda0081e634b 224 row2 = 1;
jaredwil 0:fda0081e634b 225 cur_input |= 0x20;
jaredwil 0:fda0081e634b 226 break;
jaredwil 0:fda0081e634b 227 }
jaredwil 0:fda0081e634b 228 }
jaredwil 0:fda0081e634b 229
jaredwil 0:fda0081e634b 230 //Check each colum to see if it is high
jaredwil 0:fda0081e634b 231 //if it is "turn on" that respective bit
jaredwil 0:fda0081e634b 232 if(col1 == 1)
jaredwil 0:fda0081e634b 233 cur_input |= 0x01;
jaredwil 0:fda0081e634b 234 else if(col2 == 1)
jaredwil 0:fda0081e634b 235 cur_input |= 0x02;
jaredwil 0:fda0081e634b 236 else if(col3 == 1)
jaredwil 0:fda0081e634b 237 cur_input |= 0x04;
jaredwil 0:fda0081e634b 238 else cur_input = 0; //Detect button release or no input set input to 0
jaredwil 1:54ae0e3ae1b7 239
jaredwil 3:f45ab902c0ad 240 if(cur_input == 0x22)
jaredwil 3:f45ab902c0ad 241 leds = 0x1;
jaredwil 3:f45ab902c0ad 242
jaredwil 1:54ae0e3ae1b7 243 //leds = keypadMap[cur_input]; //KEYPAD DEBUG
jaredwil 1:54ae0e3ae1b7 244 if(cur_input != prev_input && cur_input != 0){
jaredwil 1:54ae0e3ae1b7 245 floorDesired = keypadMap[cur_input];
jaredwil 1:54ae0e3ae1b7 246 floors[floorDesired - 1] = true;
jaredwil 1:54ae0e3ae1b7 247 nextFloor = getNextFloor();
jaredwil 3:f45ab902c0ad 248 pc.printf("Current Floor = %d, Next Floor = %d, Direction = %d, Last floor input = %d\r",curFloor,nextFloor, direction, floorDesired);
jaredwil 3:f45ab902c0ad 249
jaredwil 1:54ae0e3ae1b7 250 }
jaredwil 0:fda0081e634b 251 //Maintain the past input
jaredwil 0:fda0081e634b 252 prev_input = cur_input;
jaredwil 3:f45ab902c0ad 253 //fix this error i don't know why it exsits
jaredwil 3:f45ab902c0ad 254
jaredwil 1:54ae0e3ae1b7 255
jaredwil 1:54ae0e3ae1b7 256 ////////////////////////MOVE FLOORS//////////////////////////////////////////////////
jaredwil 1:54ae0e3ae1b7 257
jaredwil 3:f45ab902c0ad 258
jaredwil 3:f45ab902c0ad 259 if(curFloor == nextFloor){ //if correct stop moving and get the next floor
jaredwil 1:54ae0e3ae1b7 260 stop();
jaredwil 3:f45ab902c0ad 261 if(open == 0)
jaredwil 3:f45ab902c0ad 262 {
jaredwil 3:f45ab902c0ad 263 openDoors();
jaredwil 3:f45ab902c0ad 264 open = 1;
jaredwil 3:f45ab902c0ad 265 }
jaredwil 1:54ae0e3ae1b7 266 floors[nextFloor-1] = false;
jaredwil 3:f45ab902c0ad 267 if(reach == 0 ){
jaredwil 3:f45ab902c0ad 268
jaredwil 3:f45ab902c0ad 269 nextFloor = getNextFloor();
jaredwil 3:f45ab902c0ad 270 wait(1);
jaredwil 3:f45ab902c0ad 271 reach = 1;
jaredwil 3:f45ab902c0ad 272 pc.printf("Current Floor = %d, Next Floor = %d, Direction = %d, Last floor input = %d\r",curFloor,nextFloor, direction, floorDesired);
jaredwil 3:f45ab902c0ad 273 }
jaredwil 3:f45ab902c0ad 274 reach = 1;
jaredwil 1:54ae0e3ae1b7 275 }
jaredwil 3:f45ab902c0ad 276 if(curFloor < nextFloor){ //if the cur floor is below the nextfloor move up
jaredwil 3:f45ab902c0ad 277 reach = 0;
jaredwil 3:f45ab902c0ad 278 closeDoors();
jaredwil 1:54ae0e3ae1b7 279 up();
jaredwil 1:54ae0e3ae1b7 280 direction = 1;
jaredwil 1:54ae0e3ae1b7 281 }
jaredwil 3:f45ab902c0ad 282 if(curFloor > nextFloor){ //if the cur floor is above the nextfloor move down
jaredwil 3:f45ab902c0ad 283 if(open == 1)
jaredwil 3:f45ab902c0ad 284 {
jaredwil 3:f45ab902c0ad 285 closeDoors();
jaredwil 3:f45ab902c0ad 286 open = 0;
jaredwil 3:f45ab902c0ad 287 }
jaredwil 3:f45ab902c0ad 288 reach = 0;
jaredwil 1:54ae0e3ae1b7 289 down();
jaredwil 1:54ae0e3ae1b7 290 direction = 2;
jaredwil 1:54ae0e3ae1b7 291 }
jaredwil 2:dcd904176b27 292
jaredwil 3:f45ab902c0ad 293 }
jaredwil 0:fda0081e634b 294 }