/* 
 * File:  servo.c
 * Project: Helmet.X
 * Author: Claire Chen and Mark Zhao
 */

#include "servo.h"

/**
 * Set up necessary hardware for servo. 
 * 
 * Timer 2 is reserved for servos, setup with a 20ms period
 * 
 * Servo Control Pin is set to RPA0 (pin2)
 */
void init_servo(void){
  //Setup timer 2 with a 20 ms period.
  OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_64, PERIOD_20_MS);

  OpenOC1(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
  PPSOutput(1,RPA0,OC1);
}

/**
 * Given a degree from 0 to 190, output the corresopnding signal to drive
 * the motor to that degree
 * 
 * @params: int degree - degree measurement from -90 to 90 degrees
 */
void turn_servo(int degrees){
  int timer_val;
  int deg;
  
  deg = degrees - 90; //normalize to -90 to 90
  if(deg > 90){
    deg = 90;
  }
  if (deg < -90){
    deg = -90;
  }
  
  //maps 90 degrees to -90 degrees to 680us to 2400us, since servo is not exact.
  timer_val = (int) (-(1080/180.0)*deg + 960); 
  //timer_val = degrees; //for testing
  
  //Entire period is 12500 or PERIOD_20_MS
  //We are running timer 2 at a 1:64 prescaler, each timer tick is 1.6us
  SetDCOC1PWM(timer_val);
}