/* Collection Of Robot Drivers (CORD) for Johuco Phoenix robot */
/* Copyright 1997, Johuco Ltd., All Rights Reserved */
/* Written by Jon Connell, 10/97, Version 1.3 beta  */

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

/* function prototypes (ifndef needed to prevent loops in pseudo-library) */

#ifndef _CORD_
#include <cord.h>
#endif


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

/* turn left LED eye either on or off */

void left_led (int val)
{
  asm(" ldaa __d173");
  asm(" anda #LEFT_MASK");
  asm(" tst 1+%val");
  asm(" beq __left_off");
  asm(" oraa #LEFT_EYE");
  asm("__left_off");
  asm(" staa __d173");
  asm(" staa LEDS");
}


/* turn right LED eye either on or off */

void right_led (int val)
{
  asm(" ldaa __d173");
  asm(" anda #RIGHT_MASK");
  asm(" tst 1+%val");
  asm(" beq __right_off");
  asm(" oraa #RIGHT_EYE");
  asm("__right_off");
  asm(" staa __d173");
  asm(" staa LEDS");
}


/* left led is bit 1, right led is bit 0 */

void both_leds (int val)
{
  asm(" ldab 1+%val");
  asm(" aslb");
  asm(" aslb");
  asm(" aslb");
  asm(" aslb");
  asm(" aslb");
  asm(" andb #EYES");
  asm(" ldaa __d173");
  asm(" anda #EYE_MASK");
  asm(" aba");
  asm(" staa __d173");
  asm(" staa LEDS");
}


/* turn on both LEDs proportional to some 8 bit value */
/* useful for debugging using an oscilloscope on R14  */
/* range: 0 -> 0.5ms, 100 -> 4.4ms, 255 ->10.5ms      */

void val_to_leds (int val)
{
  both_leds(3);
  sleep_us((val + 13) * 39);
  both_leds(0);
}


/* turn audio beeper on or off                                */
/* if toggle time is non-zero, inverts state later (32ms max) */

void beeper (int val, int toggle_us)
{
  asm(" bclr ITCTL1 #$C0");	/* disable timer output compare */
  asm(" tst 1+%val");           /* set state on or off */
  asm(" beq __beep_off");
  asm(" bset IPORTA #BEEPER");
  asm(" bra __beep_time");
  asm("__beep_off");
  asm(" bclr IPORTA #BEEPER");
  asm(" brn __beep_time");
  asm("__beep_time");
  asm(" ldd %toggle_us");
  asm(" beq __beep_done");
  asm(" lslb");			/* adjust time for real usecs */
  asm(" rola");			  
  asm(" subd #27");		
  asm(" addd ITCNT");
  asm(" std ITOC2");
  asm(" bset ITCTL1 #$40");	/* set up to toggle line */
  asm("__beep_done");
}


/* turns beeper on for about 100 ms */

void beep ()
{
  asm(" bclr ITCTL1 #$C0");	/* stop any compares in progress */
  asm(" bset IPORTA #BEEPER");  /* turn beeper on */
  asm(" ldd #173");		/* schedule automatic turn off time */
  asm(" addd ITCNT");
  asm(" std ITOC2");
  asm(" bset ITCTL1 #$80");	/* set up to clear output bit */
}


/* simple function to turn beeper on continuously */

void tone ()
{
  asm(" bclr ITCTL1 #$C0");	
  asm(" bset IPORTA #BEEPER");
}


/* simple function to turn beeper off */

void quiet ()
{
  asm(" bclr ITCTL1 #$C0");	
  asm(" bclr IPORTA #BEEPER");
}


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

/* set direction for drive motor (1 is forward, -1 is backward, 0 to stop) */
/* if time-out is non-zero, shuts down motor after specified time          */

