const int trig = 2; const int echo = 7; int currentFloor = 1; int destinationFloor = 1; void setup() { // initialize digital pin 13 as an output. pinMode(12, OUTPUT); // Direction pinMode(3, OUTPUT); // PWM pinMode(9, OUTPUT); // Brake pinMode(4,INPUT_PULLUP); pinMode(5,INPUT_PULLUP); pinMode(6,INPUT_PULLUP); Serial.begin(57600); while (!Serial) { } } void loop() { long duration, cm; int firstFloor_Button = digitalRead(6); int secondFloor_Button = digitalRead(5); int thirdFloor_Button = digitalRead(4); if(firstFloor_Button ==0) // check the control panel buttons destinationFloor =1; else if (secondFloor_Button == 0) destinationFloor = 2; else if (thirdFloor_Button ==0) destinationFloor = 3; if ( destinationFloor > currentFloor){ digitalWrite(9, LOW); // lose Brake digitalWrite(12, HIGH); // set direction analogWrite(3,180); // set speed } else if ( destinationFloor < currentFloor){ digitalWrite(9, LOW); // lose Brake digitalWrite(12, LOW); // set direction analogWrite(3,180); // set speed } else if ( destinationFloor == currentFloor){ Serial.print("We Arrived"); Serial.println(); digitalWrite(9, HIGH); // Brake analogWrite(3,0); // set speed } pinMode(trig, OUTPUT); // check the distance bettween elevetor and ceiling digitalWrite(trig, LOW); delayMicroseconds(2); digitalWrite(trig, HIGH); delayMicroseconds(5); digitalWrite(trig, LOW); pinMode(echo, INPUT); duration = pulseIn(echo, HIGH); cm = duration / 29 / 2; // convert time to distance in normal humans units Serial.print("current Floor "); Serial.print(currentFloor); Serial.print(" destinationFloor "); Serial.print( destinationFloor); Serial.println(); Serial.print(cm); Serial.print("cm"); Serial.println(); if ( cm < 10 ) currentFloor = 3; else if (25 > cm && cm > 20 ) currentFloor = 2; else if (cm >= 35 ) currentFloor = 1; }