WIT_EmbOS_Gr1 / Team_Sprint2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   C12832 mbed rtos RangeFinder

main.cpp.orig@10:ca6f2769964e, 2015-04-13 (annotated)

Committer:
Argensis
Date:
Mon Apr 13 13:01:02 2015 +0000
Revision:
10:ca6f2769964e
Merge Andreas movement and Tony's OpenCV/Serial code.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Argensis 10:ca6f2769964e 1 #include "mbed.h"
Argensis 10:ca6f2769964e 2 #include "rtos.h"
Argensis 10:ca6f2769964e 3 #include "Servo.h"
Argensis 10:ca6f2769964e 4 #include "C12832.h"
Argensis 10:ca6f2769964e 5 Servo sPan(p21);
Argensis 10:ca6f2769964e 6 Servo sTilt(p22);
Argensis 10:ca6f2769964e 7 Serial pc(USBTX, USBRX);
Argensis 10:ca6f2769964e 8 Servo sVert(p23);
Argensis 10:ca6f2769964e 9 Sonar distance(p24);
Argensis 10:ca6f2769964e 10 Mutex mutexIn;
Argensis 10:ca6f2769964e 11 Mutex mutexOut;
Argensis 10:ca6f2769964e 12
Argensis 10:ca6f2769964e 13 AnalogIn p1(p19);
Argensis 10:ca6f2769964e 14 AnalogIn p2(p20);
Argensis 10:ca6f2769964e 15
Argensis 10:ca6f2769964e 16 // Global variables
Argensis 10:ca6f2769964e 17 float corHoriz; // horizontal variable arrives from OpenCV
Argensis 10:ca6f2769964e 18 float corVert; // vertical variable arrives from OpenCV
Argensis 10:ca6f2769964e 19 float distance; // distance measured by the sonar
Argensis 10:ca6f2769964e 20 float outVert; // rr
Argensis 10:ca6f2769964e 21 float outTilt;
Argensis 10:ca6f2769964e 22 float outHoriz;
Argensis 10:ca6f2769964e 23 C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7....
Argensis 10:ca6f2769964e 24
Argensis 10:ca6f2769964e 25 /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo.
Argensis 10:ca6f2769964e 26 @update s1, s2 */
Argensis 10:ca6f2769964e 27 void serial_thread(void const *args) {
Argensis 10:ca6f2769964e 28 while (true) {
Argensis 10:ca6f2769964e 29 mutexIn.lock();
Argensis 10:ca6f2769964e 30 // pc.gets(cordinates,4);
Argensis 10:ca6f2769964e 31 // cordinates = pc.putc(pc.getc());
Argensis 10:ca6f2769964e 32 pc.scanf("%s \n %s \n ",corHoriz,corVert);// read from serial port the data
Argensis 10:ca6f2769964e 33 mutexIn.unlock();
Argensis 10:ca6f2769964e 34 Thread::wait(200);
Argensis 10:ca6f2769964e 35 }
Argensis 10:ca6f2769964e 36 }
Argensis 10:ca6f2769964e 37
Argensis 10:ca6f2769964e 38 /* Thread LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen.
Argensis 10:ca6f2769964e 39 @update inData */
Argensis 10:ca6f2769964e 40 void lcd_thread(void const *args) {
Argensis 10:ca6f2769964e 41 while (true) {
Argensis 10:ca6f2769964e 42 mutexIn.lock();
Argensis 10:ca6f2769964e 43 mutexOut.lock();
Argensis 10:ca6f2769964e 44 // Display values on the LCD screen
Argensis 10:ca6f2769964e 45 lcd.cls(); // clear the display
Argensis 10:ca6f2769964e 46 lcd.locate(0,5); // the location where you want your charater to be displayed
Argensis 10:ca6f2769964e 47 lcd.printf("Hor:%s",corHoriz);
Argensis 10:ca6f2769964e 48 lcd.locate(0,20); // the location where you want your charater to be displayed
Argensis 10:ca6f2769964e 49 lcd.printf("Ver%s", corVert);
Argensis 10:ca6f2769964e 50 mutexIn.unlock();
Argensis 10:ca6f2769964e 51 mutexOut.unlock();
Argensis 10:ca6f2769964e 52 Thread::wait(25);
Argensis 10:ca6f2769964e 53 }
Argensis 10:ca6f2769964e 54 }
Argensis 10:ca6f2769964e 55
Argensis 10:ca6f2769964e 56 /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
Argensis 10:ca6f2769964e 57 @update inData */
Argensis 10:ca6f2769964e 58 void control_thread(void const *args) {
Argensis 10:ca6f2769964e 59 while (true) {
Argensis 10:ca6f2769964e 60 mutexIn.lock();
Argensis 10:ca6f2769964e 61 float differ;
Argensis 10:ca6f2769964e 62 differ = exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt));
Argensis 10:ca6f2769964e 63 if (corVert > .5) { // check if face is below the half of the camera view
Argensis 10:ca6f2769964e 64 if (outTilt > .5) { // check if lamp head is facing down
Argensis 10:ca6f2769964e 65 // moves lamp down by the fraction of the difference from the middle
Argensis 10:ca6f2769964e 66 mutexIn.unlock();
Argensis 10:ca6f2769964e 67 Thread::wait(25);
Argensis 10:ca6f2769964e 68 }
Argensis 10:ca6f2769964e 69 }
Argensis 10:ca6f2769964e 70
Argensis 10:ca6f2769964e 71 /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo.
Argensis 10:ca6f2769964e 72 @update s1, s2 */
Argensis 10:ca6f2769964e 73 void servo_thread(void const *args) {
Argensis 10:ca6f2769964e 74 while (true) {
Argensis 10:ca6f2769964e 75 mutexOut.lock();
Argensis 10:ca6f2769964e 76 sTi1t = outTilt;
Argensis 10:ca6f2769964e 77 sPan = outHoriz;
Argensis 10:ca6f2769964e 78 sVert = outVert;
Argensis 10:ca6f2769964e 79 mutexOut.unlock();
Argensis 10:ca6f2769964e 80 Thread::wait(200);
Argensis 10:ca6f2769964e 81 }
Argensis 10:ca6f2769964e 82 }
Argensis 10:ca6f2769964e 83
Argensis 10:ca6f2769964e 84 int main() {
Argensis 10:ca6f2769964e 85 Thread thread_1(serial_thread); // Start Serial Thread
Argensis 10:ca6f2769964e 86 Thread thread_2(lcd_thread); // Start LCD Thread
Argensis 10:ca6f2769964e 87 Thread thread_3(control_thread); // Start Control Thread
Argensis 10:ca6f2769964e 88 Thread thread_4(servo_thread); // Start Servo Thread
Argensis 10:ca6f2769964e 89 while(1) {
Argensis 10:ca6f2769964e 90 Thread::wait(10);
Argensis 10:ca6f2769964e 91 }
Argensis 10:ca6f2769964e 92 }