#ifndef SERVOS_H
#define SERVOS_H

#define SERVO_DDR         DDRD
#define SERVO_L_REG       OCR0A
#define SERVO_R_REG       OCR0B
#define SERVO_L_BIT       PD6
#define SERVO_R_BIT       PD5

// Not just MID +- 500us because of rounding errors (e.g. 19.9 will become
// 19 and not 20 because of integer division)
#define SERVO_MID         1381UL
#define SERVO_MAX         1883UL
#define SERVO_MIN         880UL

#define SERVO_L_STOP      SERVO_MID
#define SERVO_L_FORW      SERVO_MAX
#define SERVO_L_BACKW     SERVO_MIN

#define SERVO_R_STOP      SERVO_MID
#define SERVO_R_FORW      SERVO_MIN
#define SERVO_R_BACKW     SERVO_MAX

#define SERVO_DIR_BACKW   0
#define SERVO_DIR_FORW    1

#define SERVO_90_TURN_MS  1000
#define SERVO_CM_PER_S    15 // Distance in cm that servos travel in 1s

#define PRESCALER         1024UL
#define PWM_PERIOD_MS     (uint16_t)((256000UL * PRESCALER) / F_CPU)
#define US2TIMER0(us)     (uint16_t)((51U * us) / (200U * PWM_PERIOD_MS))

void init_servos(void);
void servo_speed(uint8_t direction, uint8_t load_percent_l, uint8_t load_percent_r);

#endif /* SERVOS_H */