File content as of revision 3:bebfc64cefe4:

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

#define sensorStartTreshold 0.07
#define sensorStopTreshold 0.02

#define calibrateFactorSenzA 1
#define calibrateFactorSenzB 0.97
#define calibrateFactorSenzC 1.06
#define calibrateFactorSenzD 0.9

Serial pc(USBTX, USBRX);

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 < 20; i++) {  
        SensA += ainSensA;
        SensB += ainSensB;
        SensC += ainSensC;
        SensD += ainSensD;
    }
    SensA /= 20;
    SensB /= 20;
    SensC /= 20;
    SensD /= 20;
        
    SensA *= calibrateFactorSenzA;
    SensB *= calibrateFactorSenzB;
    SensC *= calibrateFactorSenzC;
    SensD *= calibrateFactorSenzD;
        
    valAzimut = (SensA + SensB) - (SensC + SensD); 
    valElevacija = (SensB + SensC) - (SensA + SensD);
    
//    pc.printf("az:%6.3f el:%6.3f :: A:%.3f B:%.3f C:%.3f D:%.3f \n", 
//            valAzimut, valElevacija, SensA, SensB, SensC, SensD);
    pc.printf("az:%6.3f el:%6.3f \n", valAzimut, valElevacija);
}

int main() {

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

    while(1) {
    
        readValuesForAveraging();
        
        bool someWrokDone = false;
        
// ----- azimut -----
        
        if(!(*motorAz).isMoving() && (abs(valAzimut) > sensorStartTreshold)) {
            if(valAzimut > 0) 
                (*motorAz).movePositive();
            else
                (*motorAz).moveNegative();
            someWrokDone = true;
        }
        
        if((*motorAz).isMoving() && (abs(valAzimut) < sensorStopTreshold)) {
            (*motorAz).stop();
            someWrokDone = true;
        }
        
// ----- elevacija -----

        if(!(*motorEl).isMoving() && (abs(valElevacija) > sensorStartTreshold)) {
            if(valElevacija > 0) 
                (*motorEl).movePositive();
            else
                (*motorEl).moveNegative();
            someWrokDone = true;
        }
        
        if((*motorEl).isMoving() && (abs(valElevacija) < sensorStopTreshold)) {
            (*motorEl).stop();
            someWrokDone = true;
        }

// ---- ništa nije dirano ----

        if(!someWrokDone)
            wait_ms(500);
        
        
        
        
        /*
        if(valAzimut > sensorStartTreshold) {
            if(!(*motorAz).isMoving())
                (*motorAz).movePositive();
            continue;        
        }
        
        if(valAzimut < (-1 * sensorStartTreshold)) {
            if(!(*motorAz).isMoving())
                (*motorAz).moveNegative();
            continue;        
        }
        
        if((*motorAz).isMoving() && (abs(valAzimut) < sensorStopTreshold)) {
            (*motorAz).stop();
        }
        
        if(valElevacija > sensorStartTreshold) {
            if(!(*motorEl).isMoving())
                (*motorEl).movePositive();
            continue;        
        }
        
        if(valElevacija < (-1 * sensorStartTreshold)) {
            if(!(*motorEl).isMoving())
                (*motorEl).moveNegative();
            continue;        
        }
        
        if((*motorEl).isMoving() && (abs(valElevacija) < sensorStopTreshold)) {
            (*motorEl).stop();
        }
        */
        
 /*       
        if (valElevacija > sensorTreshold){ // positive azimuth deviation
            (*motorEl).movePositive();
            pc.printf("elevacija pozitiv \n");            
        }
        else if (valElevacija < (-1 * sensorTreshold)) { // negative azimuth deviation
            (*motorEl).moveNegative();
            pc.printf("elevacija negativ \n");            
        }
 */       
    }
}