#include "system_sam3x.h"
#include "at91sam3x8.h"
#include "include.c"


/* 
* SERVO INITERING
* Aktiverar servon på Channel ID 1
*/

void initPWM(){
  *AT91C_PMC_PCER = (0x1<<(AT91C_ID_PIOB));                           //PMC -> PIOB
  *AT91C_PMC_PCER1 = 1<<4;                                            //PCM -> PWM
  *AT91C_PIOB_PER = (1 << 17);                                        //Enable PIN 17

  *AT91C_PIOB_PDR = AT91C_PIO_PB17;                                    //   Let peripheral control the pin
  *AT91C_PIOB_ABMR = AT91C_PIO_PB17;                                   //   Activate peripheral B to control the pin
  
  *AT91C_PWMC_ENA = AT91C_PWMC_CHID1;                                  //Enable CHID1
  *AT91C_PWMC_CH1_CMR = 0x5;                                           //Master/32 (0b0101)
  
  *AT91C_PWMC_CH1_CPRDR = 52500;                                       //Beräknat till 52500
  *AT91C_PWMC_CH1_CDTYR = 1650;
}

/* 
* SERVO SET POSITION
* Ändrar position på servon
* Ex för användning
* servo_setPos(1500);
* 
* Standard värden: 
*             min:  1700 (ca 40 grader)
*             max:  5700 (ca 130 grader)
*/

void servo_setPos(int x){
  *AT91C_PWMC_CH1_CDTYR = x;
}

/* 
* SERVO GET POSITION
* Retunerar värdet från servons position.
* För att få ut grader multiplikationen: 5700/x = 130 => x = 5700/130 = 43.7 (avrundat till 44)
* För att printa ut graderna
* printf("%d", (servo_getPos() / 44)).
*/

int servo_getPos(){
  return *AT91C_PWMC_CH1_CDTYR;
}