inspektors/robot/include/servos.h

#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 STOP +- 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 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_full_forw(void);
void servo_turn_forw_r(void);
void servo_turn_forw_l(void);
void servo_full_backw(void);
void servo_turn_backw_r(void);
void servo_turn_backw_l(void);


#endif /* SERVOS_H */