#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 MAX_SPEED 28000

int currDirection;

void setDirection(int x) {
  currDirection = x;
  DIRECTION = x;
}

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 main() {

  int i,x;

  INIT_SERIAL;
  INIT_ANALOG;
  IRE_ON;
  x = 0;
  
  init_motor();
  setDirection(0xFF);
  
  CLEAR;
  HOME;

  do {
    
    printf("\nspeed up\n");
    for (i = 4000; i < MAX_SPEED; i = i + 1000) {
      motor(1, i);
      motor(2, i);   
      delay(200);
    }
    while (analog(2) < 95) {
      printf("cruising! %d  \r", analog(2));
    }
    
    printf("\nslow down\n");
    for (i = MAX_SPEED; i > 4000; i = i - 1000) {
      motor(1, i);
      motor(2, i);   
      delay(200);
    }
    
    printf("turning motors off\n");
    motor(1, 0);
    motor(2, 0);
    delay(5000);
    
    printf("change direction\n");
    if (currDirection == 0x00)
      setDirection(0xFF);
    else
      setDirection(0x00);
    while (analog(2) > 95) {
      printf("waiting! %d  \r", analog(2));
      delay(100);
    }
    
  } while(1);
  

}