ESE519 / Mbed 2 deprecated elevatorLab_1

attempt 1

Dependencies:   mbed

main.cpp@3:f45ab902c0ad, 2015-03-27 (annotated)

Committer:
jaredwil
Date:
Fri Mar 27 19:33:46 2015 +0000
Revision:
3:f45ab902c0ad
Parent:
2:dcd904176b27
Child:
4:857d2663c894
ff

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 1:54ae0e3ae1b7 6 PwmOut EN(p23);
jaredwil 1:54ae0e3ae1b7 7 DigitalOut IN1(p24);
jaredwil 1:54ae0e3ae1b7 8 DigitalOut IN2(p25);
jaredwil 1:54ae0e3ae1b7 9
jaredwil 1:54ae0e3ae1b7 10
jaredwil 1:54ae0e3ae1b7 11 //p23 LEFT p24 RIGHT
jaredwil 1:54ae0e3ae1b7 12 PwmOut leftDoor(p21);
jaredwil 1:54ae0e3ae1b7 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 1:54ae0e3ae1b7 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 1:54ae0e3ae1b7 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 1:54ae0e3ae1b7 118 int temp;
jaredwil 1:54ae0e3ae1b7 119 void irFall() {
jaredwil 1:54ae0e3ae1b7 120 irT.stop();
jaredwil 1:54ae0e3ae1b7 121 float irPeriod = 2 * irT.read_us();
jaredwil 1:54ae0e3ae1b7 122
jaredwil 1:54ae0e3ae1b7 123 temp = getFloor(irPeriod);
jaredwil 1:54ae0e3ae1b7 124 if(temp != 0)
jaredwil 1:54ae0e3ae1b7 125 curFloor = temp;
jaredwil 1:54ae0e3ae1b7 126 irT.reset();
jaredwil 1:54ae0e3ae1b7 127 }
jaredwil 1:54ae0e3ae1b7 128
jaredwil 1:54ae0e3ae1b7 129 int getNextFloor() {
jaredwil 1:54ae0e3ae1b7 130 //up
jaredwil 1:54ae0e3ae1b7 131 if (direction == 1){
jaredwil 3:f45ab902c0ad 132 for (int i = curFloor; i< 5; ++i){
jaredwil 3:f45ab902c0ad 133 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 134 return i+1;
jaredwil 1:54ae0e3ae1b7 135 }
jaredwil 1:54ae0e3ae1b7 136 for (int i = curFloor - 2; i >=0 ; --i){
jaredwil 3:f45ab902c0ad 137 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 138 return i+1;
jaredwil 1:54ae0e3ae1b7 139 }
jaredwil 3:f45ab902c0ad 140 return curFloor;
jaredwil 1:54ae0e3ae1b7 141 }
jaredwil 1:54ae0e3ae1b7 142 //down
jaredwil 3:f45ab902c0ad 143 else if (direction == 2){
jaredwil 1:54ae0e3ae1b7 144 for (int i = curFloor - 2; i >=0 ; --i){
jaredwil 3:f45ab902c0ad 145
jaredwil 3:f45ab902c0ad 146 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 147 return i+1;
jaredwil 1:54ae0e3ae1b7 148 }
jaredwil 3:f45ab902c0ad 149 for (int i = curFloor; i< 5; ++i){
jaredwil 3:f45ab902c0ad 150 if(floors[i]==true)
jaredwil 1:54ae0e3ae1b7 151 return i+1;
jaredwil 1:54ae0e3ae1b7 152 }
jaredwil 3:f45ab902c0ad 153 return curFloor;
jaredwil 1:54ae0e3ae1b7 154 }
jaredwil 1:54ae0e3ae1b7 155 else return floorDesired;
jaredwil 3:f45ab902c0ad 156 //else return;
jaredwil 3:f45ab902c0ad 157
jaredwil 1:54ae0e3ae1b7 158 }
jaredwil 1:54ae0e3ae1b7 159
jaredwil 0:fda0081e634b 160 int main() {
jaredwil 0:fda0081e634b 161
jaredwil 0:fda0081e634b 162 init();
jaredwil 0:fda0081e634b 163 //Initialize rows to 0 and keypad stuff
jaredwil 0:fda0081e634b 164 row1 = 0;
jaredwil 0:fda0081e634b 165 row2 = 0;
jaredwil 0:fda0081e634b 166 t1.start();
jaredwil 0:fda0081e634b 167 int cur_input = 0x00;
jaredwil 0:fda0081e634b 168 int prev_input = 0x00;
jaredwil 1:54ae0e3ae1b7 169 //float irPeriod = 0;
jaredwil 1:54ae0e3ae1b7 170 irIN.rise(&irRise);
jaredwil 1:54ae0e3ae1b7 171 irIN.fall(&irFall);
jaredwil 0:fda0081e634b 172
jaredwil 0:fda0081e634b 173 //initialize servo and dc motor
jaredwil 1:54ae0e3ae1b7 174 float freq = 50; //set 50 HZ freq for PWM and motor
jaredwil 1:54ae0e3ae1b7 175 EN.period(1/freq);
jaredwil 1:54ae0e3ae1b7 176
jaredwil 1:54ae0e3ae1b7 177
jaredwil 1:54ae0e3ae1b7 178 leftDoor.period(1/freq);
jaredwil 1:54ae0e3ae1b7 179 leftDoor= 0.125; //init door closed
jaredwil 1:54ae0e3ae1b7 180 rightDoor.period(1/freq);
jaredwil 1:54ae0e3ae1b7 181 rightDoor= 0.075; //itit door closed
jaredwil 0:fda0081e634b 182
jaredwil 3:f45ab902c0ad 183 int reach = 0;
jaredwil 3:f45ab902c0ad 184
jaredwil 3:f45ab902c0ad 185 //go down until 1st floor detected as starting point
jaredwil 3:f45ab902c0ad 186 pc.printf("Starting elevator \r");
jaredwil 3:f45ab902c0ad 187
jaredwil 3:f45ab902c0ad 188 while(curFloor != 1)
jaredwil 3:f45ab902c0ad 189 {
jaredwil 3:f45ab902c0ad 190 down();
jaredwil 3:f45ab902c0ad 191 }
jaredwil 3:f45ab902c0ad 192
jaredwil 3:f45ab902c0ad 193 stop();
jaredwil 3:f45ab902c0ad 194
jaredwil 3:f45ab902c0ad 195
jaredwil 3:f45ab902c0ad 196
jaredwil 0:fda0081e634b 197 while(1) {
jaredwil 3:f45ab902c0ad 198
jaredwil 0:fda0081e634b 199
jaredwil 0:fda0081e634b 200 /////////////Below contains all code in order to detect current input from keypad////////////////////////////
jaredwil 0:fda0081e634b 201 if(prev_input == 0){
jaredwil 0:fda0081e634b 202
jaredwil 0:fda0081e634b 203 row1 = 0;
jaredwil 0:fda0081e634b 204 row2 = 0;
jaredwil 0:fda0081e634b 205 //Keep each pin on for 4ms
jaredwil 0:fda0081e634b 206 //"turn on" appropriat bit in map when on.
jaredwil 0:fda0081e634b 207 switch(t1.read_ms()%8){
jaredwil 0:fda0081e634b 208 case 0:
jaredwil 0:fda0081e634b 209 row1 = 1;
jaredwil 0:fda0081e634b 210 cur_input |= 0x10;
jaredwil 0:fda0081e634b 211 break;
jaredwil 0:fda0081e634b 212 case 4:
jaredwil 0:fda0081e634b 213 row2 = 1;
jaredwil 0:fda0081e634b 214 cur_input |= 0x20;
jaredwil 0:fda0081e634b 215 break;
jaredwil 0:fda0081e634b 216 }
jaredwil 0:fda0081e634b 217 }
jaredwil 0:fda0081e634b 218
jaredwil 0:fda0081e634b 219 //Check each colum to see if it is high
jaredwil 0:fda0081e634b 220 //if it is "turn on" that respective bit
jaredwil 0:fda0081e634b 221 if(col1 == 1)
jaredwil 0:fda0081e634b 222 cur_input |= 0x01;
jaredwil 0:fda0081e634b 223 else if(col2 == 1)
jaredwil 0:fda0081e634b 224 cur_input |= 0x02;
jaredwil 0:fda0081e634b 225 else if(col3 == 1)
jaredwil 0:fda0081e634b 226 cur_input |= 0x04;
jaredwil 0:fda0081e634b 227 else cur_input = 0; //Detect button release or no input set input to 0
jaredwil 1:54ae0e3ae1b7 228
jaredwil 3:f45ab902c0ad 229 if(cur_input == 0x22)
jaredwil 3:f45ab902c0ad 230 leds = 0x1;
jaredwil 3:f45ab902c0ad 231
jaredwil 1:54ae0e3ae1b7 232 //leds = keypadMap[cur_input]; //KEYPAD DEBUG
jaredwil 1:54ae0e3ae1b7 233 if(cur_input != prev_input && cur_input != 0){
jaredwil 1:54ae0e3ae1b7 234 floorDesired = keypadMap[cur_input];
jaredwil 1:54ae0e3ae1b7 235 floors[floorDesired - 1] = true;
jaredwil 1:54ae0e3ae1b7 236 nextFloor = getNextFloor();
jaredwil 3:f45ab902c0ad 237 pc.printf("Current Floor = %d, Next Floor = %d, Direction = %d, Last floor input = %d\r",curFloor,nextFloor, direction, floorDesired);
jaredwil 3:f45ab902c0ad 238
jaredwil 1:54ae0e3ae1b7 239 }
jaredwil 0:fda0081e634b 240 //Maintain the past input
jaredwil 0:fda0081e634b 241 prev_input = cur_input;
jaredwil 3:f45ab902c0ad 242 //fix this error i don't know why it exsits
jaredwil 3:f45ab902c0ad 243
jaredwil 1:54ae0e3ae1b7 244
jaredwil 1:54ae0e3ae1b7 245 ////////////////////////MOVE FLOORS//////////////////////////////////////////////////
jaredwil 1:54ae0e3ae1b7 246
jaredwil 3:f45ab902c0ad 247
jaredwil 3:f45ab902c0ad 248 if(curFloor == nextFloor){ //if correct stop moving and get the next floor
jaredwil 1:54ae0e3ae1b7 249 stop();
jaredwil 3:f45ab902c0ad 250 if(open == 0)
jaredwil 3:f45ab902c0ad 251 {
jaredwil 3:f45ab902c0ad 252 openDoors();
jaredwil 3:f45ab902c0ad 253 open = 1;
jaredwil 3:f45ab902c0ad 254 }
jaredwil 1:54ae0e3ae1b7 255 floors[nextFloor-1] = false;
jaredwil 3:f45ab902c0ad 256 if(reach == 0 ){
jaredwil 3:f45ab902c0ad 257
jaredwil 3:f45ab902c0ad 258 nextFloor = getNextFloor();
jaredwil 3:f45ab902c0ad 259 wait(1);
jaredwil 3:f45ab902c0ad 260 //pc.printf("Current Floor = %d, Next Floor = %d\r",curFloor,nextFloor);
jaredwil 3:f45ab902c0ad 261 reach = 1;
jaredwil 3:f45ab902c0ad 262 pc.printf("Current Floor = %d, Next Floor = %d, Direction = %d, Last floor input = %d\r",curFloor,nextFloor, direction, floorDesired);
jaredwil 3:f45ab902c0ad 263 }
jaredwil 3:f45ab902c0ad 264 reach = 1;
jaredwil 1:54ae0e3ae1b7 265 }
jaredwil 3:f45ab902c0ad 266 if(curFloor < nextFloor){ //if the cur floor is below the nextfloor move up
jaredwil 3:f45ab902c0ad 267 reach = 0;
jaredwil 3:f45ab902c0ad 268 closeDoors();
jaredwil 1:54ae0e3ae1b7 269 up();
jaredwil 1:54ae0e3ae1b7 270 direction = 1;
jaredwil 1:54ae0e3ae1b7 271 }
jaredwil 3:f45ab902c0ad 272 if(curFloor > nextFloor){ //if the cur floor is above the nextfloor move down
jaredwil 3:f45ab902c0ad 273 if(open == 1)
jaredwil 3:f45ab902c0ad 274 {
jaredwil 3:f45ab902c0ad 275 closeDoors();
jaredwil 3:f45ab902c0ad 276 open = 0;
jaredwil 3:f45ab902c0ad 277 }
jaredwil 3:f45ab902c0ad 278 reach = 0;
jaredwil 1:54ae0e3ae1b7 279 down();
jaredwil 1:54ae0e3ae1b7 280 direction = 2;
jaredwil 1:54ae0e3ae1b7 281 }
jaredwil 2:dcd904176b27 282
jaredwil 3:f45ab902c0ad 283 }
jaredwil 0:fda0081e634b 284 }