Hemanth Kumar Lekkala / Mbed 2 deprecated Lab4

final

Dependencies:   Motor mbed

main.cpp@1:d4b4c89e4a43, 2015-04-13 (annotated)

Committer:
hlekkala
Date:
Mon Apr 13 19:52:01 2015 +0000
Revision:
1:d4b4c89e4a43
Parent:
0:9de45e2c32dc 
final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
snehanayak16 0:9de45e2c32dc 1 // Test
snehanayak16 0:9de45e2c32dc 2
snehanayak16 0:9de45e2c32dc 3 #include "mbed.h"
snehanayak16 0:9de45e2c32dc 4 #include "Motor.h"
snehanayak16 0:9de45e2c32dc 5 #include <vector>
snehanayak16 0:9de45e2c32dc 6
snehanayak16 0:9de45e2c32dc 7 PwmOut servo1(p21); // Left motor
snehanayak16 0:9de45e2c32dc 8 PwmOut servo2(p22); // Right motor
snehanayak16 0:9de45e2c32dc 9
snehanayak16 0:9de45e2c32dc 10 AnalogIn a_pin(p20);
snehanayak16 0:9de45e2c32dc 11
snehanayak16 0:9de45e2c32dc 12 using namespace std;
snehanayak16 0:9de45e2c32dc 13 Timer period_t;
snehanayak16 0:9de45e2c32dc 14
snehanayak16 0:9de45e2c32dc 15 InterruptIn pin(p24);
snehanayak16 0:9de45e2c32dc 16
snehanayak16 0:9de45e2c32dc 17 Motor m(p23, p6, p7);
snehanayak16 0:9de45e2c32dc 18
snehanayak16 0:9de45e2c32dc 19 Serial pc(USBTX, USBRX);
snehanayak16 0:9de45e2c32dc 20 vector<int> request;
snehanayak16 0:9de45e2c32dc 21 // Scanning Keypad
snehanayak16 0:9de45e2c32dc 22 DigitalOut row2(p11);
snehanayak16 0:9de45e2c32dc 23 //DigitalOut row3(p12);
snehanayak16 0:9de45e2c32dc 24 //DigitalOut row4(p14);
snehanayak16 0:9de45e2c32dc 25 DigitalOut row1(p18);
snehanayak16 0:9de45e2c32dc 26
snehanayak16 0:9de45e2c32dc 27 InterruptIn column1(p13);
snehanayak16 0:9de45e2c32dc 28 InterruptIn column2(p15);
snehanayak16 0:9de45e2c32dc 29 InterruptIn column3(p16);
snehanayak16 0:9de45e2c32dc 30 //DigitalIn column4(p17);
snehanayak16 0:9de45e2c32dc 31 Ticker t_scan;
snehanayak16 0:9de45e2c32dc 32
snehanayak16 0:9de45e2c32dc 33 Ticker rowsTick;
snehanayak16 0:9de45e2c32dc 34
snehanayak16 0:9de45e2c32dc 35 int flag=0;
snehanayak16 0:9de45e2c32dc 36 float freq,period;
snehanayak16 0:9de45e2c32dc 37 int count = 0;
snehanayak16 0:9de45e2c32dc 38 int i=0;
snehanayak16 0:9de45e2c32dc 39 int numfloor,diff; // Floor number from keypad (input)
snehanayak16 0:9de45e2c32dc 40 int curfloor;
snehanayak16 0:9de45e2c32dc 41
snehanayak16 0:9de45e2c32dc 42 int move; // Up = 1 , down = 0
snehanayak16 0:9de45e2c32dc 43
snehanayak16 0:9de45e2c32dc 44 void sort(){
snehanayak16 0:9de45e2c32dc 45 int num;
snehanayak16 0:9de45e2c32dc 46 int n = request.size();
snehanayak16 0:9de45e2c32dc 47 for(int l=0; l<n; l++){
snehanayak16 0:9de45e2c32dc 48 for(int m=0; m < n-l-1; m++){
snehanayak16 0:9de45e2c32dc 49 if(request[m] > request[m+1]){
snehanayak16 0:9de45e2c32dc 50 num = request[m];
snehanayak16 0:9de45e2c32dc 51 request[m] = request[m+1];
snehanayak16 0:9de45e2c32dc 52 request[m+1] = num;
snehanayak16 0:9de45e2c32dc 53 }
snehanayak16 0:9de45e2c32dc 54 }
snehanayak16 0:9de45e2c32dc 55 }
snehanayak16 0:9de45e2c32dc 56
snehanayak16 0:9de45e2c32dc 57 void door_oc(){
snehanayak16 0:9de45e2c32dc 58 pc.printf("On floor : %d \r\n",numfloor);
snehanayak16 0:9de45e2c32dc 59 pc.printf("Door Opening\r\n");
snehanayak16 0:9de45e2c32dc 60 servo1.pulsewidth(0.00075);
snehanayak16 0:9de45e2c32dc 61 servo2.pulsewidth(0.00075);
snehanayak16 0:9de45e2c32dc 62
snehanayak16 0:9de45e2c32dc 63 wait(2);
snehanayak16 0:9de45e2c32dc 64
snehanayak16 0:9de45e2c32dc 65 pc.printf("Door Closing\r\n");
snehanayak16 0:9de45e2c32dc 66 servo1.pulsewidth(0.00225);
snehanayak16 0:9de45e2c32dc 67 servo2.pulsewidth(0.00225);
snehanayak16 0:9de45e2c32dc 68
snehanayak16 0:9de45e2c32dc 69 }
snehanayak16 0:9de45e2c32dc 70
snehanayak16 0:9de45e2c32dc 71
snehanayak16 0:9de45e2c32dc 72 int exist(int number)
snehanayak16 0:9de45e2c32dc 73 {
snehanayak16 0:9de45e2c32dc 74
snehanayak16 0:9de45e2c32dc 75 for(int t=0;t<request.size();t++)
snehanayak16 0:9de45e2c32dc 76 {
snehanayak16 0:9de45e2c32dc 77 if(number == request[t])
snehanayak16 0:9de45e2c32dc 78 {
snehanayak16 0:9de45e2c32dc 79 return 1;
snehanayak16 0:9de45e2c32dc 80 }
snehanayak16 0:9de45e2c32dc 81 }
snehanayak16 0:9de45e2c32dc 82 return 0;
snehanayak16 0:9de45e2c32dc 83 }
snehanayak16 0:9de45e2c32dc 84
snehanayak16 0:9de45e2c32dc 85 // Scanning Keypad
snehanayak16 0:9de45e2c32dc 86
snehanayak16 0:9de45e2c32dc 87 void scan(){
snehanayak16 0:9de45e2c32dc 88 flag++;
snehanayak16 0:9de45e2c32dc 89 if(flag>2){
snehanayak16 0:9de45e2c32dc 90 flag = 1;
snehanayak16 0:9de45e2c32dc 91 }
snehanayak16 0:9de45e2c32dc 92 switch(flag){
snehanayak16 0:9de45e2c32dc 93 case 1 : row2 = 1;row1 = 0; break;
snehanayak16 0:9de45e2c32dc 94 case 2 : row1 = 1;row2 = 0; break;
snehanayak16 0:9de45e2c32dc 95 default : break;
snehanayak16 0:9de45e2c32dc 96 }
snehanayak16 0:9de45e2c32dc 97
snehanayak16 0:9de45e2c32dc 98 }
snehanayak16 0:9de45e2c32dc 99 void fall1(){
snehanayak16 0:9de45e2c32dc 100 if(flag == 1)
snehanayak16 0:9de45e2c32dc 101 {
snehanayak16 0:9de45e2c32dc 102 int c1 = exist(1);
snehanayak16 0:9de45e2c32dc 103 if( c1 == 0 && request.size()<4)
snehanayak16 0:9de45e2c32dc 104 {
snehanayak16 0:9de45e2c32dc 105 request.push_back(1);
snehanayak16 0:9de45e2c32dc 106 sort();
snehanayak16 0:9de45e2c32dc 107 //pc.printf("Buff size %d\r\n",request.size());
snehanayak16 0:9de45e2c32dc 108 pc.printf("You typed : 1\r\n");
snehanayak16 0:9de45e2c32dc 109 }
snehanayak16 0:9de45e2c32dc 110 }
snehanayak16 0:9de45e2c32dc 111 else if(flag == 2)
snehanayak16 0:9de45e2c32dc 112 {
snehanayak16 0:9de45e2c32dc 113 int c4 = exist(4);
snehanayak16 0:9de45e2c32dc 114 if( c4 == 0 && request.size()<4)
snehanayak16 0:9de45e2c32dc 115 {
snehanayak16 0:9de45e2c32dc 116 request.push_back(4);
snehanayak16 0:9de45e2c32dc 117 sort();
snehanayak16 0:9de45e2c32dc 118 //pc.printf("Buff size %d\r\n",request.size());
snehanayak16 0:9de45e2c32dc 119 pc.printf("You typed : 4\r\n");
snehanayak16 0:9de45e2c32dc 120 }
snehanayak16 0:9de45e2c32dc 121 }
snehanayak16 0:9de45e2c32dc 122 }
snehanayak16 0:9de45e2c32dc 123 void fall2(){
snehanayak16 0:9de45e2c32dc 124 if(flag == 1)
snehanayak16 0:9de45e2c32dc 125 {
snehanayak16 0:9de45e2c32dc 126 int c2 = exist(2);
snehanayak16 0:9de45e2c32dc 127 if( c2 == 0 && request.size()<4)
snehanayak16 0:9de45e2c32dc 128 {
snehanayak16 0:9de45e2c32dc 129 request.push_back(2);
snehanayak16 0:9de45e2c32dc 130 sort();
snehanayak16 0:9de45e2c32dc 131 //pc.printf("Buff size %d\r\n",request.size());
snehanayak16 0:9de45e2c32dc 132 pc.printf("You typed : 2\r\n");
snehanayak16 0:9de45e2c32dc 133 }
snehanayak16 0:9de45e2c32dc 134 }
snehanayak16 0:9de45e2c32dc 135 else if(flag == 2)
snehanayak16 0:9de45e2c32dc 136 {
snehanayak16 0:9de45e2c32dc 137 int c5 = exist(5);
snehanayak16 0:9de45e2c32dc 138 if( c5 == 0 && request.size()<4)
snehanayak16 0:9de45e2c32dc 139 {
snehanayak16 0:9de45e2c32dc 140 request.push_back(5);
snehanayak16 0:9de45e2c32dc 141 sort();
snehanayak16 0:9de45e2c32dc 142 //pc.printf("Buff size %d\r\n",request.size());
snehanayak16 0:9de45e2c32dc 143 pc.printf("You typed : 5\r\n");
snehanayak16 0:9de45e2c32dc 144 }
snehanayak16 0:9de45e2c32dc 145 }
snehanayak16 0:9de45e2c32dc 146 }
snehanayak16 0:9de45e2c32dc 147 void fall3(){
snehanayak16 0:9de45e2c32dc 148 if(flag == 1)
snehanayak16 0:9de45e2c32dc 149 {
snehanayak16 0:9de45e2c32dc 150 int c3 = exist(3);
snehanayak16 0:9de45e2c32dc 151 if( c3 == 0 && request.size()<4)
snehanayak16 0:9de45e2c32dc 152 {
snehanayak16 0:9de45e2c32dc 153 request.push_back(3);
snehanayak16 0:9de45e2c32dc 154 sort();
snehanayak16 0:9de45e2c32dc 155 //pc.printf("Buff size %d\r\n",request.size());
snehanayak16 0:9de45e2c32dc 156 pc.printf("You typed : 3\r\n");
snehanayak16 0:9de45e2c32dc 157 }
snehanayak16 0:9de45e2c32dc 158 }
snehanayak16 0:9de45e2c32dc 159 }
snehanayak16 0:9de45e2c32dc 160
snehanayak16 0:9de45e2c32dc 161
snehanayak16 0:9de45e2c32dc 162 void direction(){
snehanayak16 0:9de45e2c32dc 163 for(int k = 0;k<request.size();k++){
snehanayak16 0:9de45e2c32dc 164 if(request[k] == curfloor){
snehanayak16 0:9de45e2c32dc 165 numfloor = curfloor;
snehanayak16 0:9de45e2c32dc 166 door_oc();
snehanayak16 0:9de45e2c32dc 167 request.erase(request.begin()+k);
snehanayak16 0:9de45e2c32dc 168 break;
snehanayak16 0:9de45e2c32dc 169 }
snehanayak16 0:9de45e2c32dc 170 }
snehanayak16 0:9de45e2c32dc 171 if(request.size()>0){
snehanayak16 0:9de45e2c32dc 172 int max = request[request.size()-1];
snehanayak16 0:9de45e2c32dc 173 if(max > curfloor){
snehanayak16 0:9de45e2c32dc 174 move = 1;
snehanayak16 0:9de45e2c32dc 175 }
snehanayak16 0:9de45e2c32dc 176 else if(max < curfloor){
snehanayak16 0:9de45e2c32dc 177 move = 0;
snehanayak16 0:9de45e2c32dc 178 }
snehanayak16 0:9de45e2c32dc 179 }
snehanayak16 0:9de45e2c32dc 180 }
snehanayak16 0:9de45e2c32dc 181
snehanayak16 0:9de45e2c32dc 182
snehanayak16 0:9de45e2c32dc 183 int floor_detect(){
snehanayak16 0:9de45e2c32dc 184
snehanayak16 0:9de45e2c32dc 185 if(freq > 90.0 && freq < 110.0){
snehanayak16 0:9de45e2c32dc 186 curfloor = 1;
snehanayak16 0:9de45e2c32dc 187 pc.printf("Floor : %d \r\n",curfloor);
snehanayak16 0:9de45e2c32dc 188 //wait(2);
snehanayak16 0:9de45e2c32dc 189 }
snehanayak16 0:9de45e2c32dc 190 else if(freq > 190.0 && freq < 210.0){
snehanayak16 0:9de45e2c32dc 191 curfloor = 2;
snehanayak16 0:9de45e2c32dc 192 pc.printf("Floor : %d \r\n",curfloor);
snehanayak16 0:9de45e2c32dc 193 //wait(2);
snehanayak16 0:9de45e2c32dc 194 }
snehanayak16 0:9de45e2c32dc 195 else if(freq > 290.0 && freq < 310.0){
snehanayak16 0:9de45e2c32dc 196 curfloor = 3;
snehanayak16 0:9de45e2c32dc 197 pc.printf("Floor : %d \r\n",curfloor);
snehanayak16 0:9de45e2c32dc 198 //wait(2);
snehanayak16 0:9de45e2c32dc 199 }
snehanayak16 0:9de45e2c32dc 200 else if(freq > 390.0 && freq < 410.0){
snehanayak16 0:9de45e2c32dc 201 curfloor = 4;
snehanayak16 0:9de45e2c32dc 202 pc.printf("Floor : %d \r\n",curfloor);
snehanayak16 0:9de45e2c32dc 203 //wait(2);
snehanayak16 0:9de45e2c32dc 204 }
snehanayak16 0:9de45e2c32dc 205 else if(freq > 490.0 && freq < 510.0){
snehanayak16 0:9de45e2c32dc 206 curfloor = 5;
snehanayak16 0:9de45e2c32dc 207 pc.printf("Floor : %d \r\n",curfloor);
snehanayak16 0:9de45e2c32dc 208 //wait(2);
snehanayak16 0:9de45e2c32dc 209 }
snehanayak16 0:9de45e2c32dc 210
snehanayak16 0:9de45e2c32dc 211
snehanayak16 0:9de45e2c32dc 212
snehanayak16 0:9de45e2c32dc 213 return curfloor;
snehanayak16 0:9de45e2c32dc 214 }
snehanayak16 0:9de45e2c32dc 215
snehanayak16 0:9de45e2c32dc 216
snehanayak16 0:9de45e2c32dc 217
snehanayak16 0:9de45e2c32dc 218 void rise_edge(){
snehanayak16 0:9de45e2c32dc 219
snehanayak16 0:9de45e2c32dc 220 if(a_pin.read()>0.95){
snehanayak16 0:9de45e2c32dc 221 count ++;
snehanayak16 0:9de45e2c32dc 222 if(count == 1){
snehanayak16 0:9de45e2c32dc 223 period_t.start();
snehanayak16 0:9de45e2c32dc 224 }
snehanayak16 0:9de45e2c32dc 225 else if(count == 2){
snehanayak16 0:9de45e2c32dc 226 period_t.stop();
snehanayak16 0:9de45e2c32dc 227 period = period_t.read();
snehanayak16 0:9de45e2c32dc 228 freq = 1.0/period;
snehanayak16 0:9de45e2c32dc 229 period_t.reset();
snehanayak16 0:9de45e2c32dc 230 count = 0;
snehanayak16 0:9de45e2c32dc 231 }
snehanayak16 0:9de45e2c32dc 232 }
snehanayak16 0:9de45e2c32dc 233 if(freq > 0.0){
snehanayak16 0:9de45e2c32dc 234 //pc.printf("Freq: %3.2f\r\n",freq);
snehanayak16 0:9de45e2c32dc 235 }
snehanayak16 0:9de45e2c32dc 236 }
snehanayak16 0:9de45e2c32dc 237
snehanayak16 0:9de45e2c32dc 238 int get_index(int floor){
snehanayak16 0:9de45e2c32dc 239 for(int k = 0;k<request.size();k++){
snehanayak16 0:9de45e2c32dc 240 if(request[k]==floor){
snehanayak16 0:9de45e2c32dc 241 return k;
snehanayak16 0:9de45e2c32dc 242 }
snehanayak16 0:9de45e2c32dc 243 }
snehanayak16 0:9de45e2c32dc 244 return -1;
snehanayak16 0:9de45e2c32dc 245 }
snehanayak16 0:9de45e2c32dc 246
snehanayak16 0:9de45e2c32dc 247 int main() {
snehanayak16 0:9de45e2c32dc 248
snehanayak16 0:9de45e2c32dc 249 t_scan.attach(&scan,0.004);
snehanayak16 0:9de45e2c32dc 250 column1.fall(&fall1);
snehanayak16 0:9de45e2c32dc 251 column2.fall(&fall2);
snehanayak16 0:9de45e2c32dc 252 column3.fall(&fall3);
snehanayak16 0:9de45e2c32dc 253
snehanayak16 0:9de45e2c32dc 254 servo1.period(0.020);
snehanayak16 0:9de45e2c32dc 255
hlekkala 1:d4b4c89e4a43 256 request.push_back(1);
snehanayak16 0:9de45e2c32dc 257
hlekkala 1:d4b4c89e4a43 258
snehanayak16 0:9de45e2c32dc 259 pc.printf("Start freq : %d\r\n", freq);
snehanayak16 0:9de45e2c32dc 260 pin.rise(&rise_edge);
hlekkala 1:d4b4c89e4a43 261
snehanayak16 0:9de45e2c32dc 262 pc.printf("Start Now ............................. \r\n");
snehanayak16 0:9de45e2c32dc 263 while(1){
snehanayak16 0:9de45e2c32dc 264
snehanayak16 0:9de45e2c32dc 265 //sort();
snehanayak16 0:9de45e2c32dc 266 while(request.size()!=0){
snehanayak16 0:9de45e2c32dc 267 sort();
snehanayak16 0:9de45e2c32dc 268 curfloor = floor_detect();
snehanayak16 0:9de45e2c32dc 269 direction();
snehanayak16 0:9de45e2c32dc 270 if(move == 1){
snehanayak16 0:9de45e2c32dc 271 for(int k = 0;k<request.size();k++){
snehanayak16 0:9de45e2c32dc 272 sort();
snehanayak16 0:9de45e2c32dc 273 numfloor = request[k];
snehanayak16 0:9de45e2c32dc 274 diff = numfloor - curfloor;
snehanayak16 0:9de45e2c32dc 275 while(diff > 0){
snehanayak16 0:9de45e2c32dc 276 //pc.printf("DC ON +1 \r\n");
snehanayak16 0:9de45e2c32dc 277 m.speed(1.0);
hlekkala 1:d4b4c89e4a43 278
snehanayak16 0:9de45e2c32dc 279 curfloor = floor_detect();
snehanayak16 0:9de45e2c32dc 280 if(exist(curfloor)){
snehanayak16 0:9de45e2c32dc 281 numfloor = curfloor;
snehanayak16 0:9de45e2c32dc 282 }
snehanayak16 0:9de45e2c32dc 283 int f_index = get_index(curfloor);
snehanayak16 0:9de45e2c32dc 284 diff = numfloor - curfloor;
snehanayak16 0:9de45e2c32dc 285 if(diff == 0 && f_index!= -1){
snehanayak16 0:9de45e2c32dc 286 //wait(2);
snehanayak16 0:9de45e2c32dc 287 pc.printf("DC OFF \r\n");
snehanayak16 0:9de45e2c32dc 288 m.speed(0.0);
snehanayak16 0:9de45e2c32dc 289 request.erase(request.begin()+f_index);
snehanayak16 0:9de45e2c32dc 290 k--;
snehanayak16 0:9de45e2c32dc 291 door_oc();
snehanayak16 0:9de45e2c32dc 292 }
snehanayak16 0:9de45e2c32dc 293 }
snehanayak16 0:9de45e2c32dc 294 }
snehanayak16 0:9de45e2c32dc 295
snehanayak16 0:9de45e2c32dc 296
snehanayak16 0:9de45e2c32dc 297 }
snehanayak16 0:9de45e2c32dc 298 else if(move == 0){
snehanayak16 0:9de45e2c32dc 299 for(int k = request.size()-1;k >= 0;k--){
snehanayak16 0:9de45e2c32dc 300 sort();
snehanayak16 0:9de45e2c32dc 301 numfloor = request[k];
snehanayak16 0:9de45e2c32dc 302 diff = numfloor - curfloor;
snehanayak16 0:9de45e2c32dc 303 while(diff < 0){
snehanayak16 0:9de45e2c32dc 304 m.speed(-1.0);
hlekkala 1:d4b4c89e4a43 305
snehanayak16 0:9de45e2c32dc 306 curfloor = floor_detect();
snehanayak16 0:9de45e2c32dc 307 if(exist(curfloor)){
snehanayak16 0:9de45e2c32dc 308 numfloor = curfloor;
snehanayak16 0:9de45e2c32dc 309 }
snehanayak16 0:9de45e2c32dc 310 int f_index = get_index(curfloor);
snehanayak16 0:9de45e2c32dc 311 diff = numfloor - curfloor;
snehanayak16 0:9de45e2c32dc 312 if(diff == 0 && f_index!= -1){
snehanayak16 0:9de45e2c32dc 313 pc.printf("DC OFF \r\n");
snehanayak16 0:9de45e2c32dc 314 //wait(2);
snehanayak16 0:9de45e2c32dc 315 m.speed(0.0);
snehanayak16 0:9de45e2c32dc 316 request.erase(request.begin()+k);
snehanayak16 0:9de45e2c32dc 317 k++;
snehanayak16 0:9de45e2c32dc 318 door_oc();
snehanayak16 0:9de45e2c32dc 319 }
snehanayak16 0:9de45e2c32dc 320 }
snehanayak16 0:9de45e2c32dc 321 }
snehanayak16 0:9de45e2c32dc 322 }
snehanayak16 0:9de45e2c32dc 323 }
snehanayak16 0:9de45e2c32dc 324 }
snehanayak16 0:9de45e2c32dc 325 }
snehanayak16 0:9de45e2c32dc 326