void drive (int dir, int stop_us)
{
  asm(" bclr ITCTL1 #$30");	/* disable timer output compare */
  asm(" ldd %dir");             /* see if motor stop requested */
  asm(" bne __motor_run");
  asm(" bclr IPORTA #MOTOR_ON");
  asm(" bra __motor_done");
  asm("__motor_run");		/* set direction bit for motor */
  asm(" bmi __motor_back");
  asm(" bset IPORTD #MOTOR_DIR");
  asm(" bra __motor_time");
  asm("__motor_back");
  asm(" bclr IPORTD #MOTOR_DIR");
  asm(" brn __motor_time");
  asm("__motor_time");		
  asm(" bset IPORTA #MOTOR_ON");/* activate drive motor */
  asm(" ldd %stop_us");	        /* determine stop time (if any) */
  asm(" beq __motor_done");
  asm(" lsld");			/* adjust time for real usecs */
  asm(" subd #42");		
  asm(" addd ITCNT");
  asm(" std ITOC3");
  asm(" bset ITCTL1 #$20");	/* set line to zero at compare time */
  asm("__motor_done");
}


/* set direction for steering (1 is left, -1 is right, 0 for straight) */
/* if time-out is non-zero, stops turning after specified time         */

void turn (int dir, int stop_us)
{
  asm(" bclr ITCTL1 #$0C");	/* disable timer output compare */
  asm(" ldd %dir");             /* see if straight requested */
  asm(" bne __turn_active");
  asm(" bclr IPORTA #TURN_ON");
  asm(" bra __turn_done");
  asm("__turn_active");		/* set direction bit for solenoid */
  asm(" bmi __turn_right");
  asm(" bset IPORTD #TURN_DIR");
  asm(" bra __turn_time");
  asm("__turn_right");
  asm(" bclr IPORTD #TURN_DIR");
  asm(" brn __turn_time");
  asm("__turn_time");		
  asm(" bset IPORTA #TURN_ON"); /* activate turn solenoid */
  asm(" ldd %stop_us");		/* determine stop time (if any) */
  asm(" beq __turn_done");
  asm(" lsld");			/* adjust time for real usecs */
  asm(" subd #42");		
  asm(" addd ITCNT");
  asm(" std ITOC4");
  asm(" bset ITCTL1 #$08");	/* set line to zero at compare time */
  asm("__turn_done");
}


/* full power forward with no timeout (same as drive(1, 0)) */

void forward ()
{
  asm(" bclr ITCTL1 #$30");	
  asm(" bset IPORTD #MOTOR_DIR");
  asm(" bset IPORTA #MOTOR_ON");
}


/* full power backward with no timeout (same as drive(-1, 0)) */

void backward ()
{
  asm(" bclr ITCTL1 #$30");	
  asm(" bclr IPORTD #MOTOR_DIR");
  asm(" bset IPORTA #MOTOR_ON");
}


/* turn wheels to left (same as turn(1, 0) - goes right if backing up) */

void left ()
{
  asm(" bclr ITCTL1 #$0C");	
  asm(" bset IPORTD #TURN_DIR");
  asm(" bset IPORTA #TURN_ON");
}


/* turn wheels to right (same as turn(-1, 0) - goes left if backing up) */

void right ()
{
  asm(" bclr ITCTL1 #$0C");	
  asm(" bclr IPORTD #TURN_DIR");
  asm(" bset IPORTA #TURN_ON");
}


/* disables turns to left or right (same as turn(0, 0)) */

void straight ()
{
  asm(" bclr ITCTL1 #$0C");	
  asm(" bclr IPORTA #TURN_ON");
}


/* turns off main motor (same as drive(0, 0)) */

void stop ()
{
  asm(" bclr ITCTL1 #$3C");	
  asm(" bclr IPORTA #MOTOR_ON");
}


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

/* emit 4 IR pulses of increasing power: 0.6ms separated by 0.6ms */
/* record level at which IR receivers come on and stay on         */
/* answer returned as two nybbles in lower byte = 0:0:left:right  */

int ir_blip (int narrow)
{
  int i, vals, lf = 0, rt = 0;

  for (i = 0; i < 4; i++)
  {
     vals = ir_bits(narrow, i);
     if ((vals & 0x02) == 0) 
       lf = 0;
     else if (lf == 0)
       lf = 4 - i;
     if ((vals & 0x01) == 0) 
       rt = 0;
     else if (rt == 0)
       rt = 4 - i;
  }
  return((lf << 4) | rt);
}


/* pulse IR and get response bits (bit 1 = left, bit 0 = right) */
/* has a pause so that back to back calls will yield valid data */

