#ifndef __SERVO_H__
#define __SERVO_H__

#include "../common/def.h"

typedef struct _servo {
    volatile int8_t servoDiret;
    volatile uint16_t servoPos;
    volatile uint8_t servoEnabled;
} SERVO;

void servoInit(SERVO *servo);

void servoMove180(SERVO *servo);

#endif /* __SERVO_H__ */

// servo.c
#include "servo.h"

void servoInit(SERVO *servo) {
    servo->servoPos = 370;
    servo->servoDiret = 1;
    servo->servoEnabled = 0;
}

void servoMove180(SERVO *servo) {
    servo->servoPos += servo->servoDiret;
    
    if(servo->servoPos >= 635) {
        servo->servoPos = 635;
        servo->servoDiret = -1;
    }
    else if (servo->servoPos < 105) {
        servo->servoPos = 105;
        servo->servoDiret = 1;
    }
}