AES TEAM / Mbed 2 deprecated AppliedOpSys

basic version

Dependencies:   C12832_lcd USBHost mbed

main.cpp@12:309dc0947373, 2015-02-05 (annotated)

Committer:
cathal66
Date:
Thu Feb 05 17:05:31 2015 +0000
Revision:
12:309dc0947373
Parent:
10:6059097698f7 
Deleted the second folder of the rtos; Harley fixed the problem with the storage of the data;

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