Hemanth Kumar Lekkala
final
Diff: main.cpp
- Revision:
- 0:9de45e2c32dc
- Child:
- 1:d4b4c89e4a43
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Mar 31 03:43:30 2015 +0000
@@ -0,0 +1,390 @@
+// Test
+
+#include "mbed.h"
+#include "Motor.h"
+#include <vector>
+
+PwmOut servo1(p21); // Left motor
+PwmOut servo2(p22); // Right motor
+
+AnalogIn a_pin(p20);
+
+using namespace std;
+Timer period_t;
+
+InterruptIn pin(p24);
+
+Motor m(p23, p6, p7);
+
+Serial pc(USBTX, USBRX);
+vector<int> request;
+// Scanning Keypad
+DigitalOut row2(p11);
+//DigitalOut row3(p12);
+//DigitalOut row4(p14);
+DigitalOut row1(p18);
+
+InterruptIn column1(p13);
+InterruptIn column2(p15);
+InterruptIn column3(p16);
+//DigitalIn column4(p17);
+Ticker t_scan;
+
+Ticker rowsTick;
+
+int flag=0;
+float freq,period;
+int count = 0;
+int i=0;
+int numfloor,diff; // Floor number from keypad (input)
+int curfloor;
+
+int move; // Up = 1 , down = 0
+
+void sort(){
+ int num;
+ int n = request.size();
+ for(int l=0; l<n; l++){
+ for(int m=0; m < n-l-1; m++){
+ if(request[m] > request[m+1]){
+ num = request[m];
+ request[m] = request[m+1];
+ request[m+1] = num;
+ }
+ }
+ }
+ //pc.printf("Queue : ");
+ /*
+ for(int m = 0; m<n; m++){
+ pc.printf("%d ",request[m]);
+ } */
+ //pc.printf("\r\n");
+ }
+
+void door_oc(){
+ pc.printf("On floor : %d \r\n",numfloor);
+ pc.printf("Door Opening\r\n");
+ servo1.pulsewidth(0.00075);
+ servo2.pulsewidth(0.00075);
+
+ wait(2);
+
+ pc.printf("Door Closing\r\n");
+ servo1.pulsewidth(0.00225);
+ servo2.pulsewidth(0.00225);
+
+ }
+
+
+int exist(int number)
+{
+
+ for(int t=0;t<request.size();t++)
+ {
+ if(number == request[t])
+ {
+ return 1;
+ }
+ }
+ return 0;
+}
+
+// Scanning Keypad
+
+void scan(){
+ flag++;
+ if(flag>2){
+ flag = 1;
+ }
+ switch(flag){
+ case 1 : row2 = 1;row1 = 0; break;
+ case 2 : row1 = 1;row2 = 0; break;
+ default : break;
+ }
+
+ }
+void fall1(){
+ if(flag == 1)
+ {
+ int c1 = exist(1);
+ if( c1 == 0 && request.size()<4)
+ {
+ request.push_back(1);
+ sort();
+ //pc.printf("Buff size %d\r\n",request.size());
+ pc.printf("You typed : 1\r\n");
+ }
+ }
+ else if(flag == 2)
+ {
+ int c4 = exist(4);
+ if( c4 == 0 && request.size()<4)
+ {
+ request.push_back(4);
+ sort();
+ //pc.printf("Buff size %d\r\n",request.size());
+ pc.printf("You typed : 4\r\n");
+ }
+ }
+ }
+void fall2(){
+ if(flag == 1)
+ {
+ int c2 = exist(2);
+ if( c2 == 0 && request.size()<4)
+ {
+ request.push_back(2);
+ sort();
+ //pc.printf("Buff size %d\r\n",request.size());
+ pc.printf("You typed : 2\r\n");
+ }
+ }
+ else if(flag == 2)
+ {
+ int c5 = exist(5);
+ if( c5 == 0 && request.size()<4)
+ {
+ request.push_back(5);
+ sort();
+ //pc.printf("Buff size %d\r\n",request.size());
+ pc.printf("You typed : 5\r\n");
+ }
+ }
+ }
+void fall3(){
+ if(flag == 1)
+ {
+ int c3 = exist(3);
+ if( c3 == 0 && request.size()<4)
+ {
+ request.push_back(3);
+ sort();
+ //pc.printf("Buff size %d\r\n",request.size());
+ pc.printf("You typed : 3\r\n");
+ }
+ }
+ }
+
+
+void direction(){
+ for(int k = 0;k<request.size();k++){
+ if(request[k] == curfloor){
+ numfloor = curfloor;
+ door_oc();
+ request.erase(request.begin()+k);
+ break;
+ }
+ }
+ if(request.size()>0){
+ int max = request[request.size()-1];
+ if(max > curfloor){
+ move = 1;
+ }
+ else if(max < curfloor){
+ move = 0;
+ }
+ }
+ }
+
+
+int floor_detect(){
+
+ if(freq > 90.0 && freq < 110.0){
+ curfloor = 1;
+ pc.printf("Floor : %d \r\n",curfloor);
+ //wait(2);
+ }
+ else if(freq > 190.0 && freq < 210.0){
+ curfloor = 2;
+ pc.printf("Floor : %d \r\n",curfloor);
+ //wait(2);
+ }
+ else if(freq > 290.0 && freq < 310.0){
+ curfloor = 3;
+ pc.printf("Floor : %d \r\n",curfloor);
+ //wait(2);
+ }
+ else if(freq > 390.0 && freq < 410.0){
+ curfloor = 4;
+ pc.printf("Floor : %d \r\n",curfloor);
+ //wait(2);
+ }
+ else if(freq > 490.0 && freq < 510.0){
+ curfloor = 5;
+ pc.printf("Floor : %d \r\n",curfloor);
+ //wait(2);
+ }
+
+
+
+ return curfloor;
+ }
+
+
+
+void rise_edge(){
+
+ if(a_pin.read()>0.95){
+ count ++;
+ if(count == 1){
+ period_t.start();
+ }
+ else if(count == 2){
+ period_t.stop();
+ period = period_t.read();
+ freq = 1.0/period;
+ period_t.reset();
+ count = 0;
+ }
+ }
+ if(freq > 0.0){
+ //pc.printf("Freq: %3.2f\r\n",freq);
+ }
+ }
+
+int get_index(int floor){
+ for(int k = 0;k<request.size();k++){
+ if(request[k]==floor){
+ return k;
+ }
+ }
+ return -1;
+ }
+
+int main() {
+
+ t_scan.attach(&scan,0.004);
+ column1.fall(&fall1);
+ column2.fall(&fall2);
+ column3.fall(&fall3);
+
+ servo1.period(0.020);
+
+ //request.push_back(5);
+ //request.push_back(4);
+ //request.push_back(3);
+ //request.push_back(1);
+
+ /*
+ row1 = 0; //row initialization
+ row2 = 0;
+ */
+ pc.printf("Start freq : %d\r\n", freq);
+ pin.rise(&rise_edge);
+ /*
+ while(!(freq>90.0 && freq <110.0)){
+ m.speed(-1.0);
+ }
+ m.speed(0.0);
+ */
+ pc.printf("Start Now ............................. \r\n");
+ while(1){
+
+ //sort();
+ while(request.size()!=0){
+ sort();
+ curfloor = floor_detect();
+ direction();
+ if(move == 1){
+ for(int k = 0;k<request.size();k++){
+ sort();
+ numfloor = request[k];
+ diff = numfloor - curfloor;
+ while(diff > 0){
+ //pc.printf("DC ON +1 \r\n");
+ m.speed(1.0);
+ //pc.printf("Service for : %d \r\n",numfloor);
+ //pc.printf("Now at : %d \r\n",curfloor);
+ //pc.printf("Going Up \r\n");
+ curfloor = floor_detect();
+ if(exist(curfloor)){
+ numfloor = curfloor;
+ }
+ int f_index = get_index(curfloor);
+ diff = numfloor - curfloor;
+ if(diff == 0 && f_index!= -1){
+ //wait(2);
+ pc.printf("DC OFF \r\n");
+ m.speed(0.0);
+ request.erase(request.begin()+f_index);
+ k--;
+ door_oc();
+ }
+ }
+ }
+
+
+ }
+ else if(move == 0){
+ for(int k = request.size()-1;k >= 0;k--){
+ sort();
+ numfloor = request[k];
+ diff = numfloor - curfloor;
+ while(diff < 0){
+ //pc.printf("DC ON -1 \r\n");
+ m.speed(-1.0);
+ //pc.printf("Service for : %d \r\n",numfloor);
+ //pc.printf("Now at : %d \r\n",curfloor);
+ //pc.printf("Going Down \r\n");
+ curfloor = floor_detect();
+ if(exist(curfloor)){
+ numfloor = curfloor;
+ }
+ int f_index = get_index(curfloor);
+ diff = numfloor - curfloor;
+ if(diff == 0 && f_index!= -1){
+ pc.printf("DC OFF \r\n");
+ //wait(2);
+ m.speed(0.0);
+ request.erase(request.begin()+k);
+ k++;
+ door_oc();
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
+// Move 1
+/*
+ for(int k = 0;k<request.size();k++){
+ sort();
+ numfloor = request[k];
+ diff = numfloor - curfloor;
+ while(diff > 0){
+ //pc.printf("DC ON +1 \r\n");
+ m.speed(1.0);
+ //pc.printf("Service for : %d \r\n",numfloor);
+ //pc.printf("Now at : %d \r\n",curfloor);
+ //pc.printf("Going Up \r\n");
+ curfloor = floor_detect();
+ diff = numfloor - curfloor;
+ if(diff == 0){
+ //wait(2);
+ pc.printf("DC OFF \r\n");
+ m.speed(0.0);
+ request.erase(request.begin()+k);
+ k--;
+ door_oc();
+ }
+ }
+ } */
+
+/*
+
+numfloor = floor_detect();
+ while(numfloor!=request[request.size()-1]){
+ numfloor = floor_detect();
+ diff = numfloor - curfloor;
+ if(diff > 0){
+ m.speed(1.0);
+ if(exist(numfloor)){
+ door_oc();
+
+ }
+
+ }
+
+ } */
\ No newline at end of file