/**
 * requires 
 *     load servo.icb
 *     load servo.c
 *     load compass.icb 
 *     load expservo.icb    expansion board servo, for sonar
 *     load xencrd0.icb     right wheel  s encdr works fine 
 *     load xencdr6.icb    left wheel   s=slow, f=fast
 */

float strip_dist = 0.85;  /* inches */
float cnt_dist   = 0.424; /* inches  (pi*d / cnt) 6.785840016 / 16 */
float wheel_c2c  = 6.05;  /* inches, wheel center to center */
float wheel_dia  = 2.16;  /* inches */
int   stripes_wh = 8;     /* stripes on a wheel */
int   cnt_per_rev= 16;    /* verified by one_rev() */

int speed=100;

int   right_wheel= 0;
int   left_wheel = 1;


/*****************************************************************************/

int STRIPES_PER_TURN   = 45;      /* Transitions per full rotation of robot */
int FULL_TURN          = STRIPES_PER_TURN << 1;       /* Half-transitions */
                                                      /* per 2pi radians   */
int HALF_TURN          = STRIPES_PER_TURN;            /* Per pi radians */
int QUARTER_TURN       = STRIPES_PER_TURN >> 1;       /* Per pi/2 radians */
int THREE_QUARTER_TURN = 3 * STRIPES_PER_TURN >> 1;   /* Per 3pi/2 radians */


/*
 * Set the handyboard to listen to the sonar 
 */
void
sonar_init()
{
        bit_set(0x1009, 0x30);          /* ddrd */
        bit_set(0x1021, 1);             /* at tctl2, */
        bit_clear(0x1021, 2);           /* set tic3 for rising edge */
}


/*
 * Initialize all peripherals
 */
void 
init()
{
        sonar_init();
        init_expbd_servos(1);
        /* servo_on(); */
        /* comp_init(1); */
}

/*
 * Read the sonar close up 
 * Returns Inches 
 */
int
sonar_closeup()
{
   int start_time;
   
   poke(0x1023, 1);                                /* clear tic3 flag */
   start_time= peekword(0x100e);
   poke(0x1008, 0x20);
   
   while ((peekword(0x100e) - start_time) < 1000);
   
   bit_set(0x1008, 0x30);                  /* turn on BINH */
   
   while (!(peek(0x1000) & 0x01)) {
      if ((peekword(0x100e) - start_time) < 0) {
         /* if too much time has elapsed, abort */
         bit_clear(0x1008, 0x30);
         return -1;
      }
      defer();
   }
   
   bit_clear(0x1008, 0x30);
   
   /** return inches **/
   return (peekword(0x1014) - start_time)/150;     /* 0x1014 is tic3 */
}

/*
 * Turn the servo N degrees, 
 * 0 degrees is right, 180 is left, 90 is centered. 
 */
void 
servo5_deg(float deg)
{
   float w = ((deg / 360.0) * 3000.0);
   int where = (int)w + 1800;
   /* printf("deg=%f w=%f wh=%d\n", deg, w, where); */
   /* msleep (5000L); */
   servo5 = where;
}

/*
 * Turn the servo mounted sonar to a heading (degrees), and 
 * make a distance reading. 0 degrees is right, 180 is left, 
 * 90 is centered. 
 */
int
look(int where)
{
        servo5_deg((float)where);
        /* scan in direction */
        return sonar_closeup();  /* distance from sonar */
}


/* 
 * spin the left tire one complete revolution counting the 
 * encoder stripes
 */
int
one_rev()
{
   encoder6_counts = 0;
   motor(left_wheel,50);
   while(encoder6_counts < 16); /* 16 counts per rev = 11+5 coast */
   /** stop, no coast, no backspin **/
   motor(left_wheel,-100);
   msleep(10L);
   ao();
   msleep(2000L);  /** wait until everything is stopped **/
   return encoder6_counts;
}


/* 
 * spin the right tire one complete revolution counting the 
 * encoder stripes
 */
int
r_one_rev()
{
   encoder0_counts = 0;
   motor(right_wheel,50);
   while(encoder0_counts < 16); /* 16 counts per rev = 11+5 coast */
   /** STOP no coast, no backspin **/
   motor(right_wheel,-100);
   msleep(10L);
   ao();

   msleep(2000L); /** for sure wait 'til everything is stopped **/
   return encoder0_counts;
}


/* 
 * quick test to see if everything is working
 * on power up
 */
void 
bot_test()
{       
        int heading;

        init();

        look(150);
        sleep(1.0);
        look(30);
        sleep(1.0);
        look(90);

        one_rev();
}

/*
 * Run the robot straight forward until value
 * stripes were counted on the encoder
 */
int
go_till(int val)
{
   int r_speed;
   int start0 = encoder0_counts;
   int start6 = encoder6_counts;
   
   if (val < 0)
      r_speed = speed * -1;
   else
      r_speed = speed;
   
   motor(1,r_speed);
   motor(0,r_speed);
   
   while(encoder6_counts-start6 < val)  
      {
         if (encoder6_counts > encoder0_counts+2)
            motor(left_wheel,0);
         else
            motor(left_wheel,r_speed);
         if (encoder0_counts > encoder6_counts+2)
            motor(right_wheel,0);
         else
            motor(right_wheel,r_speed);
      }
   ao(); 

/*   sleep(2.0); */
   return encoder6_counts;
}

   /*   22.36  counts per 180 degrees */
/*
 * turn the robot N degrees
 * + is left, - is right 
 */
int
turn(int deg)
{
   int counts = (int)(((float)deg / 180.0) * 22.0);
   int mot1_speed;
   int mot0_speed;
   
   if (counts < 0)
      counts *= -1;
   encoder0_counts = 0;
   encoder6_counts = 0;
   
   if (deg > 0) {
      mot1_speed = speed;
      mot0_speed = -speed;
   } else {
      mot1_speed = -speed;
      mot0_speed = speed;
   }
   
   while(encoder6_counts < counts) { /* + coast */
      if (encoder6_counts > encoder0_counts+2)
         motor(left_wheel,0);
      else
         motor(left_wheel,mot1_speed);
      if (encoder0_counts > encoder6_counts+2)
         motor(right_wheel,0);
      else
         motor(right_wheel,mot0_speed);
   }
   ao();
   encoder0_counts = 0;
   encoder6_counts = 0;
}

void
main_routine()
{
   int one_way;
   int other;
   int dist1;
   int dist2;
   int dist3;
   int i;
   
   init();
   for(;;) {
      one_way = go_till(10);
      dist1 = look(90);
      msleep(50L);
      dist2 = look(90);
      msleep(50L);
      dist3 = look(90);
      msleep(50L);

      /*
       * Keep count on LCD sonar distances
       * distN are 3 samples, to be averaged
       */
      printf("Dist = %d  %d  %d\n", dist1,dist2,dist3);

      if (dist2 < 20) {
         /*
          * distance too close, look for best way around obsticle 
          */
         dist1 = look(135); /* left */
         msleep(1000L);
         dist2 = look(45);  /* right */
         msleep(1000L);
         dist3 = look(90);  /* ahead */
         if (dist1 < dist3)
            turn(30);       /* turn left */
         else
            turn(-30);      /* turn right */
         motor(1,-speed);
         motor(0,-speed);
         msleep(700L);
         if (dist1 < dist3) /* turn more */
            turn(30);
         else
            turn(-30);
         ao();
      }
   }
}


void
main()
{
        main_routine();
}