AES TEAM / Mbed 2 deprecated AppliedOpSys

basic version

Dependencies:   C12832_lcd USBHost mbed

main.cpp@11:f4639eb91e5f, 2015-02-02 (annotated)

Committer:
cathal66
Date:
Mon Feb 02 13:34:05 2015 +0000
Revision:
11:f4639eb91e5f
Parent:
9:88d70b0c1b8b 
okl

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