#include <stdio.h>    /* printf and other standard I/O routines */
#include <hc11.h>     /* define HC11 registers */
#include <vectors.h>  /* define interrupt vector array */
#include <isrdecl.h>  /* initialize unused interrupt service routines */
#include <serial.h>   /* initialize serial I/O to PC */
//#include <analog.h>
#include <motor5.h>
#include <misc.h>


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

void delay(int x) {
/* x == 5000 is ABOUT one second on an 8mhz 68HC11 */
  unsigned int i,z;
  for (i = 0; i < x; i++) {
    z = 65536;
    z += z/2;
    z /= z;
    z /= i;
    z -= z/2;
    z *= x;
  }
}

void directionTest() {
  int currDirection;
  
  /** Test DIRECTION register **/
  currDirection = 0x00;
  do {
    DIRECTION = currDirection; 
    printf("DIRECTION = %d\n", currDirection);
    printf("New value: ");
    currDirection = read_int();
  } while (1);
}

//void init_motor() {
//  PACTL = 0x88; /* PACTL=10001000: Set PA7 and PA3 for output, disable pulse accumulator */
//}

void motorTest() {

  int i, pwm;
    
  init_motor();
  DIRECTION = 0x80;  /* 10000000 */
 
  do {
//    printf("PORTA: %d\n", PORTA);
//    printf("New value: ");
//    i = read_int();
//    PORTA = i;
    motor(1, 30000);
    motor(2, 30000);
  } while (1);

}

void delayTest() {
  int x = 0;
  do {
    x++;
    printf("%d\n", x);
    delay(5000);
  } while(1);
}

void analogTest() {
  int i;
  CLEAR;
  do {
    HOME;
    for (i = 0; i < 8; i++)
      printf("PE%d: %d  \n", i, analog(i));
  } while(1);
}

void main() {

  int i,x,IR[2],currDirection;
  
  currDirection = 0xFF;

  INIT_SERIAL;
  INIT_ANALOG;
  IRE_ON;
  x = 0;
  
  CLEAR;
  
  printf("Press a button to start (check motor power)");
  do {
  } while (analog(0) < 10);
  
  analogTest();
//  delayTest();   
//  directionTest();
//  motorTest();

}