Milan Draganic / Mbed 2 deprecated SunflowerMach1

motor i senzor

Dependencies:   TextLCD mbed PinDetect

main.cpp@2:0bf41ad96558, 2013-11-09 (annotated)

Committer:
mdraganic
Date:
Sat Nov 09 07:04:13 2013 +0000
Revision:
2:0bf41ad96558
Parent:
1:3500bf8487d0
Child:
3:bebfc64cefe4
added sensor averageing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mdraganic 0:7447b8021b33 1 #include "mbed.h"
mdraganic 0:7447b8021b33 2 #include "Motor.h"
mdraganic 1:3500bf8487d0 3 #include "MotorPwm.h"
mdraganic 0:7447b8021b33 4
mdraganic 0:7447b8021b33 5 AnalogIn ainSensA(p17);
mdraganic 0:7447b8021b33 6 AnalogIn ainSensB(p18);
mdraganic 0:7447b8021b33 7 AnalogIn ainSensC(p19);
mdraganic 0:7447b8021b33 8 AnalogIn ainSensD(p20);
mdraganic 0:7447b8021b33 9
mdraganic 2:0bf41ad96558 10 float valAzimut = 0;
mdraganic 2:0bf41ad96558 11 float valElevacija = 0;
mdraganic 2:0bf41ad96558 12
mdraganic 2:0bf41ad96558 13 float SensA, SensB, SensC, SensD;
mdraganic 2:0bf41ad96558 14 void readValuesForAveraging() {
mdraganic 2:0bf41ad96558 15
mdraganic 2:0bf41ad96558 16 SensA = 0;
mdraganic 2:0bf41ad96558 17 SensB = 0;
mdraganic 2:0bf41ad96558 18 SensC = 0;
mdraganic 2:0bf41ad96558 19 SensD = 0;
mdraganic 2:0bf41ad96558 20
mdraganic 2:0bf41ad96558 21 for (int i = 0; i <= 9; i++) {
mdraganic 2:0bf41ad96558 22 SensA += ainSensA;
mdraganic 2:0bf41ad96558 23 SensB += ainSensB;
mdraganic 2:0bf41ad96558 24 SensC += ainSensC;
mdraganic 2:0bf41ad96558 25 SensD += ainSensD;
mdraganic 2:0bf41ad96558 26 }
mdraganic 2:0bf41ad96558 27 SensA /= 10;
mdraganic 2:0bf41ad96558 28 SensB /= 10;
mdraganic 2:0bf41ad96558 29 SensC /= 10;
mdraganic 2:0bf41ad96558 30 SensD /= 10;
mdraganic 2:0bf41ad96558 31
mdraganic 2:0bf41ad96558 32 valAzimut = (SensA + SensB) - (SensC + SensD);
mdraganic 2:0bf41ad96558 33 valElevacija = (SensB + SensC) - (SensA + SensD);
mdraganic 2:0bf41ad96558 34 }
mdraganic 0:7447b8021b33 35
mdraganic 0:7447b8021b33 36 int main() {
mdraganic 0:7447b8021b33 37
mdraganic 1:3500bf8487d0 38 Motor *motorEl = new MotorPwm(p25, p26);
mdraganic 1:3500bf8487d0 39 Motor *motorAz = new MotorPwm(p23, p24);
mdraganic 0:7447b8021b33 40
mdraganic 0:7447b8021b33 41 while(1) {
mdraganic 0:7447b8021b33 42
mdraganic 2:0bf41ad96558 43 readValuesForAveraging();
mdraganic 0:7447b8021b33 44
mdraganic 2:0bf41ad96558 45 if (valAzimut > 0.2){ // positive azimuth deviation
mdraganic 2:0bf41ad96558 46 (*motorAz).movePositive();
mdraganic 2:0bf41ad96558 47 }
mdraganic 2:0bf41ad96558 48 else if (valAzimut < -0.2) { // negative azimuth deviation
mdraganic 2:0bf41ad96558 49 (*motorAz).moveNegative();
mdraganic 2:0bf41ad96558 50 }
mdraganic 2:0bf41ad96558 51 (*motorAz).stop();
mdraganic 2:0bf41ad96558 52
mdraganic 2:0bf41ad96558 53 if (valElevacija > 0.2){ // positive azimuth deviation
mdraganic 2:0bf41ad96558 54 (*motorEl).movePositive();
mdraganic 2:0bf41ad96558 55 }
mdraganic 2:0bf41ad96558 56 else if (valElevacija < -0.2) { // negative azimuth deviation
mdraganic 2:0bf41ad96558 57 (*motorEl).moveNegative();
mdraganic 2:0bf41ad96558 58 }
mdraganic 2:0bf41ad96558 59 (*motorEl).stop();
mdraganic 0:7447b8021b33 60 }
mdraganic 0:7447b8021b33 61 }
mdraganic 2:0bf41ad96558 62
mdraganic 2:0bf41ad96558 63