ESE519
attempt 1
Dependencies: mbed
Diff: main.cpp
- Revision:
- 1:54ae0e3ae1b7
- Parent:
- 0:fda0081e634b
- Child:
- 2:dcd904176b27
--- 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//////////////////////////////////////////////////////////