ESE519 / Mbed 2 deprecated elevatorLab_1

attempt 1

Dependencies:   mbed

main.cpp@4:857d2663c894, 2015-03-27 (annotated)

Committer:
jaredwil
Date:
Fri Mar 27 22:24:19 2015 +0000
Revision:
4:857d2663c894
Parent:
3:f45ab902c0ad
Child:
5:d5f2ff7ca039
final demoed

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 //elevator I/O
jaredwil 0:fda0081e634b 5 BusOut leds(LED1,LED2,LED3,LED4);
jaredwil 4:857d2663c894 6 PwmOut EN(p22);
jaredwil 4:857d2663c894 7 DigitalOut IN1(p30);
jaredwil 4:857d2663c894 8 DigitalOut IN2(p29);
jaredwil 1:54ae0e3ae1b7 9
jaredwil 1:54ae0e3ae1b7 10
jaredwil 1:54ae0e3ae1b7 11 //p23 LEFT p24 RIGHT
jaredwil 1:54ae0e3ae1b7 12 PwmOut leftDoor(p21);
jaredwil 4:857d2663c894 13 //PwmOut rightDoor(p22);
jaredwil 1:54ae0e3ae1b7 14
jaredwil 1:54ae0e3ae1b7 15 InterruptIn irIN(p16);
jaredwil 0:fda0081e634b 16
jaredwil 0:fda0081e634b 17 Serial pc(USBTX, USBRX);
jaredwil 0:fda0081e634b 18
jaredwil 0:fda0081e634b 19 //itizilize digital I/O for keypad
jaredwil 1:54ae0e3ae1b7 20 DigitalOut row1(p27);
jaredwil 1:54ae0e3ae1b7 21 DigitalOut row2(p28);
jaredwil 0:fda0081e634b 22 DigitalIn col1(p20);
jaredwil 0:fda0081e634b 23 DigitalIn col2(p19);
jaredwil 0:fda0081e634b 24 DigitalIn col3(p18);
jaredwil 0:fda0081e634b 25
jaredwil 0:fda0081e634b 26 //timer for row switch
jaredwil 0:fda0081e634b 27 Timer t1;
jaredwil 0:fda0081e634b 28 Timer irT;
jaredwil 0:fda0081e634b 29 //init keypad map
jaredwil 0:fda0081e634b 30 map<int, int> keypadMap;
jaredwil 0:fda0081e634b 31
jaredwil 3:f45ab902c0ad 32 int curFloor; //initialize at 2
jaredwil 1:54ae0e3ae1b7 33 int floorDesired = 1; //init floor buffer
jaredwil 3:f45ab902c0ad 34 bool floors[5] = {false, false, false,false,false};
jaredwil 1:54ae0e3ae1b7 35
jaredwil 3:f45ab902c0ad 36 int open = 0;
jaredwil 1:54ae0e3ae1b7 37 int count = 0;
jaredwil 3:f45ab902c0ad 38 int nextFloor = 1;
jaredwil 1:54ae0e3ae1b7 39
jaredwil 1:54ae0e3ae1b7 40 //direction 1:up; 2: down;
jaredwil 1:54ae0e3ae1b7 41 int direction = 0;
jaredwil 1:54ae0e3ae1b7 42
jaredwil 1:54ae0e3ae1b7 43 float first = 10000.0;
jaredwil 1:54ae0e3ae1b7 44 float second = 5000.0;
jaredwil 1:54ae0e3ae1b7 45 float third = 3333.0;
jaredwil 1:54ae0e3ae1b7 46 float fourth = 2500.0;
jaredwil 1:54ae0e3ae1b7 47 float fifth = 2000.0;
jaredwil 1:54ae0e3ae1b7 48
jaredwil 3:f45ab902c0ad 49 float tol = 150.0;
jaredwil 1:54ae0e3ae1b7 50
jaredwil 0:fda0081e634b 51 void init(){
jaredwil 1:54ae0e3ae1b7 52 /*
jaredwil 0:fda0081e634b 53 keypadMap[0x11]= 0x1;
jaredwil 0:fda0081e634b 54 keypadMap[0x12]= 0x2;
jaredwil 0:fda0081e634b 55 keypadMap[0x14]= 0x4;
jaredwil 0:fda0081e634b 56 keypadMap[0x18]= 0x0;
jaredwil 0:fda0081e634b 57 keypadMap[0x21]= 0x8;
jaredwil 0:fda0081e634b 58 keypadMap[0x22]= 0x0;
jaredwil 0:fda0081e634b 59 keypadMap[0x24]= 0x0;
jaredwil 0:fda0081e634b 60 keypadMap[0x28]= 0x0;
jaredwil 1:54ae0e3ae1b7 61 */
jaredwil 1:54ae0e3ae1b7 62
jaredwil 1:54ae0e3ae1b7 63 keypadMap[0x11]= 1;
jaredwil 1:54ae0e3ae1b7 64 keypadMap[0x12]= 2;
jaredwil 1:54ae0e3ae1b7 65 keypadMap[0x14]= 3;
jaredwil 1:54ae0e3ae1b7 66 keypadMap[0x21]= 4;
jaredwil 3:f45ab902c0ad 67 keypadMap[0x24]= 5;
jaredwil 3:f45ab902c0ad 68 //keypadMap[0x24]= 0;
jaredwil 0:fda0081e634b 69 }
jaredwil 0:fda0081e634b 70
jaredwil 1:54ae0e3ae1b7 71 void openDoors(){
jaredwil 1:54ae0e3ae1b7 72
jaredwil 1:54ae0e3ae1b7 73 leftDoor= 0.075; //init door closed
jaredwil 4:857d2663c894 74 //rightDoor= 0.125; //itit door closed]
jaredwil 1:54ae0e3ae1b7 75
jaredwil 1:54ae0e3ae1b7 76 }
jaredwil 1:54ae0e3ae1b7 77 void closeDoors(){
jaredwil 1:54ae0e3ae1b7 78
jaredwil 1:54ae0e3ae1b7 79 leftDoor= 0.125; //init door closed
jaredwil 4:857d2663c894 80 //rightDoor= 0.075; //itit door closed
jaredwil 1:54ae0e3ae1b7 81 }
jaredwil 1:54ae0e3ae1b7 82
jaredwil 1:54ae0e3ae1b7 83 void up(){
jaredwil 3:f45ab902c0ad 84 EN = 0.6;
jaredwil 1:54ae0e3ae1b7 85 IN1 = 1;
jaredwil 1:54ae0e3ae1b7 86 IN2 = 0.0;
jaredwil 1:54ae0e3ae1b7 87 }
jaredwil 1:54ae0e3ae1b7 88 void down(){
jaredwil 3:f45ab902c0ad 89 EN = 0.6;
jaredwil 1:54ae0e3ae1b7 90 IN1 = 0.0;
jaredwil 1:54ae0e3ae1b7 91 IN2 = 1;
jaredwil 1:54ae0e3ae1b7 92 }
jaredwil 1:54ae0e3ae1b7 93
jaredwil 1:54ae0e3ae1b7 94 void stop(){
jaredwil 1:54ae0e3ae1b7 95 EN = 0.0;
jaredwil 1:54ae0e3ae1b7 96 IN1 = 0.0;
jaredwil 1:54ae0e3ae1b7 97 IN2 = 0.0;
jaredwil 1:54ae0e3ae1b7 98 }
jaredwil 1:54ae0e3ae1b7 99
jaredwil 1:54ae0e3ae1b7 100 int getFloor(float period) {
jaredwil 1:54ae0e3ae1b7 101 if (first - tol < period && first + tol >= period)
jaredwil 1:54ae0e3ae1b7 102 return 1;
jaredwil 1:54ae0e3ae1b7 103 else if (second - tol < period && second + tol >= period)
jaredwil 1:54ae0e3ae1b7 104 return 2;
jaredwil 1:54ae0e3ae1b7 105 else if (third - tol < period && third + tol >= period)
jaredwil 1:54ae0e3ae1b7 106 return 3;
jaredwil 1:54ae0e3ae1b7 107 else if (fourth - tol < period && fourth + tol >= period)
jaredwil 1:54ae0e3ae1b7 108 return 4;
jaredwil 1:54ae0e3ae1b7 109 else if (fifth - tol < period && fifth + tol >= period)
jaredwil 1:54ae0e3ae1b7 110 return 5;
jaredwil 1:54ae0e3ae1b7 111 else return 0;
jaredwil 1:54ae0e3ae1b7 112 }
jaredwil 1:54ae0e3ae1b7 113
jaredwil 1:54ae0e3ae1b7 114 void irRise() {
jaredwil 1:54ae0e3ae1b7 115 irT.start();
jaredwil 1:54ae0e3ae1b7 116 }
jaredwil 1:54ae0e3ae1b7 117
jaredwil 4:857d2663c894 118 int aggr;
jaredwil 4:857d2663c894 119 int temp;
jaredwil 4:857d2663c894 120 int prev;
jaredwil 4:857d2663c894 121
jaredwil 1:54ae0e3ae1b7 122 void irFall() {
jaredwil 1:54ae0e3ae1b7 123 irT.stop();
jaredwil 1:54ae0e3ae1b7 124 float irPeriod = 2 * irT.read_us();
jaredwil 1:54ae0e3ae1b7 125
jaredwil 1:54ae0e3ae1b7 126 temp = getFloor(irPeriod);
jaredwil 4:857d2663c894 127 if(temp != 0){
jaredwil 4:857d2663c894 128 if(temp == prev)
jaredwil 4:857d2663c894 129 aggr++;
jaredwil 4:857d2663c894 130 else aggr = 0;
jaredwil 4:857d2663c894 131 if(aggr == 15){
jaredwil 4:857d2663c894 132 curFloor = temp;
jaredwil 4:857d2663c894 133 aggr = 0;
jaredwil 4:857d2663c894 134 }
jaredwil 4:857d2663c894 135 prev = temp;
jaredwil 4:857d2663c894 136 }
jaredwil 1:54ae0e3ae1b7 137 irT.reset();
jaredwil 1:54ae0e3ae1b7 138 }
jaredwil 1:54ae0e3ae1b7 139
jaredwil 1:54ae0e3ae1b7 140 int getNextFloor() {
jaredwil 1:54ae0e3ae1b7 141 //up
jaredwil 1:54ae0e3ae1b7 142 if (direction == 1){
jaredwil 3:f45ab902c0ad 143 for (int i = curFloor; i< 5; ++i){
jaredwil 3:f45ab902c0ad 144 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 145 return i+1;
jaredwil 1:54ae0e3ae1b7 146 }
jaredwil 1:54ae0e3ae1b7 147 for (int i = curFloor - 2; i >=0 ; --i){
jaredwil 3:f45ab902c0ad 148 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 149 return i+1;
jaredwil 1:54ae0e3ae1b7 150 }
jaredwil 3:f45ab902c0ad 151 return curFloor;
jaredwil 1:54ae0e3ae1b7 152 }
jaredwil 1:54ae0e3ae1b7 153 //down
jaredwil 3:f45ab902c0ad 154 else if (direction == 2){
jaredwil 1:54ae0e3ae1b7 155 for (int i = curFloor - 2; i >=0 ; --i){
jaredwil 3:f45ab902c0ad 156
jaredwil 3:f45ab902c0ad 157 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 158 return i+1;
jaredwil 1:54ae0e3ae1b7 159 }
jaredwil 3:f45ab902c0ad 160 for (int i = curFloor; i< 5; ++i){
jaredwil 3:f45ab902c0ad 161 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 162 return i+1;
jaredwil 1:54ae0e3ae1b7 163 }
jaredwil 3:f45ab902c0ad 164 return curFloor;
jaredwil 1:54ae0e3ae1b7 165 }
jaredwil 1:54ae0e3ae1b7 166 else return floorDesired;
jaredwil 3:f45ab902c0ad 167 //else return;
jaredwil 3:f45ab902c0ad 168
jaredwil 1:54ae0e3ae1b7 169 }
jaredwil 1:54ae0e3ae1b7 170
jaredwil 0:fda0081e634b 171 int main() {
jaredwil 0:fda0081e634b 172
jaredwil 0:fda0081e634b 173 init();
jaredwil 0:fda0081e634b 174 //Initialize rows to 0 and keypad stuff
jaredwil 0:fda0081e634b 175 row1 = 0;
jaredwil 0:fda0081e634b 176 row2 = 0;
jaredwil 0:fda0081e634b 177 t1.start();
jaredwil 0:fda0081e634b 178 int cur_input = 0x00;
jaredwil 0:fda0081e634b 179 int prev_input = 0x00;
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
jaredwil 4:857d2663c894 191 //rightDoor.period(1/freq);
jaredwil 4:857d2663c894 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 //pc.printf("Current Floor = %d, Next Floor = %d\r",curFloor,nextFloor);
jaredwil 3:f45ab902c0ad 272 reach = 1;
jaredwil 3:f45ab902c0ad 273 pc.printf("Current Floor = %d, Next Floor = %d, Direction = %d, Last floor input = %d\r",curFloor,nextFloor, direction, floorDesired);
jaredwil 3:f45ab902c0ad 274 }
jaredwil 3:f45ab902c0ad 275 reach = 1;
jaredwil 1:54ae0e3ae1b7 276 }
jaredwil 3:f45ab902c0ad 277 if(curFloor < nextFloor){ //if the cur floor is below the nextfloor move up
jaredwil 3:f45ab902c0ad 278 reach = 0;
jaredwil 3:f45ab902c0ad 279 closeDoors();
jaredwil 1:54ae0e3ae1b7 280 up();
jaredwil 1:54ae0e3ae1b7 281 direction = 1;
jaredwil 1:54ae0e3ae1b7 282 }
jaredwil 3:f45ab902c0ad 283 if(curFloor > nextFloor){ //if the cur floor is above the nextfloor move down
jaredwil 3:f45ab902c0ad 284 if(open == 1)
jaredwil 3:f45ab902c0ad 285 {
jaredwil 3:f45ab902c0ad 286 closeDoors();
jaredwil 3:f45ab902c0ad 287 open = 0;
jaredwil 3:f45ab902c0ad 288 }
jaredwil 3:f45ab902c0ad 289 reach = 0;
jaredwil 1:54ae0e3ae1b7 290 down();
jaredwil 1:54ae0e3ae1b7 291 direction = 2;
jaredwil 1:54ae0e3ae1b7 292 }
jaredwil 2:dcd904176b27 293
jaredwil 3:f45ab902c0ad 294 }
jaredwil 0:fda0081e634b 295 }