attempt 1
Dependencies: mbed
Diff: main.cpp
- Revision:
- 1:54ae0e3ae1b7
- Parent:
- 0:fda0081e634b
- Child:
- 2:dcd904176b27
diff -r fda0081e634b -r 54ae0e3ae1b7 main.cpp --- a/main.cpp Fri Mar 20 21:00:49 2015 +0000 +++ b/main.cpp Wed Mar 25 00:35:40 2015 +0000 @@ -3,17 +3,23 @@ //elevator I/O BusOut leds(LED1,LED2,LED3,LED4); -PwmOut DC(p21); -PwmOut servo1(p22); -PwmOut servo2(p23); -AnalogIn irIN(p16); +PwmOut EN(p23); +DigitalOut IN1(p24); +DigitalOut IN2(p25); + + +//p23 LEFT p24 RIGHT +PwmOut leftDoor(p21); +PwmOut rightDoor(p22); + +InterruptIn irIN(p16); Serial pc(USBTX, USBRX); //itizilize digital I/O for keypad -DigitalOut row1(p24); -DigitalOut row2(p25); +DigitalOut row1(p27); +DigitalOut row2(p28); DigitalIn col1(p20); DigitalIn col2(p19); DigitalIn col3(p18); @@ -24,7 +30,26 @@ //init keypad map map<int, int> keypadMap; +int curFloor = 1; //initialize at 1 +int floorDesired = 1; //init floor buffer +bool floors[5]; + +int count = 0; +int nextFloor; + +//direction 1:up; 2: down; +int direction = 0; + +float first = 10000.0; +float second = 5000.0; +float third = 3333.0; +float fourth = 2500.0; +float fifth = 2000.0; + +float tol = 250.0; + void init(){ + /* keypadMap[0x11]= 0x1; keypadMap[0x12]= 0x2; keypadMap[0x14]= 0x4; @@ -33,8 +58,99 @@ keypadMap[0x22]= 0x0; keypadMap[0x24]= 0x0; keypadMap[0x28]= 0x0; + */ + + keypadMap[0x11]= 1; + keypadMap[0x12]= 2; + keypadMap[0x14]= 3; + keypadMap[0x21]= 4; + keypadMap[0x22]= 5; } +void openDoors(){ + + leftDoor= 0.075; //init door closed + rightDoor= 0.125; //itit door closed] + + } +void closeDoors(){ + + leftDoor= 0.125; //init door closed + rightDoor= 0.075; //itit door closed + } + +void up(){ + EN = 0.2; + IN1 = 1; + IN2 = 0.0; + } +void down(){ + EN = 0.2; + IN1 = 0.0; + IN2 = 1; + } + +void stop(){ + EN = 0.0; + IN1 = 0.0; + IN2 = 0.0; + } + +int getFloor(float period) { + if (first - tol < period && first + tol >= period) + return 1; + else if (second - tol < period && second + tol >= period) + return 2; + else if (third - tol < period && third + tol >= period) + return 3; + else if (fourth - tol < period && fourth + tol >= period) + return 4; + else if (fifth - tol < period && fifth + tol >= period) + return 5; + else return 0; + } + +void irRise() { + irT.start(); + } + +int temp; +void irFall() { + irT.stop(); + float irPeriod = 2 * irT.read_us(); + + temp = getFloor(irPeriod); + if(temp != 0) + curFloor = temp; + irT.reset(); + } + +int getNextFloor() { + //up + if (direction == 1){ + for (int i = curFloor; i< 4; ++i){ + if(floors[i]) + return i+1; + } + for (int i = curFloor - 2; i >=0 ; --i){ + if(floors[i]) + return i+1; + } + } + //down + if (direction == 2){ + for (int i = curFloor - 2; i >=0 ; --i){ + if(floors[i]) + return i+1; + } + for (int i = curFloor; i< 4; ++i){ + if(floors[i]) + return i+1; + } + } + else return floorDesired; + } + int main() { init(); @@ -44,40 +160,26 @@ t1.start(); int cur_input = 0x00; int prev_input = 0x00; - float irPeriod = 0; + //float irPeriod = 0; + irIN.rise(&irRise); + irIN.fall(&irFall); - int nofloor = 0; //set nofloor state to 0 - + //ELEVATOR STATE + //int nofloor = 0; //set nofloor state to 0 + + int Serv = 0; //initialize servo and dc motor - float freq = 50; - DC.period(1/freq); // set PWM period to user specified for high tone - DC=0.4; // set duty cycle to 50% - servo1.period(1/freq); // set PWM period to user specified for high tone - servo1=0.0375; // set duty cycle to 50% - servo2.period(1/freq); // set PWM period to user specified for high tone - servo2=0.1125; // set duty cycle to 50% + float freq = 50; //set 50 HZ freq for PWM and motor + EN.period(1/freq); + + + leftDoor.period(1/freq); + leftDoor= 0.125; //init door closed + rightDoor.period(1/freq); + rightDoor= 0.075; //itit door closed while(1) { - -/////////////////////////////floor detection////////////////////////////////////// - - -//floor detected -if(irIN > 0.95){ - irT.start(); //start IRfloor timer - nofloor = 0; //set nofloor state to 0 - } -else if(irIN < 0.5 && nofloor == 0){ - irT.stop(); //stop timer - irPeriod = irT.read_us(); //read time - irT.reset(); //reset - nofloor = 1; //no current floor detcted - //make floor decision based on irPeriod - pc.printf("Detected Period: %f val %f\r",irPeriod,irIN.read()); - - } - - + pc.printf("Current Floor = %d, Next Floor = %d\r",curFloor,nextFloor); /////////////Below contains all code in order to detect current input from keypad//////////////////////////// if(prev_input == 0){ @@ -107,10 +209,35 @@ else if(col3 == 1) cur_input |= 0x04; else cur_input = 0; //Detect button release or no input set input to 0 - - leds = keypadMap[cur_input]; + + //leds = keypadMap[cur_input]; //KEYPAD DEBUG + if(cur_input != prev_input && cur_input != 0){ + floorDesired = keypadMap[cur_input]; + floors[floorDesired - 1] = true; + nextFloor = getNextFloor(); + //Serv++; + } //Maintain the past input prev_input = cur_input; + +////////////////////////MOVE FLOORS////////////////////////////////////////////////// + + + if(curFloor == nextFloor){ + stop(); + floors[nextFloor-1] = false; + nextFloor = getNextFloor(); + } + if(curFloor < nexFloor){ + up(); + direction = 1; + } + if(curFloor > nextFloor){ + down(); + direction = 2; + } + + /////////////////////////////////////////END KEYPAD//////////////////////////////////////////////////////////