File content as of revision 2:7dc265489818:

#include "servo.h"

#define SERVO_FREQUENCY 80

Servo::Servo(void) {
    
    MyServoGui.uiMarkerPos = 0;
    uiDesiredPosition = 0;
    uiCurrentPosition = 0;
    Callib();
    
}

void Servo::GoTo(unsigned int uiPosition) {
    uiDesiredPosition = uiPosition % 360;
    eState = IN_PROGRESS;
    while(eState != IDLE) {
       // wait(1/SERVO_FREQUENCY);
        Automat();
    }
}

void Servo::Callib(void) {
    eState = CALLIB;
    while(eState != IDLE) {
       // wait(1/SERVO_FREQUENCY);
        Automat();
    }
    MyServoGui.uiMarkerStartPos = MyServoGui.uiMarkerPos;
    MyServoGui.uiMarkerPos = 0;
    uiCurrentPosition = 0;
    uiDesiredPosition = 0;
}

void Servo::Automat(void) {
    uiCurrentPosition = MyServoGui.uiMarkerPos;
    switch(eState) {
        case IN_PROGRESS:
            if(uiCurrentPosition < uiDesiredPosition) {
                MyServoGui.LedStepRight();
                eState = IN_PROGRESS;
            }
            else if(uiCurrentPosition > uiDesiredPosition){
                MyServoGui.LedStepLeft();
                eState = IN_PROGRESS;
            }
            else {
                eState = IDLE;
            }
        break;
        case IDLE:
            if (uiCurrentPosition != uiDesiredPosition) {
                eState = IN_PROGRESS;
            }
            else {
                eState = IDLE;
            }
        break;
        case CALLIB:
            if(MyServoGui.eReadDetector() == ACTIVE) {
                eState = IDLE;
            }
            else {
                MyServoGui.LedStepLeft();
                eState = CALLIB;
            }
        break;
    }
}