Lab3

Dependencies:   mbed-rtos mbed

Fork of ESE519_Lab3_v4 by ese519

Committer:
jfields
Date:
Sat Oct 03 00:02:31 2015 +0000
Revision:
0:015087d61ca1
Child:
1:e79ac0826624
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jfields 0:015087d61ca1 1 #include "mbed.h"
jfields 0:015087d61ca1 2
jfields 0:015087d61ca1 3 Serial pc(USBTX,USBRX);
jfields 0:015087d61ca1 4 // functions
jfields 0:015087d61ca1 5 void find_keys();
jfields 0:015087d61ca1 6 Timer t;
jfields 0:015087d61ca1 7
jfields 0:015087d61ca1 8 // init board
jfields 0:015087d61ca1 9 PwmOut dc_motor(p26);
jfields 0:015087d61ca1 10 PwmOut servo1(p25);
jfields 0:015087d61ca1 11 PwmOut servo2(p24);
jfields 0:015087d61ca1 12 DigitalOut in1(p14); // elevator direction
jfields 0:015087d61ca1 13 DigitalOut in2(p13); // elevator direction
jfields 0:015087d61ca1 14 AnalogIn IRSensor(p16); // read IR sensor
jfields 0:015087d61ca1 15 //AnalogOut test(p18);
jfields 0:015087d61ca1 16
jfields 0:015087d61ca1 17 // keyboard read
jfields 0:015087d61ca1 18 DigitalIn col1(p20);
jfields 0:015087d61ca1 19 DigitalIn col2(p19);
jfields 0:015087d61ca1 20 DigitalIn col3(p18);
jfields 0:015087d61ca1 21 DigitalOut row1(p30);
jfields 0:015087d61ca1 22 DigitalOut row2(p27);
jfields 0:015087d61ca1 23
jfields 0:015087d61ca1 24 DigitalOut ind1(LED1);
jfields 0:015087d61ca1 25 DigitalOut ind2(LED2);
jfields 0:015087d61ca1 26 DigitalOut ind3(LED3);
jfields 0:015087d61ca1 27 DigitalOut ind4(LED4);
jfields 0:015087d61ca1 28
jfields 0:015087d61ca1 29
jfields 0:015087d61ca1 30 // init vars
jfields 0:015087d61ca1 31 float ADC_Voltage;
jfields 0:015087d61ca1 32 float period = 0.02; // sec
jfields 0:015087d61ca1 33 float dty_dc = 0.75; // PCT [0-100]
jfields 0:015087d61ca1 34 float dty_servo_open = 0.1125; // PCT [3.75-11.25]
jfields 0:015087d61ca1 35 float dty_servo_close = 0.0375; // PCT [3.75-11.25]
jfields 0:015087d61ca1 36 int cur_floor = 1;
jfields 0:015087d61ca1 37 int desired_floor = 1;
jfields 0:015087d61ca1 38 int tperiod;
jfields 0:015087d61ca1 39
jfields 0:015087d61ca1 40 int main() {
jfields 0:015087d61ca1 41
jfields 0:015087d61ca1 42 // set period (constant)
jfields 0:015087d61ca1 43 dc_motor.period(period);
jfields 0:015087d61ca1 44 // servo1.period(period);
jfields 0:015087d61ca1 45 // servo2.period(period);
jfields 0:015087d61ca1 46
jfields 0:015087d61ca1 47 // start with closed doors
jfields 0:015087d61ca1 48 // servo1.write(dty_servo_close);
jfields 0:015087d61ca1 49 //servo2.write(dty_servo_close);
jfields 0:015087d61ca1 50
jfields 0:015087d61ca1 51 // run elevator program
jfields 0:015087d61ca1 52 while(1) {
jfields 0:015087d61ca1 53
jfields 0:015087d61ca1 54
jfields 0:015087d61ca1 55 // check for key press
jfields 0:015087d61ca1 56 find_keys();
jfields 0:015087d61ca1 57
jfields 0:015087d61ca1 58 // check if need to move
jfields 0:015087d61ca1 59 if (cur_floor != desired_floor) {
jfields 0:015087d61ca1 60
jfields 0:015087d61ca1 61 // determine direction
jfields 0:015087d61ca1 62 if (cur_floor > desired_floor) { // move down
jfields 0:015087d61ca1 63 in1 = 0;
jfields 0:015087d61ca1 64 in2 = 1;
jfields 0:015087d61ca1 65 }
jfields 0:015087d61ca1 66 else { // move up
jfields 0:015087d61ca1 67 in1 = 1;
jfields 0:015087d61ca1 68 in2 = 0;
jfields 0:015087d61ca1 69 }
jfields 0:015087d61ca1 70
jfields 0:015087d61ca1 71 // start car
jfields 0:015087d61ca1 72 dc_motor.write(dty_dc);
jfields 0:015087d61ca1 73
jfields 0:015087d61ca1 74 // check IR sensors
jfields 0:015087d61ca1 75
jfields 0:015087d61ca1 76 while (cur_floor != desired_floor) {
jfields 0:015087d61ca1 77 // check IR emitter here!
jfields 0:015087d61ca1 78
jfields 0:015087d61ca1 79 while(!ADC_Voltage)
jfields 0:015087d61ca1 80 ADC_Voltage = IRSensor.read();
jfields 0:015087d61ca1 81
jfields 0:015087d61ca1 82 t.start();
jfields 0:015087d61ca1 83
jfields 0:015087d61ca1 84 while(ADC_Voltage)
jfields 0:015087d61ca1 85 ADC_Voltage = IRSensor.read();
jfields 0:015087d61ca1 86
jfields 0:015087d61ca1 87 t.stop();
jfields 0:015087d61ca1 88
jfields 0:015087d61ca1 89 tperiod = 2 * t.read_us();
jfields 0:015087d61ca1 90
jfields 0:015087d61ca1 91 t.reset();
jfields 0:015087d61ca1 92
jfields 0:015087d61ca1 93 if(tperiod > 9900 && tperiod < 10100)
jfields 0:015087d61ca1 94 cur_floor = 1;
jfields 0:015087d61ca1 95 else if(tperiod > 3900 && tperiod < 4100)
jfields 0:015087d61ca1 96 cur_floor = 2;
jfields 0:015087d61ca1 97 else if(tperiod > 1900 && tperiod < 2100)
jfields 0:015087d61ca1 98 cur_floor = 3;
jfields 0:015087d61ca1 99 else if(tperiod > 1300 && tperiod < 1500)
jfields 0:015087d61ca1 100 cur_floor = 4;
jfields 0:015087d61ca1 101 else if(tperiod > 900 && tperiod < 1100)
jfields 0:015087d61ca1 102 cur_floor = 5;
jfields 0:015087d61ca1 103
jfields 0:015087d61ca1 104 //pc.printf("%d", cur_floor);
jfields 0:015087d61ca1 105 //wait_ms(1000);
jfields 0:015087d61ca1 106 }
jfields 0:015087d61ca1 107 }
jfields 0:015087d61ca1 108
jfields 0:015087d61ca1 109 // stop car
jfields 0:015087d61ca1 110 dc_motor.write(0);
jfields 0:015087d61ca1 111
jfields 0:015087d61ca1 112 // }
jfields 0:015087d61ca1 113
jfields 0:015087d61ca1 114 // open door
jfields 0:015087d61ca1 115
jfields 0:015087d61ca1 116 // servo1.write(dty_servo_open);
jfields 0:015087d61ca1 117 // servo2.write(dty_servo_open);
jfields 0:015087d61ca1 118
jfields 0:015087d61ca1 119
jfields 0:015087d61ca1 120 }
jfields 0:015087d61ca1 121
jfields 0:015087d61ca1 122
jfields 0:015087d61ca1 123 }
jfields 0:015087d61ca1 124
jfields 0:015087d61ca1 125 void find_keys() {
jfields 0:015087d61ca1 126
jfields 0:015087d61ca1 127 // check row 1
jfields 0:015087d61ca1 128 row1 = 0;
jfields 0:015087d61ca1 129 row2 = 1;
jfields 0:015087d61ca1 130 if (!col1) {
jfields 0:015087d61ca1 131 desired_floor = 1;
jfields 0:015087d61ca1 132 }
jfields 0:015087d61ca1 133 if (!col2) {
jfields 0:015087d61ca1 134 desired_floor = 2;
jfields 0:015087d61ca1 135 }
jfields 0:015087d61ca1 136 if (!col3) {
jfields 0:015087d61ca1 137 desired_floor = 3;
jfields 0:015087d61ca1 138 }
jfields 0:015087d61ca1 139
jfields 0:015087d61ca1 140 // check row 2
jfields 0:015087d61ca1 141 row1 = 1;
jfields 0:015087d61ca1 142 row2 = 0;
jfields 0:015087d61ca1 143 if (!col1) {
jfields 0:015087d61ca1 144 desired_floor = 4;
jfields 0:015087d61ca1 145 }
jfields 0:015087d61ca1 146
jfields 0:015087d61ca1 147 }