// File: Servos.h
//------------------------------------------------------------------------------
// Author: Giovanni de Sanctis
// Email: info@lateral-technologies.com
//------------------------------------------------------------------------------
// Date: Dec 2018
// Controls the 2 servos
//------------------------------------------------------------------------------

#ifndef SERVOS_H
#define	SERVOS_H

#include <stdint.h>

#define K			2
//#define P0			(K*500) 
//#define P1			(K*850) 
//#define P2			(K*1050)
//#define P3			(K*1350)
//#define P4			(K*1500)
//#define P5			(K*1750)
//#define P6			(K*2050)
//#define P7			(K*2200)
//#define P9			(K*2500)

#define PL              0x0F                //Last position, don't update
#define P0              0 //(K*500)         //    0 degrees
#define P1              1 //(K*750)         // 22.5 degrees
#define P2              2 //(K*1000)        // 45.0 degrees
#define P3              3 //(K*1250)        // 67.5 degrees
#define P4              4 //(K*1500)        // 90.0 degrees
#define P5              5 //(K*1750)        //112.5 degrees
#define P6              6 //(K*2000)        //135 0 degrees
#define P7              7 //(K*2250)        //157.5 degrees
#define P8              8 //(K*2500)        //180.0 degrees

#define SERVO1_HOME     P8
#define SERVO2_HOME     P0
#define POS2SERVO(x) (int16_t)(((int16_t)K*550)+(int16_t)x*K*240)

#define T_PWM_SERVO 20
#define F_SERVO     50u                     //1/T_PwM_SERVO = 1/20ms = 50 Hz

//Initialise servos
void initServos();
//Powers the servos
void startServos();
//Turns off power to the servos
void stopServos();
//Return >0 if servo power is on
uint8_t isServoOn();

//Set servo to position pos (K*500 to K*2500)
void setServo1(int16_t pos);
void setServo2(int16_t pos);

#endif	/* SERVOS_H */