Code for Sprint 2
Dependencies: C12832 mbed rtos RangeFinder
main.cpp@12:f6d4980c48d6, 2015-04-13 (annotated)
- Committer:
- Soldier7
- Date:
- Mon Apr 13 13:03:16 2015 +0000
- Revision:
- 12:f6d4980c48d6
- Parent:
- 8:fe434a018d96
- Child:
- 13:4ef3d9835b13
Unfinished changes on tilt and up servo movement.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Soldier7 | 0:5edc27224a37 | 1 | #include "mbed.h" |
Soldier7 | 0:5edc27224a37 | 2 | #include "rtos.h" |
Soldier7 | 0:5edc27224a37 | 3 | #include "Servo.h" |
Soldier7 | 0:5edc27224a37 | 4 | #include "C12832.h" |
Soldier7 | 8:fe434a018d96 | 5 | Servo sPan(p21); |
Soldier7 | 7:24d62ef1ed34 | 6 | Servo sTilt(p22); |
Soldier7 | 0:5edc27224a37 | 7 | Serial pc(USBTX, USBRX); |
Soldier7 | 8:fe434a018d96 | 8 | Servo sVert(p23); |
Soldier7 | 12:f6d4980c48d6 | 9 | AnalogIn distance(p19); |
Soldier7 | 0:5edc27224a37 | 10 | Mutex mutexIn; |
Soldier7 | 0:5edc27224a37 | 11 | Mutex mutexOut; |
Soldier7 | 0:5edc27224a37 | 12 | |
Soldier7 | 8:fe434a018d96 | 13 | // Global variables |
Soldier7 | 8:fe434a018d96 | 14 | float corHoriz; // horizontal variable arrives from OpenCV |
Soldier7 | 8:fe434a018d96 | 15 | float corVert; // vertical variable arrives from OpenCV |
Soldier7 | 8:fe434a018d96 | 16 | float distance; // distance measured by the sonar |
Soldier7 | 12:f6d4980c48d6 | 17 | float outVert; // |
Soldier7 | 8:fe434a018d96 | 18 | float outTilt; |
Soldier7 | 8:fe434a018d96 | 19 | float outHoriz; |
Soldier7 | 0:5edc27224a37 | 20 | C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7.... |
Soldier7 | 0:5edc27224a37 | 21 | |
Soldier7 | 0:5edc27224a37 | 22 | /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo. |
Soldier7 | 0:5edc27224a37 | 23 | @update s1, s2 */ |
Soldier7 | 12:f6d4980c48d6 | 24 | void serial_thread(void const *args) |
Soldier7 | 12:f6d4980c48d6 | 25 | { |
Soldier7 | 0:5edc27224a37 | 26 | while (true) { |
Soldier7 | 0:5edc27224a37 | 27 | mutexIn.lock(); |
Soldier7 | 12:f6d4980c48d6 | 28 | // pc.gets(cordinates,4); |
Soldier7 | 12:f6d4980c48d6 | 29 | // cordinates = pc.putc(pc.getc()); |
Soldier7 | 12:f6d4980c48d6 | 30 | pc.scanf("%s \n %s \n ",corHoriz,corVert);// read from serial port the data |
Soldier7 | 0:5edc27224a37 | 31 | mutexIn.unlock(); |
Soldier7 | 0:5edc27224a37 | 32 | Thread::wait(200); |
Soldier7 | 0:5edc27224a37 | 33 | } |
Soldier7 | 0:5edc27224a37 | 34 | } |
Soldier7 | 12:f6d4980c48d6 | 35 | |
Soldier7 | 0:5edc27224a37 | 36 | /* Thread LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen. |
Soldier7 | 0:5edc27224a37 | 37 | @update inData */ |
Soldier7 | 12:f6d4980c48d6 | 38 | void lcd_thread(void const *args) |
Soldier7 | 12:f6d4980c48d6 | 39 | { |
Soldier7 | 0:5edc27224a37 | 40 | while (true) { |
Soldier7 | 0:5edc27224a37 | 41 | mutexIn.lock(); |
Soldier7 | 0:5edc27224a37 | 42 | mutexOut.lock(); |
Soldier7 | 0:5edc27224a37 | 43 | // Display values on the LCD screen |
Soldier7 | 0:5edc27224a37 | 44 | lcd.cls(); // clear the display |
Soldier7 | 4:70090f3b1f07 | 45 | lcd.locate(0,5); // the location where you want your charater to be displayed |
Soldier7 | 4:70090f3b1f07 | 46 | lcd.printf("Hor:%s",corHoriz); |
Ali_taher | 3:3d53799c2f18 | 47 | lcd.locate(0,20); // the location where you want your charater to be displayed |
Soldier7 | 4:70090f3b1f07 | 48 | lcd.printf("Ver%s", corVert); |
Soldier7 | 0:5edc27224a37 | 49 | mutexIn.unlock(); |
Soldier7 | 0:5edc27224a37 | 50 | mutexOut.unlock(); |
Soldier7 | 0:5edc27224a37 | 51 | Thread::wait(25); |
Soldier7 | 0:5edc27224a37 | 52 | } |
Soldier7 | 0:5edc27224a37 | 53 | } |
Soldier7 | 0:5edc27224a37 | 54 | |
Soldier7 | 0:5edc27224a37 | 55 | /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen. |
Soldier7 | 0:5edc27224a37 | 56 | @update inData */ |
Soldier7 | 12:f6d4980c48d6 | 57 | void control_thread(void const *args) |
Soldier7 | 12:f6d4980c48d6 | 58 | { |
Soldier7 | 0:5edc27224a37 | 59 | while (true) { |
Soldier7 | 0:5edc27224a37 | 60 | mutexIn.lock(); |
Soldier7 | 8:fe434a018d96 | 61 | float differ; |
Soldier7 | 8:fe434a018d96 | 62 | differ = exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt)); |
Soldier7 | 12:f6d4980c48d6 | 63 | if (corVert > .45) { // check if face is below the half of the camera view |
Soldier7 | 12:f6d4980c48d6 | 64 | if (outTilt > .45) { // check if lamp head is facing down |
Soldier7 | 12:f6d4980c48d6 | 65 | // moves lamp down by the fraction of the difference from the middle |
Soldier7 | 12:f6d4980c48d6 | 66 | outVert = outVert + |
Soldier7 | 12:f6d4980c48d6 | 67 | outTilt = outTilt + |
Soldier7 | 12:f6d4980c48d6 | 68 | } else if (outTilt < .55) { // check if lamp head is facing up |
Soldier7 | 12:f6d4980c48d6 | 69 | // |
Soldier7 | 12:f6d4980c48d6 | 70 | } |
Soldier7 | 12:f6d4980c48d6 | 71 | |
Soldier7 | 12:f6d4980c48d6 | 72 | } else if (corVert < .55) { |
Soldier7 | 12:f6d4980c48d6 | 73 | // moves lamp up by the fraction of the difference from the middle |
Soldier7 | 12:f6d4980c48d6 | 74 | } |
Soldier7 | 0:5edc27224a37 | 75 | } |
Soldier7 | 12:f6d4980c48d6 | 76 | mutexIn.unlock(); |
Soldier7 | 12:f6d4980c48d6 | 77 | Thread::wait(25); |
Soldier7 | 12:f6d4980c48d6 | 78 | } |
Soldier7 | 0:5edc27224a37 | 79 | } |
Soldier7 | 0:5edc27224a37 | 80 | |
Soldier7 | 0:5edc27224a37 | 81 | /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo. |
Soldier7 | 0:5edc27224a37 | 82 | @update s1, s2 */ |
Soldier7 | 12:f6d4980c48d6 | 83 | void servo_thread(void const *args) |
Soldier7 | 12:f6d4980c48d6 | 84 | { |
Soldier7 | 0:5edc27224a37 | 85 | while (true) { |
Soldier7 | 0:5edc27224a37 | 86 | mutexOut.lock(); |
Soldier7 | 8:fe434a018d96 | 87 | sTi1t = outTilt; |
Soldier7 | 8:fe434a018d96 | 88 | sPan = outHoriz; |
Soldier7 | 8:fe434a018d96 | 89 | sVert = outVert; |
Soldier7 | 0:5edc27224a37 | 90 | mutexOut.unlock(); |
Soldier7 | 0:5edc27224a37 | 91 | Thread::wait(200); |
Soldier7 | 0:5edc27224a37 | 92 | } |
Soldier7 | 0:5edc27224a37 | 93 | } |
Soldier7 | 0:5edc27224a37 | 94 | |
Soldier7 | 12:f6d4980c48d6 | 95 | int main() |
Soldier7 | 12:f6d4980c48d6 | 96 | { |
Soldier7 | 0:5edc27224a37 | 97 | Thread thread_1(serial_thread); // Start Serial Thread |
Soldier7 | 0:5edc27224a37 | 98 | Thread thread_2(lcd_thread); // Start LCD Thread |
Soldier7 | 0:5edc27224a37 | 99 | Thread thread_3(control_thread); // Start Control Thread |
Soldier7 | 0:5edc27224a37 | 100 | Thread thread_4(servo_thread); // Start Servo Thread |
Soldier7 | 0:5edc27224a37 | 101 | while(1) { |
Soldier7 | 0:5edc27224a37 | 102 | Thread::wait(10); |
Soldier7 | 0:5edc27224a37 | 103 | } |
Soldier7 | 0:5edc27224a37 | 104 | } |