AES TEAM / Mbed 2 deprecated AppliedOpSys

basic version

Dependencies:   C12832_lcd USBHost mbed

main.cpp@5:e82e00f12634, 2015-01-19 (annotated)

Committer:
cathal66
Date:
Mon Jan 19 15:28:35 2015 +0000
Revision:
5:e82e00f12634
Parent:
4:429d9a63eb79
Child:
6:4c62f9c91b1d
Average Moving filter

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
Hjordan 1:ebf16f5cbdec 8 Mutex sonarDistance_mutex;
Hjordan 1:ebf16f5cbdec 9 Mutex servoPosition_mutex;
Hjordan 1:ebf16f5cbdec 10
Hjordan 1:ebf16f5cbdec 11 AnalogIn sonarPin(p17);
Hjordan 1:ebf16f5cbdec 12 PwmOut servoPin(p21);
Hjordan 1:ebf16f5cbdec 13
Hjordan 1:ebf16f5cbdec 14 C12832_LCD lcd;
Hjordan 1:ebf16f5cbdec 15
Hjordan 1:ebf16f5cbdec 16 void sonarSensor(void const *args){
Hjordan 1:ebf16f5cbdec 17 while(true){
Hjordan 1:ebf16f5cbdec 18 sonarDistance_mutex.lock();
Hjordan 1:ebf16f5cbdec 19 sonarDistance = sonarPin.read();
Hjordan 1:ebf16f5cbdec 20 sonarDistance_mutex.unlock();
cathal66 5:e82e00f12634 21 Thread::wait(10);
Hjordan 1:ebf16f5cbdec 22 }
Hjordan 1:ebf16f5cbdec 23 }
Hjordan 1:ebf16f5cbdec 24
Hjordan 1:ebf16f5cbdec 25 void servoControl(void const *args){
Hjordan 1:ebf16f5cbdec 26 while(true){
Hjordan 1:ebf16f5cbdec 27 servoPosition_mutex.lock();
Hjordan 1:ebf16f5cbdec 28 servoPin.write(servoPosition);
Hjordan 1:ebf16f5cbdec 29 servoPosition_mutex.unlock();
Hjordan 1:ebf16f5cbdec 30 }
Hjordan 1:ebf16f5cbdec 31 }
Hjordan 1:ebf16f5cbdec 32
Hjordan 1:ebf16f5cbdec 33 void logic(void const *args){
Hjordan 1:ebf16f5cbdec 34 while(true){
cathal66 5:e82e00f12634 35 float Average_4[]={0,0,0,0,0,0,0};
cathal66 5:e82e00f12634 36 float Average_Sum=0;
cathal66 5:e82e00f12634 37
Hjordan 1:ebf16f5cbdec 38 sonarDistance_mutex.lock();
Hjordan 1:ebf16f5cbdec 39 servoPosition_mutex.lock();
cathal66 5:e82e00f12634 40 //servoPosition = (sonarDistance - servoPosition)/2 ;
cathal66 5:e82e00f12634 41 for(int i=0;i<6;i++)
cathal66 5:e82e00f12634 42 {
cathal66 5:e82e00f12634 43 Average_Sum = Average_Sum-Average_4[i];
cathal66 5:e82e00f12634 44 Average_4[i]= sonarDistance;
cathal66 5:e82e00f12634 45 Average_Sum = Average_Sum + Average_4[i];
cathal66 5:e82e00f12634 46 servoPosition = Average_Sum/6;
cathal66 5:e82e00f12634 47 // Thread::wait(50);
cathal66 5:e82e00f12634 48 }
cathal66 5:e82e00f12634 49
Hjordan 1:ebf16f5cbdec 50 sonarDistance_mutex.unlock();
Hjordan 1:ebf16f5cbdec 51 servoPosition_mutex.unlock();
Hjordan 1:ebf16f5cbdec 52 }
Hjordan 1:ebf16f5cbdec 53 }
Hjordan 1:ebf16f5cbdec 54
Hjordan 1:ebf16f5cbdec 55 void display(void const *args){
Hjordan 1:ebf16f5cbdec 56 while(true){
Hjordan 1:ebf16f5cbdec 57 sonarDistance_mutex.lock();
Hjordan 1:ebf16f5cbdec 58 servoPosition_mutex.lock();
Hjordan 1:ebf16f5cbdec 59 lcd.cls();
Hjordan 1:ebf16f5cbdec 60 lcd.locate(0,0);
Hjordan 3:be269540df58 61 lcd.printf("Sonar : %3.2f \nServo : %3.2f \nSCIENCE!",sonarDistance,servoPosition);
Hjordan 1:ebf16f5cbdec 62 sonarDistance_mutex.unlock();
Hjordan 1:ebf16f5cbdec 63 servoPosition_mutex.unlock();
Hjordan 1:ebf16f5cbdec 64 Thread::wait(200);
Hjordan 1:ebf16f5cbdec 65
Hjordan 1:ebf16f5cbdec 66 }
Hjordan 1:ebf16f5cbdec 67 }
Hjordan 1:ebf16f5cbdec 68
Hjordan 1:ebf16f5cbdec 69 int main(){
Hjordan 1:ebf16f5cbdec 70 sonarDistance = 0.0f;
Hjordan 1:ebf16f5cbdec 71 servoPosition = 0.0f;
Hjordan 1:ebf16f5cbdec 72
Hjordan 1:ebf16f5cbdec 73
Hjordan 1:ebf16f5cbdec 74 Thread sonarSensor_thread(sonarSensor);
Hjordan 1:ebf16f5cbdec 75 Thread servoControl_thread(servoControl);
Hjordan 1:ebf16f5cbdec 76 Thread logic_thread(logic);
Hjordan 1:ebf16f5cbdec 77 Thread display_thread(display);
Hjordan 1:ebf16f5cbdec 78
Hjordan 1:ebf16f5cbdec 79 while(true){
Hjordan 1:ebf16f5cbdec 80 }
Hjordan 1:ebf16f5cbdec 81 }