// intshaft.cpp
//
// A simple main() to test the possibility of reading the shaft encoders
// from inside a timer handler. -Dave Hershberger

#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include "timer.h"
#include "motors.h"

Motors motors;

float x, y, theta;

extern char *IN_DOS;

int num_in_dos = 0;
int num_ticks = 0;
int in_dos_was;

void every_tick(double junk)
{
  if(*IN_DOS)
    num_in_dos++;
  num_ticks++;
  in_dos_was = *IN_DOS;
}

main()
{
  motors.init();
  motors.home();

  timer->add_callback(every_tick, 1, 0);

  while(!kbhit()){
    for(int i = 0; i < 10000000; i++);
    printf("num_in_dos = %d, num_ticks = %d, in_dos_was = %d\n",
	   num_in_dos, num_ticks, in_dos_was);
//    printf("x = %g, y = %g, theta = %g\n", x, y, theta);
  }

  timer->delete_callback(every_tick, 1);

  motors.halt();
}