AES TEAM / Mbed 2 deprecated AppliedOpSys

basic version

Dependencies:   C12832_lcd USBHost mbed

main.cpp@9:88d70b0c1b8b, 2015-01-29 (annotated)

Committer:
cathal66
Date:
Thu Jan 29 16:18:19 2015 +0000
Revision:
9:88d70b0c1b8b
Parent:
8:b22ef9f44549
Child:
10:6059097698f7
Child:
11:f4639eb91e5f
PWM added to LED

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