/* BAckground Sensor Actuator Loop (BASAL) for Phoenix mobile robot */
/* Maintains a number of sensory variables and interprets commands. */
/* Copyright 1998, Johuco Ltd., All Rights Reserved */
/* Written by Jon Connell, 1/98 - 2/99, Version 1.3 */

/* ====================================================================== */

/* ifndef needed to prevent loops in pseudo-library */
/* for extra vector handlers just add defines, do not include vectors.h */

#define INSTALL_LIR 
#define INSTALL_RIR 
#define INSTALL_RTI
#include <vectors.h>
#include <cord.h>
#ifndef _BASAL_
#include <basal.h>
#endif


/* ---------------------------------------------------------------------- */

/* deadband half-width (hysteresis) vor bang-bang motor controller */

#define M_BND 4


/* private copies of command variables for double buffering */

int drive_speed2;
int turn_speed2;
int beeper_cmd2;
int led_cmd2;
int ping_slot2;


/* private copies of sensor variables for double buffering */

int passive_vals2;
int passive_time2;
int velocity2;


/* private sequencing variable */

int basal_cycle;


/* private variables for motor speed control */

int zero_motion;
int drive_width;


/* ---------------------------------------------------------------------- */

/* private function prototypes */

void basal_default (void);

void int_lir (void);
void int_rir (void);
void passive_hit (void);

void int_rti (void);
void digitize_inputs (void);
void find_ir_vals (void);

void control_motor (int desired);
int get_speed (int ref, int scale);
int full_drop (void);
int get_drop (int ref);


/* ================================================================== */

/* enable basal functionality: sensing and control through globals */

void init_basal ()
{
  /* initialize drive motor servo values */
  /* and global control variables        */
  ints_off();	
  zero_motion = full_drop();	
  velocity2 = 0;
  drive_width = 0;
  basal_default();

  /* enable 60 Hz and passive IR interrupts */	
  basal_cycle = 0;
  lir_fall(); 
  rir_fall();   
  allow_lir();
  allow_rir();
  allow_rti();
  ints_on();
  sleep_ms(100);
}


/* copy commands variables as a block to duplicate area           */
/* wait for basal interrupt to be processed (cycle count changes) */
/* reset all control variables to their default values            */

int next_basal ()
{
  int now;

  /* copy commands as an atomic operation */
  ints_off();
  drive_speed2 = drive_speed;
  turn_speed2  = turn_speed;
  beeper_cmd2  = beeper_cmd;
  led_cmd2     = led_cmd;
  ping_slot2   = ping_slot;
  ints_on();

  /* wait for commands to be taken and new sensor values to arrive */
  now = (basal_cycle & 0xFFFC);
  while ((basal_cycle & 0xFFFC) == now);
  basal_default();
  return 1;
}


/* reset commands to default values for next cycle */

void basal_default ()
{
  turn_speed  = 0;
  drive_speed = 0;
  beeper_cmd  = 0;
  led_cmd     = 0;
  ping_slot   = 0;
}


/* ====================================================================== */

/* both interrupts call same function */

void int_lir ()
{
  passive_hit();
}


void int_rir ()
{
  passive_hit();
}


/* when either IR seen, loop for a while to check validity */
/* record and disable interrupt if valid pulse (not noise) */

void passive_hit ()
{
  passive_vals2 = passive_25(24);	
  if (passive_vals2)
  {
    /* tell which quarter of which cycle hit occurred (for ping_slot) */
    if ((passive_vals2 & 0x02) != 0)	  
      passive_time2 = lir_time();
    else
      passive_time2 = rir_time();
    passive_time2 = ((passive_time2 >> 12) & 0x03);
    passive_time2 |= ((basal_cycle & 0x03) << 2);
    mask_lir(); 
    mask_rir(); 
  }
}


/* ---------------------------------------------------------------------- */

/* Main basal routine gets called by real-time interrupt (RTI)  */
/* At 60 Hz check motor speeds, at 15 Hz read A/D and pulse IRs */
/* Can pulse IRs during some other phase than 0 if interference */
/* normally 3 ms, 14 ms for IR = 23 ms / 65.5 ms = 35% of cycle */
/* spacing from 15.7ms to 17.0ms due to passive IR interrupts   */

void int_rti ()
{
  int vals;

  /* always set turn and check motor speed against command   */
  /* if drive speed very high, do not servo or find velocity */
  turn(turn_speed2, 0);
  if (drive_speed2 > 100) 
    forward();
  else if (drive_speed2 < -100)
    backward();
  else
    control_motor(drive_speed2); 

  /* transfer commands and do A/D conversions at a fixed regular interval */
  basal_cycle++;
  if ((basal_cycle & 0x03) == 0)
  {
    digitize_inputs(); 
    beeper(beeper_cmd2, 0); 
    both_leds(led_cmd2);  
    velocity = velocity2;
    passive_vals = passive_vals2;
    passive_time = passive_time2;
    allow_lir();
    allow_rir();
  }

  /* emit IR pulses only once every 4 interrupts to allow several robots */
  if ((basal_cycle & 0x03) == ping_slot2)
    find_ir_vals();	  
}


