Revision 3:306d3725ed04, committed 2017-05-21

Comitter:

jordaahh

Date:

Sun May 21 12:50:37 2017 +0000

Parent:

2:baf8a7b2b150

Commit message:

Individual Project Servo Control - University of Leeds

Changed in this revision

--- a/SystemFunctions.cpp	Tue Feb 07 14:21:54 2017 +0000
+++ b/SystemFunctions.cpp	Sun May 21 12:50:37 2017 +0000
@@ -1,25 +1,133 @@
+#include "mbed.h"
+#include "SystemFunctions.h"
+#include "Servo.h"
+
 // Functions to control and operated the system 
 
-#include "SystemFunctions.h"
+Servo servo1(p21); // PWM out to servo in first axis (Solar Azimuth)
+Servo servo2(p22); // PWM out to servo in second axis (Solar Altitude) 
+
+AnalogIn pot(p20);  // Linear pot input for manual control [initial circuit] 
+
+AnalogIn LDR1(p15); // Top Left LDR
+AnalogIn LDR2(p16); // Top Right LDR
+AnalogIn LDR3(p17); // Bottom Left LDR
+AnalogIn LDR4(p18); // Bottom Right LDR
+
+Serial pc(USBTX,USBRX);
+
+float range = 0.00095; // maximum on period set to 0.95ms
+
 
+float LDR1Array[5];
+float LDR2Array[5];
+float LDR3Array[5];
+float LDR4Array[5];
+
+float Average1;
+float Average2;
+float Average3;
+float Average4;
+
+float OverallAverage;
+
+int i;
+int j;
+int flag = 0;
+
+float value1;
+float value2; 
+float value3; 
+float value4; 
+float value5; 
 
 
 void digitalFilter(){
     i=0;
     j=0;
     for(i=0; i<5; i++){
-        LDR1Array[j]=LDR1; 
-        LDR2Array[j]=LDR2;
-        LDR3Array[j]=LDR3;
-        LDR4Array[j]=LDR4;
+        LDR1Array[j] = LDR1; 
+        LDR2Array[j] = LDR2;
+        LDR3Array[j] = LDR3;
+        LDR4Array[j] = LDR4;
         j++;
         }
-        Average1 = ((LDR1Array[4]*1)+(LDR1Array[3]*0.8)+(LDR1Array[2]*0.6)+(LDR1Array[1]*0.4)+(LDR1Array[0]*0.2))/3; 
-        Average2 = ((LDR2Array[4]*1)+(LDR2Array[3]*0.8)+(LDR2Array[2]*0.6)+(LDR2Array[1]*0.4)+(LDR2Array[0]*0.2))/3; 
-        Average3 = ((LDR3Array[4]*1)+(LDR3Array[3]*0.8)+(LDR3Array[2]*0.6)+(LDR3Array[1]*0.4)+(LDR3Array[0]*0.2))/3; 
-        Average4 = ((LDR4Array[4]*1)+(LDR4Array[3]*0.8)+(LDR4Array[2]*0.6)+(LDR4Array[1]*0.4)+(LDR4Array[0]*0.2))/3; 
+         Average1 = ((LDR1Array[4]*1)+(LDR1Array[3]*0.8)+(LDR1Array[2]*0.6)+(LDR1Array[1]*0.4)+(LDR1Array[0]*0.2))/3; //digital filtering by prioritising the latests measurement
+         Average2 = ((LDR2Array[4]*1)+(LDR2Array[3]*0.8)+(LDR2Array[2]*0.6)+(LDR2Array[1]*0.4)+(LDR2Array[0]*0.2))/3; 
+         Average3 = ((LDR3Array[4]*1)+(LDR3Array[3]*0.8)+(LDR3Array[2]*0.6)+(LDR3Array[1]*0.4)+(LDR3Array[0]*0.2))/3; 
+         Average4 = ((LDR4Array[4]*1)+(LDR4Array[3]*0.8)+(LDR4Array[2]*0.6)+(LDR4Array[1]*0.4)+(LDR4Array[0]*0.2))/3; 
+     
+      value1 = Average1;
+      value2 = Average2;
+      value3 = Average3;
+      value4 = Average4;
+      // Sends each value stored in the array to a variable. After Testing it shows that the values done't vary a lot. also uses 5 at a time sliding window effect [ 1 2 3 4 5 ] -> [ 6 7 8 9 10 ] and not [ 1 2 3 4 5 ] -> [ 2 3 4 5 6 ]   
     }
     
 void servoMovement(){
-    OverallAverage = (Average1 + Average2 + Average3 + Average4)/4;
-    }
\ No newline at end of file
+    OverallAverage = ( Average1 + Average2 + Average3 + Average4)/4;
+    
+    if((OverallAverage - 0.03) > Average1){
+        servo1 = servo1 + 0.00275; // half degree at 0.00275? rotate 1 degree (0.0055) // needs rotation function. Anti clockwise servo 1. 
+        servo2 = servo2 - 0.00275;
+        }
+        wait(0.007);
+        
+        if((OverallAverage - 0.03) > Average2 ){
+            servo1 = servo1 - 0.00275;
+            servo2 = servo2 - 0.00275;     
+        } 
+        wait(0.007);
+    
+        if((OverallAverage - 0.03) > Average3){
+            servo1 = servo1 + 0.00275;
+            servo2 = servo2 + 0.00275;
+        } 
+        wait(0.007);
+        
+        if((OverallAverage - 0.03) > Average4){
+            servo1 = servo1 - 0.00275;
+            servo2 = servo2 + 0.00275;
+        
+        wait(0.007);
+       } else { 
+        servo1 = servo1;
+        servo2 = servo2;
+        }
+        
+        
+    }
+
+void positionTest(){
+   servo1 = 0.5;
+   servo2 = 0.5;
+   wait(1);
+   servo1 = 0.6;
+   servo2 = 0.6;
+   wait(1);
+   servo1 = 0.7;
+   servo2 = 0.7;
+   wait(1);
+   servo1 = 0.8;
+   servo2 = 0.8;
+   wait(1);
+   servo1 = 0.7;
+   servo2 = 0.7;
+   wait(1);
+   servo1 = 0.6;
+   servo2 = 0.6;
+   wait(1);
+   }
+   
+    
+void serialRead(){
+        pc.baud(9600);     
+        pc.printf(" %f  %f  %f  %f  \n",value1,value2,value3,value4);
+        wait(0.2);
+    }
+    
+void initialiseServo(){
+    servo1.calibrate(range, 180); // Function to set maximum on period (0.95ms) as 180 degrees. 
+    servo2.calibrate(range, 180); // Any float value given to the servo is taken as a percentage of the maximum on period (0.95ms) of PWM signal. i.e. servo1 = 0.5 --> This is 50% of 0.95ms which will move the servo to 90 degrees. 
+    }
+    
\ No newline at end of file

