ese519
/
ESE519_Lab3_EC_v1
Lab3
Fork of ESE519_Lab3_v4 by
main.cpp@0:015087d61ca1, 2015-10-03 (annotated)
- Committer:
- jfields
- Date:
- Sat Oct 03 00:02:31 2015 +0000
- Revision:
- 0:015087d61ca1
- Child:
- 1:e79ac0826624
first
Who changed what in which revision?
User | Revision | Line number | New 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 | } |