File content as of revision 2:0bf41ad96558:

#include "mbed.h"
#include "Motor.h"
#include "MotorPwm.h"

AnalogIn ainSensA(p17);
AnalogIn ainSensB(p18);
AnalogIn ainSensC(p19);
AnalogIn ainSensD(p20);

float valAzimut = 0;
float valElevacija = 0;

float SensA, SensB, SensC, SensD;
void readValuesForAveraging() {

    SensA = 0;
    SensB = 0;
    SensC = 0;
    SensD = 0;
    
    for (int i = 0; i <= 9; i++) {  
        SensA += ainSensA;
        SensB += ainSensB;
        SensC += ainSensC;
        SensD += ainSensD;
    }
    SensA /= 10;
    SensB /= 10;
    SensC /= 10;
    SensD /= 10;
        
    valAzimut = (SensA + SensB) - (SensC + SensD); 
    valElevacija = (SensB + SensC) - (SensA + SensD);
}

int main() {

    Motor *motorEl = new MotorPwm(p25, p26);
    Motor *motorAz = new MotorPwm(p23, p24);

    while(1) {
    
        readValuesForAveraging();
        
        if (valAzimut > 0.2){ // positive azimuth deviation
            (*motorAz).movePositive();
        }
        else if (valAzimut < -0.2) { // negative azimuth deviation
            (*motorAz).moveNegative();
        }
        (*motorAz).stop(); 
        
        if (valElevacija > 0.2){ // positive azimuth deviation
            (*motorEl).movePositive();
        }
        else if (valElevacija < -0.2) { // negative azimuth deviation
            (*motorEl).moveNegative();
        }
        (*motorEl).stop(); 
    }
}