/* save result of digitizing all 8 channels in appropriate variables */

void digitize_inputs ()
{
  int b;

  /* get basic analog to digital converter values */
  digitize_group(0, &front_level, &b, &user4_level, &user5_level);
  digitize_group(1, &knob_level, &top_level, &shakiness_level, &user6_level);
  
  /* intepret values to get extra binary predicates */
  if (b < 0x40)
    button_val = 1;
  else
  {
    battery_level = b;
    button_val = 0;
  }
  if (shakiness_level >= 0xC0)
    roll_val = 1;
  else
    roll_val = 0;	  
}


/* emit 4 wide IR pulses then 4 narrow ones, record responses */
/* returns values between 0 and 100                           */
	  
void find_ir_vals ()	  
{
  int vals;
  
  vals = ir_blip(0);
  wleft_val  = 25 * (vals >> 4);
  wright_val = 25 * (vals & 0x0F);
  vals = ir_blip(1);
  nleft_val  = 25 * (vals >> 4);
  nright_val = 25 * (vals & 0x0F);
}


/* -------------------------------------------------------------------- */

/* new speed controller is bang-bang with hysteresis */
/* this gives better torque since motor is full on   */
/* makes sure reverse EMF is sampled every N cycles  */
/* expects desired speed in range -100 to 100        */
/* really only about 4 levels: 25, 50, 75, 100       */

void control_motor (int desired)
{
  int v, e, dir = 1;

  /* assume motor off at start, send duration to activate now */
  /* always on for 2.3ms, max is 13.5ms (i.e. min 2.2ms off)  */
  if (drive_width < 0)
    dir = -1;	  
  v = battery();
  drive(dir, 44 * dir * drive_width + 2300); 

  /* add sign to speed and compute smoothed version */	  
  v = get_speed(v, zero_motion);
  if (v >= 0)
  {
    v >>= 1;
    velocity2 = (dir * v + 3 * velocity2) >> 2; 
  }

  /* dither direction when zero motion desired or bad velocity */
  if ((v < 0) || ((-10 < desired) && (desired < 10)))
  {
    if (drive_width >= 0)
      drive_width = -1;
    else
      drive_width = 1;	    
  }
  else
  {
    /* determine correction for next cycle based on velocity error */
    if (desired < 0)
      e = v + desired;
    else
      e = v - desired;
  
    /* if motor substantially too slow, quickly boost to max speed */
    /* else if too fast, decrement drive proportional to error     */	    
    if (e < -8)
      drive_width -= dir * (e << 1);	      
    else if (e > 8)
      drive_width -= dir * e;

    /* make sure pulse width is in valid range */
    if (desired > 0)
      drive_width = clamp_val(drive_width, 1, 255);
    else
      drive_width = clamp_val(drive_width, -255, -1);
  }
/*
  if (velocity2 > -128)           
    val_to_leds(velocity2 + 128); 
*/
}


/* computes normalized drive motor speed                    */
/* needs motor off voltage and full-scale calibration value */

int get_speed (int ref, int scale)
{
  int ans;

  /* voltage sag determines back EMF, normalize by top speed      */
  /* be careful to check for button pressed (bad voltage reading) */
  ans = get_drop(ref);
  if (ans < 0)
    return ans;	  
  ans = (ans << 7) / scale; 
  ans = 255 - (ans << 1);
  ans = clamp_val(ans, 0, 255);
  return ans;
}


/* find maximum vlotage drop when robot is known to be stationary */

int full_drop ()
{
  int z;
  
  stop();	
  sleep_ms(100);	
  z = battery();	
  forward();
  z = get_drop(z);
  stop();
  return z;
}


/* measure voltage drop across 1 ohm resistor due to motor current */
/* takes motor off voltage as an input to adjust for battery decay */

int get_drop (int ref)
{
  int ans;
  
  /* assumes motor is on, waits 2ms for filter to stabilize  */
  /* checks to make sure button is not pressed (battery < 0) */
  sleep_us(2000);
  ans = battery();
  if (ans < 0)
    return ans;	  

  /* compute drop and saturate range at 0 and 255         */
  /* unlikely to get more than 0.7V drop out of 6V supply */
  ans = ref - ans;
  ans = clamp_val(ans, 0, 31);
  return(ans << 3);
}


/* ==================================================================== */