--- a/SystemFunctions.h	Tue Feb 07 14:21:54 2017 +0000
+++ b/SystemFunctions.h	Sun May 21 12:50:37 2017 +0000
@@ -1,21 +1,42 @@
-
-
-float range = 0.00095; // maximum on period set to 0.95ms
-float position; //= 0.5; // set initial position to middle // can probably take this out??
+/**
+@file SystemFunctions.h
+@brief Header file containing functions prototypes.
+@author Jordan Harper
+@date   March 2017
+*/
 
-float LDR1Array[5];
-float LDR2Array[5];
-float LDR3Array[5];
-float LDR4Array[5];
+/* functions */
+
+/** Sets signal rate for serial connection to PC and displays string of measured analog signals. 
+@param pc.baud() - sets signal freq.
+@param pc.printf() - prints string to PC (CoolTerm).
+@returns Float values measured from each LDR. 
+*/
+void serialRead();
 
-float Average1;
-float Average2;
-float Average3;
-float Average4;
+/** Records analog data from each LDR, places it in an array and gives array values weights based on how new the measurement is (organises them by significance to get accurate average signal). 
+@param i - counter variable. 
+@param j - counter variable. 
+@param Average1 - Average value for LDR1.  
+@param Average2 - Average value for LDR2.  
+@param Average3 - Average value for LDR3.  
+@param Average4 - Average value for LDR4.   
+@returns Average value for each LDR. 
+*/
+void digitalFilter();
 
-float OverallAverage;
+/** Moves the servo based on the signals measured from LDR's. 
+@param OverallAverage - Base value of LDR signals.
+@param Servo1 - signal sent to Servo 1 in analog form.
+@param Servo2 - signal sent to Servo 2 in analog form.  
+*/
+void servoMovement();
 
-int i;
-int j;
+/** Calibrates PWM signal. 
+@param servo.calibrate() - sets range of movement in degrees for Pulse Width value.   
+*/
+void initialiseServo();
 
-void digitalFilter();
+/** Automatically moves servos to test the analog signal conversion to PWM.   
+*/
+void positionTest();
\ No newline at end of file

--- a/main.cpp	Tue Feb 07 14:21:54 2017 +0000
+++ b/main.cpp	Sun May 21 12:50:37 2017 +0000
@@ -1,28 +1,20 @@
-// Hello World to sweep a servo through its full range
+/**
+@file main.cpp
+
+@brief Servo Control Algorithm implementation
+ */
 
 #include "mbed.h"
 #include "Servo.h"
 #include "SystemFunctions.h"
 
-Servo servo1(p21); // PWM out to servo in first axis (Solar Azimuth)
-Servo servo2(p22); // PWM out to servo in second axis (Solar Altitude) 
-
-AnalogIn pot(p20);  // Linear pot input for manual control [initial circuit] 
-
-AnalogIn LDR1(p15); // Top Left LDR
-AnalogIn LDR2(p16); // Top Right LDR
-AnalogIn LDR3(p17); // Bottom Left LDR
-AnalogIn LDR4(p18); // Bottom Right LDR
-
-int main() {    
-
-    servo1.calibrate(range, 45.0); // *****check this fucntion******
-    
+int main() {
+    initialiseServo();
+   // positionTest(); // Uncomment to test servomotor movement.
     while(1){
-        // initial code for testing one servo with a linear potentiometer.  
-        position = pot; 
-        wait_ms(0.5);
-        servo1 = position;
-        }
+    digitalFilter();
+    servoMovement();
+    //serialRead(); // Uncomment to use serial communication with PC to log analog data. (CoolTerm)
+    }
 }