AES TEAM / Mbed 2 deprecated AppliedOpSys

basic version

Dependencies:   C12832_lcd USBHost mbed

main.cpp@7:0fa7430ab812, 2015-01-26 (annotated)

Committer:
cathal66
Date:
Mon Jan 26 14:58:21 2015 +0000
Revision:
7:0fa7430ab812
Parent:
6:4c62f9c91b1d
Child:
8:b22ef9f44549
LED meter added; Adjusted the 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
cathal66 7:0fa7430ab812 5 //Varible
Hjordan 1:ebf16f5cbdec 6 float sonarDistance;
Hjordan 1:ebf16f5cbdec 7 float servoPosition;
Hjordan 1:ebf16f5cbdec 8
cathal66 7:0fa7430ab812 9 //Mutex
cathal66 6:4c62f9c91b1d 10 Mutex sonarDistance_mutex; //Init Mutex for sensor value
cathal66 6:4c62f9c91b1d 11 Mutex servoPosition_mutex; //Init Mutex for servo value
cathal66 7:0fa7430ab812 12 Mutex LED_mutex;
cathal66 7:0fa7430ab812 13
cathal66 7:0fa7430ab812 14 //I/O
cathal66 6:4c62f9c91b1d 15 AnalogIn sonarPin(p17); //Assign pin to read sonar sensor
cathal66 6:4c62f9c91b1d 16 PwmOut servoPin(p21); //Assign pin for servo output
cathal66 7:0fa7430ab812 17 DigitalOut myled1(LED1);
cathal66 7:0fa7430ab812 18 DigitalOut myled2(LED2);
cathal66 7:0fa7430ab812 19 DigitalOut myled3(LED3);
cathal66 7:0fa7430ab812 20 DigitalOut myled4(LED4);
Hjordan 1:ebf16f5cbdec 21
cathal66 7:0fa7430ab812 22 //LCD Setup
cathal66 6:4c62f9c91b1d 23 C12832_LCD lcd; //setup LCD screen
Hjordan 1:ebf16f5cbdec 24
Hjordan 1:ebf16f5cbdec 25 void sonarSensor(void const *args){
Hjordan 1:ebf16f5cbdec 26 while(true){
cathal66 6:4c62f9c91b1d 27 sonarDistance_mutex.lock(); //Mutex lock for the Servo value
cathal66 6:4c62f9c91b1d 28 sonarDistance = sonarPin.read(); //Read analog voltage and store in variable
cathal66 6:4c62f9c91b1d 29 sonarDistance_mutex.unlock(); //Mutex unlock for the Servo value
cathal66 6:4c62f9c91b1d 30 Thread::wait(10); //Pause thread for 10msec
cathal66 6:4c62f9c91b1d 31 } //End of While loop
cathal66 6:4c62f9c91b1d 32 } //End of Thread
Hjordan 1:ebf16f5cbdec 33
Hjordan 1:ebf16f5cbdec 34 void servoControl(void const *args){
Hjordan 1:ebf16f5cbdec 35 while(true){
cathal66 6:4c62f9c91b1d 36 servoPosition_mutex.lock(); //Mutex lock for the Servo value
cathal66 6:4c62f9c91b1d 37 servoPin.write(servoPosition); //Write servo value to servo pin
cathal66 6:4c62f9c91b1d 38 servoPosition_mutex.unlock(); //Mutex unlock for the Servo value
cathal66 6:4c62f9c91b1d 39 } //end of while loop
cathal66 6:4c62f9c91b1d 40 } //end of thread
Hjordan 1:ebf16f5cbdec 41
cathal66 6:4c62f9c91b1d 42 void logic(void const *args)
cathal66 6:4c62f9c91b1d 43 {
cathal66 7:0fa7430ab812 44 int Size=12;
cathal66 7:0fa7430ab812 45 float Average_4[12]; //number of value in the array
cathal66 7:0fa7430ab812 46 float Average_Sum=0;
cathal66 6:4c62f9c91b1d 47 while(true)
cathal66 6:4c62f9c91b1d 48 {
cathal66 7:0fa7430ab812 49
cathal66 7:0fa7430ab812 50 for(int i=0;i<Size;i++)
cathal66 6:4c62f9c91b1d 51 {
cathal66 6:4c62f9c91b1d 52 Average_Sum = Average_Sum-Average_4[i]; //Remove the 4th oldest value from the average sum
cathal66 7:0fa7430ab812 53
cathal66 7:0fa7430ab812 54 sonarDistance_mutex.lock(); //Mutex lock for the Sonar value
cathal66 6:4c62f9c91b1d 55 Average_4[i]= sonarDistance; //Add the new value to the array
cathal66 7:0fa7430ab812 56 sonarDistance_mutex.unlock(); //Mutex unlock for the Sonar value
cathal66 7:0fa7430ab812 57
cathal66 6:4c62f9c91b1d 58 Average_Sum = Average_Sum + Average_4[i]; //Add the new array value to the sum
cathal66 7:0fa7430ab812 59
cathal66 7:0fa7430ab812 60 servoPosition_mutex.lock(); //Mutex lock for the servo value
cathal66 7:0fa7430ab812 61 servoPosition = Average_Sum/Size; //Divide the array by the number of element in the array
cathal66 7:0fa7430ab812 62 servoPosition_mutex.unlock(); //Mutex unlock for the servo value
cathal66 7:0fa7430ab812 63
cathal66 6:4c62f9c91b1d 64 }//end for loop
cathal66 6:4c62f9c91b1d 65
cathal66 6:4c62f9c91b1d 66 }//end of while loop
cathal66 6:4c62f9c91b1d 67 }//End of thread
Hjordan 1:ebf16f5cbdec 68
Hjordan 1:ebf16f5cbdec 69 void display(void const *args){
Hjordan 1:ebf16f5cbdec 70 while(true){
Hjordan 1:ebf16f5cbdec 71 sonarDistance_mutex.lock();
Hjordan 1:ebf16f5cbdec 72 servoPosition_mutex.lock();
Hjordan 1:ebf16f5cbdec 73 lcd.cls();
Hjordan 1:ebf16f5cbdec 74 lcd.locate(0,0);
Hjordan 3:be269540df58 75 lcd.printf("Sonar : %3.2f \nServo : %3.2f \nSCIENCE!",sonarDistance,servoPosition);
Hjordan 1:ebf16f5cbdec 76 sonarDistance_mutex.unlock();
Hjordan 1:ebf16f5cbdec 77 servoPosition_mutex.unlock();
Hjordan 1:ebf16f5cbdec 78 Thread::wait(200);
Hjordan 1:ebf16f5cbdec 79
Hjordan 1:ebf16f5cbdec 80 }
Hjordan 1:ebf16f5cbdec 81 }
Hjordan 1:ebf16f5cbdec 82
cathal66 7:0fa7430ab812 83 void LED_meter(void const *args) {
cathal66 7:0fa7430ab812 84
cathal66 7:0fa7430ab812 85 float LED_distance = 0;
cathal66 7:0fa7430ab812 86
cathal66 7:0fa7430ab812 87 while(true) {
cathal66 7:0fa7430ab812 88 sonarDistance_mutex.lock();
cathal66 7:0fa7430ab812 89 LED_distance=sonarDistance;
cathal66 7:0fa7430ab812 90 sonarDistance_mutex.unlock();
cathal66 7:0fa7430ab812 91
cathal66 7:0fa7430ab812 92 if(LED_distance<=0.2)
cathal66 7:0fa7430ab812 93 {
cathal66 7:0fa7430ab812 94 myled1 = 1;
cathal66 7:0fa7430ab812 95 }
cathal66 7:0fa7430ab812 96 else
cathal66 7:0fa7430ab812 97 {
cathal66 7:0fa7430ab812 98 myled1 = 0;
cathal66 7:0fa7430ab812 99 }
cathal66 7:0fa7430ab812 100
cathal66 7:0fa7430ab812 101 if(LED_distance<=0.15)
cathal66 7:0fa7430ab812 102 {
cathal66 7:0fa7430ab812 103 myled2 = 1;
cathal66 7:0fa7430ab812 104 }
cathal66 7:0fa7430ab812 105 else
cathal66 7:0fa7430ab812 106 {
cathal66 7:0fa7430ab812 107 myled2 = 0;
cathal66 7:0fa7430ab812 108 }
cathal66 7:0fa7430ab812 109
cathal66 7:0fa7430ab812 110 if(LED_distance<=0.1)
cathal66 7:0fa7430ab812 111 {
cathal66 7:0fa7430ab812 112 myled3 = 1;
cathal66 7:0fa7430ab812 113 }
cathal66 7:0fa7430ab812 114 else
cathal66 7:0fa7430ab812 115 {
cathal66 7:0fa7430ab812 116 myled3 = 0;
cathal66 7:0fa7430ab812 117 }
cathal66 7:0fa7430ab812 118
cathal66 7:0fa7430ab812 119 if(LED_distance<=0.05)
cathal66 7:0fa7430ab812 120 {
cathal66 7:0fa7430ab812 121 myled4 = 1;
cathal66 7:0fa7430ab812 122 //s2=0.1;
cathal66 7:0fa7430ab812 123 }
cathal66 7:0fa7430ab812 124 else
cathal66 7:0fa7430ab812 125 {
cathal66 7:0fa7430ab812 126 myled4 = 0;
cathal66 7:0fa7430ab812 127 //s2=1;
cathal66 7:0fa7430ab812 128 }
cathal66 7:0fa7430ab812 129 }
cathal66 7:0fa7430ab812 130 }
cathal66 7:0fa7430ab812 131
Hjordan 1:ebf16f5cbdec 132 int main(){
Hjordan 1:ebf16f5cbdec 133 sonarDistance = 0.0f;
Hjordan 1:ebf16f5cbdec 134 servoPosition = 0.0f;
Hjordan 1:ebf16f5cbdec 135
Hjordan 1:ebf16f5cbdec 136
Hjordan 1:ebf16f5cbdec 137 Thread sonarSensor_thread(sonarSensor);
Hjordan 1:ebf16f5cbdec 138 Thread servoControl_thread(servoControl);
Hjordan 1:ebf16f5cbdec 139 Thread logic_thread(logic);
Hjordan 1:ebf16f5cbdec 140 Thread display_thread(display);
cathal66 7:0fa7430ab812 141 Thread LED_meter_thread(LED_meter);
Hjordan 1:ebf16f5cbdec 142
cathal66 6:4c62f9c91b1d 143 while(true)
cathal66 6:4c62f9c91b1d 144 {
Hjordan 1:ebf16f5cbdec 145 }
Hjordan 1:ebf16f5cbdec 146 }