/* lib_nav.c created from lib_rw11.c for DocNav project 11/25/99 by GL

/*
    VERSION HISTORY FOR lib_rw11.c

    V2.81Jun 11 1994jlj
    Created library for Rug Warrior from an earlier 
    library by jsr and fgm

*/

int abs(int arg)
 {if (arg<0)
   return -arg;
  else
   return arg;}

/*******************/
/* TIME PRIMITIVES */
/*******************/

/***********************************/
/* location of various time stuff: */
/* 0x14: time in milliseconds      */
/***********************************/

void reset_system_time()
{
  pokeword(0x14, 0);
  pokeword(0x12, 0);
}
    

/*  returns valid time from 0 to 32,767 minutes (approx)  */
float seconds()
{
  return ((float) mseconds()) / 1000.;
}

void sleep(float seconds)
{ 
    msleep((long)(seconds * 1000.));
}

void msleep(long msec)
{
    long end_time= mseconds() + msec;

    while (1) {
/* if the following test doesn't execute at least once a second,
   msleep may not halt */
long done= mseconds()-end_time;
if (done >= 0L && done <= 1000L) break;
    }
}

long timer_create_seconds(float timeout)
{
    return mseconds() + (long) (timeout * 1000.);
}

int timer_done(long timer)
{
    return timer < mseconds();
}

int analog(int port)
{   poke(0x1039, 0b10000000);
    poke(0x1030, port);
    return peek(0x1031);
}

/*****************************/
/*** Multi-Tasking Support ***/
/*****************************/

/*  gives process that calls it 256 ticks (over 1/4 sec)
    more to run before being swapped out

    call repeatedly to hog processor indefinitely  */
void hog_processor()
{
    poke(0x0a, 0);
}

int input_f(int port)/* Return 1 bit from portf PF1 or PF2 (or PF0) */
{ return 1 & (peek(0x7400) >> (port & 3));
}

int digital(int port) /* Return 1 bit from PA1 or PA2 (or PA0) */
{ return 1 & (peek(0x1000) >> (port & 3));
}

int round(float arg) /* thanks to George Musser for this one */
  {int integer;
   integer = (int)arg;
   arg -= (float)integer;
   if(arg >= 0.5)
      return integer+1;
   else if(arg <= -0.5)
      return integer-1;
   return integer;}  

/***********************************************************/
/*
/* Motor Control Primitives       
/*
/*    init_motors()- Must be called to enable motors
/*    motor(index, speed)- Control velocity of motor 0 (left) or 1 (right)
/*    drive(trans_vel, rot_vel)- Control robot translation and rotation
/*    stop()- Stop both motors
/*    note motor control revised to match Rug Warrior Pro version (int velocity) 
/* Setup two PWM channels for motor control */

/*                 Left        Right            */
int TOCx[2]     = {0x1018,     0x101A};/* Index for timer register */
int dir_mask[2] = {0b00010000, 0b00100000};/* Port D direction bits */
int pwm_mask[2] = {0b01000000, 0b00100000};/* Port A PWM bits */

int init_motors()
{ bit_set(0x1009,0b110000);/* Set PD4,5 as outputs for motor direction */
  poke(0x100C,0b01100000);/* OC1M Output compare 1 affects PA5,6 */
  bit_set(  0x1020,0b10100000);/* TCTL1 OC3 turns off PA5, OC2 PA6 */
  bit_clear(0x1020,0b01010000);/* Use set and clear to avoid other bits */
  pokeword(0x1016,0);/* When TCNT = 0, OC1 fires */
}

/* Make sure init_motors is called after a reset */

int init_motors_dummy = init_motors();

void stop()/* Stop both drive motors */
{ bit_clear(0x100D,pwm_mask[0]);/* Let OC1 turn off motors rather */
  bit_clear(0x100D,pwm_mask[1]);/* than turn them on */
}

/* Vel is in the range [-100, +100], index = 0 => Left, = 1 => Right */

void motor(int index, int vel)
{ int avel = vel;
  if (vel == 0) /* Check first for zero vel */
    { bit_clear(0x100D,pwm_mask[index]); /*  OC1 turns off the motor */
      return; } /* And, you're done */
  bit_set(0x100D,pwm_mask[index]); /* Otherwise let OC1 control the motors */
  if (vel > 0)
    { bit_set(0x1008, dir_mask[index]);} /* Forward rotation */
  else
    { avel = (- vel);
      bit_clear(0x1008, dir_mask[index]);} /* Backward rotation */
  if (avel > 99)
    pokeword(TOCx[index], 0);  /* 0 means OC1 wins, motor on */
  else
    pokeword(TOCx[index], (655 * avel));  /* Compute TOCx value */
}

/* Use drive to control motion of the robot.  A positive rot_vel makes the robot
   turn left. */

void drive(int trans_vel, int rot_vel)
{ motor(0,trans_vel - rot_vel);
  motor(1,trans_vel + rot_vel);
}

 /* WARNING:  printf's will wedge once the print buffer becomes full
    if system printing is disabled  */
void system_print_on() {bit_set(0x39, 0b00000001);}
void system_print_off() {bit_clear(0x39, 0b00000001);}
 

/********************** END *******************/