AES TEAM / Mbed 2 deprecated AppliedOpSys

basic version

Dependencies:   C12832_lcd USBHost mbed

main.cpp@6:4c62f9c91b1d, 2015-01-25 (annotated)

Committer:
cathal66
Date:
Sun Jan 25 22:26:35 2015 +0000
Revision:
6:4c62f9c91b1d
Parent:
5:e82e00f12634
Child:
7:0fa7430ab812
Added Comments;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hjordan 1:ebf16f5cbdec 1 #include "mbed.h"
Hjordan 1:ebf16f5cbdec 2 #include "rtos.h"
Hjordan 1:ebf16f5cbdec 3 #include "C12832_lcd.h"
Hjordan 1:ebf16f5cbdec 4
Hjordan 1:ebf16f5cbdec 5 float sonarDistance;
Hjordan 1:ebf16f5cbdec 6 float servoPosition;
Hjordan 1:ebf16f5cbdec 7
cathal66 6:4c62f9c91b1d 8 Mutex sonarDistance_mutex; //Init Mutex for sensor value
cathal66 6:4c62f9c91b1d 9 Mutex servoPosition_mutex; //Init Mutex for servo value
cathal66 6:4c62f9c91b1d 10
cathal66 6:4c62f9c91b1d 11 AnalogIn sonarPin(p17); //Assign pin to read sonar sensor
cathal66 6:4c62f9c91b1d 12 PwmOut servoPin(p21); //Assign pin for servo output
Hjordan 1:ebf16f5cbdec 13
cathal66 6:4c62f9c91b1d 14 C12832_LCD lcd; //setup LCD screen
Hjordan 1:ebf16f5cbdec 15
Hjordan 1:ebf16f5cbdec 16 void sonarSensor(void const *args){
Hjordan 1:ebf16f5cbdec 17 while(true){
cathal66 6:4c62f9c91b1d 18 sonarDistance_mutex.lock(); //Mutex lock for the Servo value
cathal66 6:4c62f9c91b1d 19 sonarDistance = sonarPin.read(); //Read analog voltage and store in variable
cathal66 6:4c62f9c91b1d 20 sonarDistance_mutex.unlock(); //Mutex unlock for the Servo value
cathal66 6:4c62f9c91b1d 21 Thread::wait(10); //Pause thread for 10msec
cathal66 6:4c62f9c91b1d 22 } //End of While loop
cathal66 6:4c62f9c91b1d 23 } //End of Thread
Hjordan 1:ebf16f5cbdec 24
Hjordan 1:ebf16f5cbdec 25 void servoControl(void const *args){
Hjordan 1:ebf16f5cbdec 26 while(true){
cathal66 6:4c62f9c91b1d 27 servoPosition_mutex.lock(); //Mutex lock for the Servo value
cathal66 6:4c62f9c91b1d 28 servoPin.write(servoPosition); //Write servo value to servo pin
cathal66 6:4c62f9c91b1d 29 servoPosition_mutex.unlock(); //Mutex unlock for the Servo value
cathal66 6:4c62f9c91b1d 30 } //end of while loop
cathal66 6:4c62f9c91b1d 31 } //end of thread
Hjordan 1:ebf16f5cbdec 32
cathal66 6:4c62f9c91b1d 33 void logic(void const *args)
cathal66 6:4c62f9c91b1d 34 {
cathal66 6:4c62f9c91b1d 35 while(true)
cathal66 6:4c62f9c91b1d 36 {
cathal66 6:4c62f9c91b1d 37 float Average_4[]={0,0,0,0,0,0,0}; //number of value in the array
cathal66 5:e82e00f12634 38 float Average_Sum=0;
cathal66 5:e82e00f12634 39
cathal66 6:4c62f9c91b1d 40 sonarDistance_mutex.lock(); //Mutex lock for the Sonar value
cathal66 6:4c62f9c91b1d 41 servoPosition_mutex.lock(); //Mutex lock for the servo value
cathal66 5:e82e00f12634 42
cathal66 6:4c62f9c91b1d 43 for(int i=0;i<6;i++)
cathal66 6:4c62f9c91b1d 44 {
cathal66 6:4c62f9c91b1d 45 Average_Sum = Average_Sum-Average_4[i]; //Remove the 4th oldest value from the average sum
cathal66 6:4c62f9c91b1d 46 Average_4[i]= sonarDistance; //Add the new value to the array
cathal66 6:4c62f9c91b1d 47 Average_Sum = Average_Sum + Average_4[i]; //Add the new array value to the sum
cathal66 6:4c62f9c91b1d 48 servoPosition = Average_Sum/6; //Divide the array by the number of element in the array
cathal66 6:4c62f9c91b1d 49 }//end for loop
cathal66 6:4c62f9c91b1d 50
cathal66 6:4c62f9c91b1d 51 sonarDistance_mutex.unlock(); //Mutex unlock for the Sonar value
cathal66 6:4c62f9c91b1d 52 servoPosition_mutex.unlock(); //Mutex unlock for the servo value
cathal66 6:4c62f9c91b1d 53 }//end of while loop
cathal66 6:4c62f9c91b1d 54 }//End of thread
Hjordan 1:ebf16f5cbdec 55
Hjordan 1:ebf16f5cbdec 56 void display(void const *args){
Hjordan 1:ebf16f5cbdec 57 while(true){
Hjordan 1:ebf16f5cbdec 58 sonarDistance_mutex.lock();
Hjordan 1:ebf16f5cbdec 59 servoPosition_mutex.lock();
Hjordan 1:ebf16f5cbdec 60 lcd.cls();
Hjordan 1:ebf16f5cbdec 61 lcd.locate(0,0);
Hjordan 3:be269540df58 62 lcd.printf("Sonar : %3.2f \nServo : %3.2f \nSCIENCE!",sonarDistance,servoPosition);
Hjordan 1:ebf16f5cbdec 63 sonarDistance_mutex.unlock();
Hjordan 1:ebf16f5cbdec 64 servoPosition_mutex.unlock();
Hjordan 1:ebf16f5cbdec 65 Thread::wait(200);
Hjordan 1:ebf16f5cbdec 66
Hjordan 1:ebf16f5cbdec 67 }
Hjordan 1:ebf16f5cbdec 68 }
Hjordan 1:ebf16f5cbdec 69
Hjordan 1:ebf16f5cbdec 70 int main(){
Hjordan 1:ebf16f5cbdec 71 sonarDistance = 0.0f;
Hjordan 1:ebf16f5cbdec 72 servoPosition = 0.0f;
Hjordan 1:ebf16f5cbdec 73
Hjordan 1:ebf16f5cbdec 74
Hjordan 1:ebf16f5cbdec 75 Thread sonarSensor_thread(sonarSensor);
Hjordan 1:ebf16f5cbdec 76 Thread servoControl_thread(servoControl);
Hjordan 1:ebf16f5cbdec 77 Thread logic_thread(logic);
Hjordan 1:ebf16f5cbdec 78 Thread display_thread(display);
Hjordan 1:ebf16f5cbdec 79
cathal66 6:4c62f9c91b1d 80 while(true)
cathal66 6:4c62f9c91b1d 81 {
Hjordan 1:ebf16f5cbdec 82 }
Hjordan 1:ebf16f5cbdec 83 }