Code for Sprint 2
Dependencies: C12832 mbed rtos RangeFinder
main.cpp@19:32270b453137, 2015-04-20 (annotated)
- Committer:
- Soldier7
- Date:
- Mon Apr 20 14:07:31 2015 +0000
- Revision:
- 19:32270b453137
- Parent:
- 18:5dfebb54cdbd
- Child:
- 20:5f6df7ae19a3
The tilt and the vertical servo now approximately moves together with the algorithm.
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" |
Argensis | 9:e5d24b7a921b | 5 | |
Soldier7 | 14:74733a28eb80 | 6 | Servo tiltServo(p24); |
Soldier7 | 14:74733a28eb80 | 7 | Servo panServo(p25); |
Soldier7 | 0:5edc27224a37 | 8 | Serial pc(USBTX, USBRX); |
Argensis | 10:ca6f2769964e | 9 | Servo vertServo(p23); |
Soldier7 | 15:4730277de7c2 | 10 | |
Soldier7 | 14:74733a28eb80 | 11 | AnalogIn sonar(p19); // temporary changed to potmeter |
Soldier7 | 18:5dfebb54cdbd | 12 | AnalogIn corVert(p20); // temporary |
Soldier7 | 0:5edc27224a37 | 13 | Mutex mutexIn; |
Soldier7 | 0:5edc27224a37 | 14 | Mutex mutexOut; |
Soldier7 | 0:5edc27224a37 | 15 | |
Soldier7 | 8:fe434a018d96 | 16 | // Global variables |
Soldier7 | 15:4730277de7c2 | 17 | float corHoriz = 0.5; // horizontal variable arrives from OpenCV |
Soldier7 | 18:5dfebb54cdbd | 18 | //float corVert = 0.5; // vertical variable arrives from OpenCV |
Soldier7 | 18:5dfebb54cdbd | 19 | float corVert1 = 0.5; // temporary |
Soldier7 | 15:4730277de7c2 | 20 | float distance = 0.5; // distance from the sonar sensor |
Soldier7 | 14:74733a28eb80 | 21 | float outVert; // output to vertical servo |
Soldier7 | 14:74733a28eb80 | 22 | float outTilt; // output to tilt servo |
Soldier7 | 14:74733a28eb80 | 23 | float outHoriz; // output to horizontal servo |
Soldier7 | 15:4730277de7c2 | 24 | float differ; // temporary global. Can be local. |
Soldier7 | 14:74733a28eb80 | 25 | C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5, p7.... |
Soldier7 | 0:5edc27224a37 | 26 | |
Soldier7 | 0:5edc27224a37 | 27 | /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo. |
Soldier7 | 0:5edc27224a37 | 28 | @update s1, s2 */ |
Soldier7 | 12:f6d4980c48d6 | 29 | void serial_thread(void const *args) |
Soldier7 | 12:f6d4980c48d6 | 30 | { |
Soldier7 | 0:5edc27224a37 | 31 | while (true) { |
Soldier7 | 18:5dfebb54cdbd | 32 | pc.scanf("%f,%f", &corHoriz, &corVert1);// read from serial port the data |
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 | // Display values on the LCD screen |
Soldier7 | 0:5edc27224a37 | 42 | lcd.cls(); // clear the display |
Soldier7 | 18:5dfebb54cdbd | 43 | lcd.locate(0,1); // the location where you want your charater to be displayed |
Soldier7 | 15:4730277de7c2 | 44 | lcd.printf("differ: %0.3f, OutTilt: %0.3f", differ, outTilt); |
Soldier7 | 18:5dfebb54cdbd | 45 | lcd.locate(0,9); // the location where you want your charater to be displayed |
Soldier7 | 18:5dfebb54cdbd | 46 | lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert.read(), outVert); |
Soldier7 | 18:5dfebb54cdbd | 47 | //lcd.locate(0,18); // the location where you want your charater to be displayed |
Soldier7 | 18:5dfebb54cdbd | 48 | //lcd.printf("Sonar: %0.3f", sonar.read()); |
Argensis | 9:e5d24b7a921b | 49 | Thread::wait(250); |
Soldier7 | 0:5edc27224a37 | 50 | } |
Soldier7 | 0:5edc27224a37 | 51 | } |
Soldier7 | 0:5edc27224a37 | 52 | |
Soldier7 | 0:5edc27224a37 | 53 | /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen. |
Soldier7 | 0:5edc27224a37 | 54 | @update inData */ |
Soldier7 | 12:f6d4980c48d6 | 55 | void control_thread(void const *args) |
Soldier7 | 12:f6d4980c48d6 | 56 | { |
Soldier7 | 0:5edc27224a37 | 57 | while (true) { |
Soldier7 | 0:5edc27224a37 | 58 | mutexIn.lock(); |
Soldier7 | 15:4730277de7c2 | 59 | //float differ; |
Soldier7 | 18:5dfebb54cdbd | 60 | differ = ((exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt))) - .49 ) * 2.5; |
Soldier7 | 18:5dfebb54cdbd | 61 | if (corVert > 0 && corVert < 1) { // if corVert is valid (between 0 - 1) then do movements |
Soldier7 | 12:f6d4980c48d6 | 62 | // moves lamp down by the fraction of the difference from the middle |
Soldier7 | 19:32270b453137 | 63 | outVert = (corVert * 2 * (1 - differ)); |
Soldier7 | 14:74733a28eb80 | 64 | // tilt down by the fraction of the difference from the middle |
Soldier7 | 19:32270b453137 | 65 | outTilt = (corVert * differ) + .15; |
Soldier7 | 18:5dfebb54cdbd | 66 | /*} else if (0 < corVert < 0.3) { |
Soldier7 | 14:74733a28eb80 | 67 | outVert = 0; |
Soldier7 | 14:74733a28eb80 | 68 | outTilt = corVert * .9; // Follow the low movement only with the tilt servo. |
Soldier7 | 14:74733a28eb80 | 69 | // (.9 is the correction of the side of the screen at OpenCV) |
Soldier7 | 14:74733a28eb80 | 70 | } else if (corVert > 0.7 && corVert < 1) { |
Soldier7 | 14:74733a28eb80 | 71 | outVert = 1; |
Soldier7 | 15:4730277de7c2 | 72 | outTilt = corVert * 1.1; */ |
Soldier7 | 14:74733a28eb80 | 73 | } else { // Else this is the case when there is no input from the OpenCV |
Soldier7 | 18:5dfebb54cdbd | 74 | outVert = corVert; |
Soldier7 | 18:5dfebb54cdbd | 75 | outTilt = corVert; |
Soldier7 | 14:74733a28eb80 | 76 | // TODO Pan search code here. (Searching personality.) |
Soldier7 | 12:f6d4980c48d6 | 77 | } |
Soldier7 | 0:5edc27224a37 | 78 | mutexIn.unlock(); |
Soldier7 | 0:5edc27224a37 | 79 | Thread::wait(25); |
Soldier7 | 0:5edc27224a37 | 80 | } |
Soldier7 | 0:5edc27224a37 | 81 | } |
Soldier7 | 0:5edc27224a37 | 82 | |
Soldier7 | 0:5edc27224a37 | 83 | /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo. |
Soldier7 | 0:5edc27224a37 | 84 | @update s1, s2 */ |
Soldier7 | 12:f6d4980c48d6 | 85 | void servo_thread(void const *args) |
Soldier7 | 12:f6d4980c48d6 | 86 | { |
Soldier7 | 0:5edc27224a37 | 87 | while (true) { |
Soldier7 | 0:5edc27224a37 | 88 | mutexOut.lock(); |
Argensis | 10:ca6f2769964e | 89 | tiltServo = outTilt; |
Argensis | 10:ca6f2769964e | 90 | panServo = outHoriz; |
Argensis | 10:ca6f2769964e | 91 | vertServo = outVert; |
Soldier7 | 0:5edc27224a37 | 92 | mutexOut.unlock(); |
Soldier7 | 0:5edc27224a37 | 93 | Thread::wait(200); |
Soldier7 | 0:5edc27224a37 | 94 | } |
Soldier7 | 0:5edc27224a37 | 95 | } |
Soldier7 | 0:5edc27224a37 | 96 | |
Soldier7 | 12:f6d4980c48d6 | 97 | int main() |
Soldier7 | 12:f6d4980c48d6 | 98 | { |
Soldier7 | 0:5edc27224a37 | 99 | Thread thread_1(serial_thread); // Start Serial Thread |
Soldier7 | 0:5edc27224a37 | 100 | Thread thread_2(lcd_thread); // Start LCD Thread |
Argensis | 10:ca6f2769964e | 101 | Thread thread_3(control_thread); // Start Servo Thread |
Soldier7 | 0:5edc27224a37 | 102 | Thread thread_4(servo_thread); // Start Servo Thread |
Soldier7 | 0:5edc27224a37 | 103 | while(1) { |
Argensis | 9:e5d24b7a921b | 104 | wait(1); |
Soldier7 | 0:5edc27224a37 | 105 | } |
Soldier7 | 0:5edc27224a37 | 106 | } |