//#include <stdio.h>
#include <vectors.h>
#include <serial.h>
#include <isrdecl.h>  /* initialize interrupts */
#include <servome.h>

#define INIT_ANALOG SET_BIT(OPTION,0x80)

#define SERVOS_ON SET_BIT(TMSK1,0x10)             

/*Disable OC4 interrupt: Stops all servo holding torques, useful for energy savings*/
#define SERVOS_OFF CLEAR_BIT(TMSK1, 0x10)

#define IRE_ON  *(unsigned char *)(0x7000) = 0x07
#define IRE_OFF	*(unsigned char *)(0x7000) = 0x00

/* VT100 clear screen */ 
#define CLEAR_SCREEN  printf("\x1b\x5B\x32\x4A\x04")

/* VT100 position cursor at (x,y) = (3,12) command is "\x1b[3;12H"*/
#define HOME_POSITION   printf("\x1b[1;1H")

#define TRUE 1
#define FALSE 0

unsigned char analog(unsigned char port) {
    ADCTL=port;		/* Address the selected channel */
    while((ADCTL & 0x80) != 0x80); /* Wait for A/D to finish */
    return(ADR1);		/* Return analog value */
}

void main(int k, char** argv) {
  int i, servo_index, PW;
 
  INIT_ANALOG;

  init_servome();

  INIT_SERIAL;

LOOP:
  CLEAR_SCREEN;
  HOME_POSITION;
  
  for(i = 0; i < NSERVO; i++) {
    printf("PW on PA%d = %u\n", (i+3), servo_width[i]);
  }
  
  printf("\nServo index (0-4 ==> PA3-PA7): ");
  servo_index = read_int();

  printf("Pulse Width: ");

  PW = read_int();
  servo(servo_index, PW);
goto LOOP;
}