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

#include "Servos.h"
#include "mcc_generated_files/pin_manager.h"
#include "mcc_generated_files/pwm1.h"
#include "mcc_generated_files/pwm2.h"
#include "mcc_generated_files/pwm3.h"
#include "mcc_generated_files/device_config.h"
#include "mcc_generated_files/mcc.h"
#include "Power.h"


void initServos()
{
  //Initialisation of PWMs with servo parameters (20ms, 1.5ms) is done in the PWM init
}

//Turns on power to the servos and starts the PWM peripheral
void startServos()
{
  //Skip if already on
  if (IO_SERVOS_GetValue()) return;
  
  setServo1(0); //POS2SERVO(SERVO1_HOME));
  setServo2(0); //POS2SERVO(SERVO2_HOME));
  IO_SERVOS_SetHigh();
  PWM1_Start();
  PWM2_Start();
}

//Turns off power to the servos and stops the PWM peripheral
void stopServos()
{
  //Skip if already off
  if (IO_SERVOS_GetValue()==0) return;
  
  //Put servos in the home position
  //homeServos();
  
  //Power off the PWM peripherals
  PWM1_Stop();
  PWM2_Stop();
  //Power off the servos
  IO_SERVOS_SetLow();
}

uint8_t isServoOn()
{
  return IO_SERVOS_GetValue();
}

void setServo1(int16_t pos)
{
  PWM1_DutyCycleSet((uint16_t)pos);
  PWM1_LoadBufferSet();
}
void setServo2(int16_t pos)
{
  PWM2_DutyCycleSet((uint16_t)pos);
  PWM2_LoadBufferSet();
}