int ir_bits (int narrow, int power)
{
  unsigned cnts;
  int start, no_ir;
  
  start = clock();
  no_ir = both_ir();
  cnts = (unsigned) ir_pulse(narrow, power, 24);
  if ((cnts >> 8) < 16)
    no_ir |= 0x02;	  
  if ((cnts & 0xFF) < 16)
    no_ir |= 0x01;	  
  resync(start, 1200); 
  return(~no_ir & 0x03);
}
  

/* pulse IR at 39.2 KHz with some power level (0 to 3) */
/* for a while (standard 0.6ms pulse is 24 cycles)     */
/* returns length of first received pulse on each side */
/* left is MSB and right is LSB (25.5us sample time)   */ 
/* WARNING: disables interrupts during active pulse    */

int ir_pulse (int narrow, int power, int cycles)
{
  asm(" ldaa __d173");			/* turn off IR emitters */
  asm(" anda #EYES");
  asm(" staa LEDS");
  asm(" staa __d173");
  asm(" ldaa IPORTD");			/* send power level to port D */
  asm(" anda #BASE_DIR");
  asm(" ldab 1+%power");		
  asm(" aslb");
  asm(" aslb");
  asm(" andb #IR_POWER");
  asm(" aba");
  asm(" staa IPORTD");
  asm(" tpa");				/* save old interrupt bit */
  asm(" psha");

  asm(" ldaa #WIDE_IR");		/* Y = ON:CNT_RIGHT pattern */
  asm(" tst 1+%narrow");
  asm(" beq __ir_pats");
  asm(" ldaa #NARROW_IR");
  asm("__ir_pats");
  asm(" oraa __d173");
  asm(" clrb");
  asm(" xgdy");
  asm(" ldaa __d173");			/* A:B = OFF:CNT_LEFT pattern */
  asm(" clrb");
  asm(" ldx %cycles");			/* X = total number of blips */
  asm(" sei");				/* no interrupts, timing critical */
  
  asm("__emit");
  asm(" xgdy");				/* get ON:CNT_RIGHT into A:B */
  asm(" staa LEDS");			/* 24 E clocks on (10 + 10 + 4) */
  asm(" brclr IPORTA #RIR_DETECT __rir_cnt");
  asm(" tstb");				/* if IR off now, see if ever on */
  asm(" beq __rir_nop5")
  asm(" orab #$80");			/* if was on and now off, set bit */
  asm(" bra __ir_freq");
  asm("__rir_cnt");
  asm(" bitb #$80");			/* if IR on, see if first pulse */
  asm(" bne __rir_nop5");
  asm(" incb")				/* if still on, boost A count */
  asm(" bra __ir_freq");	  
  asm("__rir_nop5");			/* make all paths 10 cycles */
  asm(" nop");
  asm(" brn __ir_freq");
  asm("__ir_freq");	  

  asm(" xgdy");				/* get OFF:CNT_LEFT into A:B */
  asm(" staa LEDS");			/* 27 E clocks off (10 + 13 + 4) */
  asm(" brclr IPORTA #LIR_DETECT __lir_cnt");	
  asm(" tstb");				/* if IR off now, see if ever on */
  asm(" beq __lir_nop2")
  asm(" orab #$80");			/* if was on and now off, set bit */
  asm(" dex");				/* loop if not done yet */
  asm(" bne __emit");
  asm(" bra __tally");
  asm("__lir_cnt");
  asm(" bitb #$80");			/* if IR on, see if first pulse */
  asm(" bne __lir_nop2");
  asm(" incb")				/* if still on, boost B count */
  asm(" dex");				/* loop if not done yet */
  asm(" bne __emit");
  asm(" bra __tally");
  asm("__lir_nop2");			/* make all paths 16 cycles */
  asm(" nop");
  asm(" dex");				/* loop if not done yet */
  asm(" bne __emit");

  asm("__tally");
  asm(" pula")				/* restore old interrupt enable */
  asm(" tap");
  asm(" pshb");				/* CNT_LEFT on stack, B = CNT_RIGHT */
  asm(" xgdy");  
  asm(" pula");  
  asm(" anda #$7F");
  asm(" andb #$7F");
}


/* emitters should be off for at least 600 us to let receivers recover  */
/* this routine called back to back with ir_pulse yields 50% duty cycle */

void ir_wait ()
{
  sleep_us(540);
}


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

/* tells whether the left IR receiver sees a signal right now */

