jordan harper / Mbed 2 deprecated ServoControl

Control servo motors

Dependencies:   Servo mbed

SystemFunctions.cpp@3:306d3725ed04, 2017-05-21 (annotated)

Committer:
jordaahh
Date:
Sun May 21 12:50:37 2017 +0000
Revision:
3:306d3725ed04
Parent:
2:baf8a7b2b150 
Individual Project Servo Control - University of Leeds

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jordaahh 3:306d3725ed04 1 #include "mbed.h"
jordaahh 3:306d3725ed04 2 #include "SystemFunctions.h"
jordaahh 3:306d3725ed04 3 #include "Servo.h"
jordaahh 3:306d3725ed04 4
jordaahh 0:b5ce567b469c 5 // Functions to control and operated the system
jordaahh 0:b5ce567b469c 6
jordaahh 3:306d3725ed04 7 Servo servo1(p21); // PWM out to servo in first axis (Solar Azimuth)
jordaahh 3:306d3725ed04 8 Servo servo2(p22); // PWM out to servo in second axis (Solar Altitude)
jordaahh 3:306d3725ed04 9
jordaahh 3:306d3725ed04 10 AnalogIn pot(p20); // Linear pot input for manual control [initial circuit]
jordaahh 3:306d3725ed04 11
jordaahh 3:306d3725ed04 12 AnalogIn LDR1(p15); // Top Left LDR
jordaahh 3:306d3725ed04 13 AnalogIn LDR2(p16); // Top Right LDR
jordaahh 3:306d3725ed04 14 AnalogIn LDR3(p17); // Bottom Left LDR
jordaahh 3:306d3725ed04 15 AnalogIn LDR4(p18); // Bottom Right LDR
jordaahh 3:306d3725ed04 16
jordaahh 3:306d3725ed04 17 Serial pc(USBTX,USBRX);
jordaahh 3:306d3725ed04 18
jordaahh 3:306d3725ed04 19 float range = 0.00095; // maximum on period set to 0.95ms
jordaahh 3:306d3725ed04 20
jordaahh 2:baf8a7b2b150 21
jordaahh 3:306d3725ed04 22 float LDR1Array[5];
jordaahh 3:306d3725ed04 23 float LDR2Array[5];
jordaahh 3:306d3725ed04 24 float LDR3Array[5];
jordaahh 3:306d3725ed04 25 float LDR4Array[5];
jordaahh 3:306d3725ed04 26
jordaahh 3:306d3725ed04 27 float Average1;
jordaahh 3:306d3725ed04 28 float Average2;
jordaahh 3:306d3725ed04 29 float Average3;
jordaahh 3:306d3725ed04 30 float Average4;
jordaahh 3:306d3725ed04 31
jordaahh 3:306d3725ed04 32 float OverallAverage;
jordaahh 3:306d3725ed04 33
jordaahh 3:306d3725ed04 34 int i;
jordaahh 3:306d3725ed04 35 int j;
jordaahh 3:306d3725ed04 36 int flag = 0;
jordaahh 3:306d3725ed04 37
jordaahh 3:306d3725ed04 38 float value1;
jordaahh 3:306d3725ed04 39 float value2;
jordaahh 3:306d3725ed04 40 float value3;
jordaahh 3:306d3725ed04 41 float value4;
jordaahh 3:306d3725ed04 42 float value5;
jordaahh 2:baf8a7b2b150 43
jordaahh 2:baf8a7b2b150 44
jordaahh 0:b5ce567b469c 45 void digitalFilter(){
jordaahh 2:baf8a7b2b150 46 i=0;
jordaahh 2:baf8a7b2b150 47 j=0;
jordaahh 2:baf8a7b2b150 48 for(i=0; i<5; i++){
jordaahh 3:306d3725ed04 49 LDR1Array[j] = LDR1;
jordaahh 3:306d3725ed04 50 LDR2Array[j] = LDR2;
jordaahh 3:306d3725ed04 51 LDR3Array[j] = LDR3;
jordaahh 3:306d3725ed04 52 LDR4Array[j] = LDR4;
jordaahh 2:baf8a7b2b150 53 j++;
jordaahh 2:baf8a7b2b150 54 }
jordaahh 3:306d3725ed04 55 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
jordaahh 3:306d3725ed04 56 Average2 = ((LDR2Array[4]*1)+(LDR2Array[3]*0.8)+(LDR2Array[2]*0.6)+(LDR2Array[1]*0.4)+(LDR2Array[0]*0.2))/3;
jordaahh 3:306d3725ed04 57 Average3 = ((LDR3Array[4]*1)+(LDR3Array[3]*0.8)+(LDR3Array[2]*0.6)+(LDR3Array[1]*0.4)+(LDR3Array[0]*0.2))/3;
jordaahh 3:306d3725ed04 58 Average4 = ((LDR4Array[4]*1)+(LDR4Array[3]*0.8)+(LDR4Array[2]*0.6)+(LDR4Array[1]*0.4)+(LDR4Array[0]*0.2))/3;
jordaahh 3:306d3725ed04 59
jordaahh 3:306d3725ed04 60 value1 = Average1;
jordaahh 3:306d3725ed04 61 value2 = Average2;
jordaahh 3:306d3725ed04 62 value3 = Average3;
jordaahh 3:306d3725ed04 63 value4 = Average4;
jordaahh 3:306d3725ed04 64 // 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 ]
jordaahh 2:baf8a7b2b150 65 }
jordaahh 0:b5ce567b469c 66
jordaahh 2:baf8a7b2b150 67 void servoMovement(){
jordaahh 3:306d3725ed04 68 OverallAverage = ( Average1 + Average2 + Average3 + Average4)/4;
jordaahh 3:306d3725ed04 69
jordaahh 3:306d3725ed04 70 if((OverallAverage - 0.03) > Average1){
jordaahh 3:306d3725ed04 71 servo1 = servo1 + 0.00275; // half degree at 0.00275? rotate 1 degree (0.0055) // needs rotation function. Anti clockwise servo 1.
jordaahh 3:306d3725ed04 72 servo2 = servo2 - 0.00275;
jordaahh 3:306d3725ed04 73 }
jordaahh 3:306d3725ed04 74 wait(0.007);
jordaahh 3:306d3725ed04 75
jordaahh 3:306d3725ed04 76 if((OverallAverage - 0.03) > Average2 ){
jordaahh 3:306d3725ed04 77 servo1 = servo1 - 0.00275;
jordaahh 3:306d3725ed04 78 servo2 = servo2 - 0.00275;
jordaahh 3:306d3725ed04 79 }
jordaahh 3:306d3725ed04 80 wait(0.007);
jordaahh 3:306d3725ed04 81
jordaahh 3:306d3725ed04 82 if((OverallAverage - 0.03) > Average3){
jordaahh 3:306d3725ed04 83 servo1 = servo1 + 0.00275;
jordaahh 3:306d3725ed04 84 servo2 = servo2 + 0.00275;
jordaahh 3:306d3725ed04 85 }
jordaahh 3:306d3725ed04 86 wait(0.007);
jordaahh 3:306d3725ed04 87
jordaahh 3:306d3725ed04 88 if((OverallAverage - 0.03) > Average4){
jordaahh 3:306d3725ed04 89 servo1 = servo1 - 0.00275;
jordaahh 3:306d3725ed04 90 servo2 = servo2 + 0.00275;
jordaahh 3:306d3725ed04 91
jordaahh 3:306d3725ed04 92 wait(0.007);
jordaahh 3:306d3725ed04 93 } else {
jordaahh 3:306d3725ed04 94 servo1 = servo1;
jordaahh 3:306d3725ed04 95 servo2 = servo2;
jordaahh 3:306d3725ed04 96 }
jordaahh 3:306d3725ed04 97
jordaahh 3:306d3725ed04 98
jordaahh 3:306d3725ed04 99 }
jordaahh 3:306d3725ed04 100
jordaahh 3:306d3725ed04 101 void positionTest(){
jordaahh 3:306d3725ed04 102 servo1 = 0.5;
jordaahh 3:306d3725ed04 103 servo2 = 0.5;
jordaahh 3:306d3725ed04 104 wait(1);
jordaahh 3:306d3725ed04 105 servo1 = 0.6;
jordaahh 3:306d3725ed04 106 servo2 = 0.6;
jordaahh 3:306d3725ed04 107 wait(1);
jordaahh 3:306d3725ed04 108 servo1 = 0.7;
jordaahh 3:306d3725ed04 109 servo2 = 0.7;
jordaahh 3:306d3725ed04 110 wait(1);
jordaahh 3:306d3725ed04 111 servo1 = 0.8;
jordaahh 3:306d3725ed04 112 servo2 = 0.8;
jordaahh 3:306d3725ed04 113 wait(1);
jordaahh 3:306d3725ed04 114 servo1 = 0.7;
jordaahh 3:306d3725ed04 115 servo2 = 0.7;
jordaahh 3:306d3725ed04 116 wait(1);
jordaahh 3:306d3725ed04 117 servo1 = 0.6;
jordaahh 3:306d3725ed04 118 servo2 = 0.6;
jordaahh 3:306d3725ed04 119 wait(1);
jordaahh 3:306d3725ed04 120 }
jordaahh 3:306d3725ed04 121
jordaahh 3:306d3725ed04 122
jordaahh 3:306d3725ed04 123 void serialRead(){
jordaahh 3:306d3725ed04 124 pc.baud(9600);
jordaahh 3:306d3725ed04 125 pc.printf(" %f %f %f %f \n",value1,value2,value3,value4);
jordaahh 3:306d3725ed04 126 wait(0.2);
jordaahh 3:306d3725ed04 127 }
jordaahh 3:306d3725ed04 128
jordaahh 3:306d3725ed04 129 void initialiseServo(){
jordaahh 3:306d3725ed04 130 servo1.calibrate(range, 180); // Function to set maximum on period (0.95ms) as 180 degrees.
jordaahh 3:306d3725ed04 131 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.
jordaahh 3:306d3725ed04 132 }
jordaahh 3:306d3725ed04 133