int left_ir (void)
{
  asm(" clra");
  asm(" clrb");
  asm(" brset IPORTA #LIR_DETECT __lir_done");
  asm(" incb");
  asm("__lir_done");
}


/* tells whether the right IR receiver sees a signal right now */

int right_ir (void)
{
  asm(" clra");
  asm(" clrb");
  asm(" brset IPORTA #RIR_DETECT __rir_done");
  asm(" incb");
  asm("__rir_done");
}


/* returns a value where left IR is bit 1, right IR is bit 0 */

int both_ir ()
{
  asm(" ldab IPORTA");
  asm(" comb");
  asm(" andb #IR_DETECTS");
  asm(" lsrb");
  asm(" clra");
}


/* tell time (us) of left receiver event (zero means not triggered) */

int lir_time ()
{
  asm(" clra");	
  asm(" clrb");
  asm(" brclr ITFLG1 #$04 lir_tdone");
  asm(" ldd ITIC1");
  asm(" lsrd");
  asm(" bne lir_tdone");
  asm(" incb");
  asm("lir_tdone");
}


/* tell time (us) of right receiver event (zero means not triggered) */

int rir_time ()
{
  asm(" clra");	
  asm(" clrb");
  asm(" brclr ITFLG1 #$02 rir_tdone");
  asm(" ldd ITIC2");
  asm(" lsrd");
  asm(" bne rir_tdone");
  asm(" incb");
  asm("rir_tdone");
}


/* listen for "wait" milliseconds for valid IR from something else */
/* most remote controls pulse for at least 10ms every 100ms        */

int passive_ir (int wait)
{
  return passive_25(40 * wait);
}


/* sample IR receivers at 25 usec rate             */
/* bit 1 is left receiver, bit 0 is right receiver */

int passive_25 (int wait)
{
  asm(" ldy %wait");	/* Y is loop count, each takes 25 usec */
  asm(" clra");		/* X is result, only lower 2 bits used */
  asm(" clrb");
  asm(" xgdx");
  asm(" ldaa #18");	/* A counts down low time of left IR  */
  asm(" tab");		/* B counts down low time of right IR */
  
  asm("pass_lir");	/* all paths are 19 cycles */
  asm(" brclr IPORTA #LIR_DETECT lir_count");
  asm(" ldaa #18");	/* if line hi, set count to max (13 cycles) */
  asm(" nop");
  asm(" brn pass_rir");
  asm(" brn pass_rir");
  asm(" bra pass_rir");
  asm("lir_count");
  asm(" tsta");		/* if line low, check count (5 cycles) */
  asm(" beq lir_hit");
  asm(" deca");		/* normally decrement count (8 cycles) */
  asm(" brn pass_rir");
  asm(" bra pass_rir");
  asm("lir_hit");
  asm(" xgdx");		/* if count done, set output bit (8 cycles) */
  asm(" orab #2");
  asm(" xgdx");

  asm("pass_rir");	/* all paths are 19 cycles */
  asm(" brclr IPORTA #RIR_DETECT rir_count");
  asm(" ldab #18");	/* if line hi, set count to max (13 cycles) */
  asm(" nop");
  asm(" brn pass_loop");
  asm(" brn pass_loop");
  asm(" bra pass_loop");
  asm("rir_count");
  asm(" tstb");		/* if line low, check count (5 cycles) */
  asm(" beq rir_hit");
  asm(" decb");		/* normally decrement count (8 cycles) */
  asm(" brn pass_loop");
  asm(" bra pass_loop");
  asm("rir_hit");
  asm(" xgdx");		/* if count done, set output bit (8 cycles) */
  asm(" orab #1");
  asm(" xgdx");
	
  asm("pass_loop");
  asm(" dey");		/* see if all loops done (12 cycles) */
  asm(" nop");
  asm(" brn pass_lir"); 
  asm(" bne pass_lir");
  asm(" xgdx");		/* after loop get result bits into D */
}


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

/* reads position of rear user potentiometer */

int knob ()
{
  asm(" ldab #KNOB");
  asm(" jsr __ad_read");
  asm(" clra");
}


/* quantizes value of potentiometer between 0 and N-1 */

int knob_coarse (int bins)
{
  asm(" ldab #KNOB");
  asm(" jsr __ad_read");
  asm(" ldaa 1+%bins");
  asm(" mul");
  asm(" tab");
  asm(" clra");
}


/* checks if top button is currently pressed */

int button ()
{
  asm(" ldab #BATTERY");
  asm(" jsr __ad_read");
  asm(" tba");
  asm(" clrb");
  asm(" cmpa #$40");
  asm(" bhi __button_off");
  asm(" incb");
  asm("__button_off");
  asm(" clra");
}


/* checks battery level, returns -1 if can not read (because of button) */

int battery ()
{
  asm(" ldab #BATTERY");
  asm(" jsr __ad_read");
  asm(" clra");
  asm(" cmpb #$40");
  asm(" bhs __battery_done");
  asm(" ldd #-1");
  asm("__battery_done");
}


/* checks if mercury switch is currently activated */

int roll ()
{
  asm(" ldab #COLLIDE");
  asm(" jsr __ad_read");
  asm(" tba");
  asm(" clrb");
  asm(" cmpa #$C0");
  asm(" blo __no_crash");
  asm(" incb");
  asm("__no_crash");
  asm(" clra");
}


/* checks how recently mercury switch was actived */

int shakiness ()
{
  asm(" ldab #COLLIDE");
  asm(" jsr __ad_read");
  asm(" clra");
}


/* returns light level (0 - 255) sensed by top-facing photocell */

int top_photo ()
{
  asm(" ldab #PH_TOP");
  asm(" jsr __ad_read");
  asm(" clra");
}


/* returns light level (0 - 255) sensed by front-facing photocell */

int front_photo ()
{
  asm(" ldab #PH_FRONT");
  asm(" jsr __ad_read");
  asm(" clra");
}


/* convert 4 channels then stop, transfer results to given variables */
/* group 0 = front photocell, battery, user4, user 5                 */
/* group 1 = knob, top photocell, mercury switch, user 6             */
/* takes about 100 usecs to complete                                 */

void digitize_group (int group, int *result1, int *result2, 
		     int *result3, int *result4)
{
  asm(" ldaa #$10");		/* generate multiple conversion command */
  asm(" tst 1+%group");
  asm(" beq group_start");
  asm(" adda #$04");
  asm("group_start");
  asm(" staa IADCTL");
  asm("group_wait");
  asm(" brclr IADCTL #$80 group_wait");		/* wait for completion */
  asm(" clra");
  asm(" ldab IADR1");				/* copy out results */
  asm(" ldy %result1");
  asm(" std 0,y");
  asm(" ldab IADR2");
  asm(" ldy %result2");
  asm(" std 0,y");
  asm(" ldab IADR3");
  asm(" ldy %result3");
  asm(" std 0,y");
  asm(" ldab IADR4");
  asm(" ldy %result4");
  asm(" std 0,y");
}


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

/* wait a given number of microseconds (65ms max, 4us resolution) */

void sleep_us (int time)
{
  asm(" ldd %time");
  asm(" subd #19");
  asm(" bcs __us_done");
  asm(" lsrd");
  asm(" lsrd");
  asm(" xgdx");
  asm(" brn __us_loop");
  asm("__us_loop");
  asm(" nop");
  asm(" dex");
  asm(" bne __us_loop");
  asm("__us_done");
}


/* wait a given number of milliseconds (65 seconds max) */

void sleep_ms (int time)
{
  asm(" ldx %time");
  asm(" beq __ms_done");
  asm("__ms_outer");
  asm(" ldab #250");
  asm("__ms_inner");	
  asm(" brn __ms_inner");
  asm(" decb");
  asm(" bne __ms_inner");
  asm(" dex");
  asm(" bne __ms_outer");
  asm("__ms_done");
}


/* wait for free running counter to overflow (happens at 30 Hz) */

void await_tick ()
{
  asm(" bset ITFLG2 #$80");	
  asm("__no_tick");	
  asm(" brclr ITFLG2 #$80 __no_tick");
}


/* returns half the value of free running counter (1us per count) */
/* adjust value for calling time (6 for JSR EXT + 2 for LDD)      */

int clock () 
{ 
  asm(" ldd ITCNT");
  asm(" subd #8");
  asm(" lsrd"); 
}


/* waits until 1 usec free running clock reaches a particular value */
/* assumes time has not occurred yet, results are only approximate  */

void resync (int ref, int delta)
{
  sleep_us((ref + delta - clock()) & 0x3FFF - 39);
}


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