/* "ROVLIB"  ROVER FUNCTION LIBRARY */

#include <tt7.h>
#include <stdlib.h>
#include "rovlib.h"
#include "rovsys.h"
#include "rovint.h"
#include "rovfile.h"

#ifdef AZTEC_C    /* Include if compiler is Aztec C */
#include <sim.h>
#include <pins.h>
#include <atod.h>
#include <tpu.h>
#include <timing.h>
#include <excepts.h>
#include <serio.h>
#include "tserial.h"
#define IOPORT1   ((uspv) (0xE00000))
#define IOPORT2   ((uspv) (0xE00002))

#else                   /* Include if Turbo C */
#include "tt7pclib.h"
#include "conio.h"
int g_test_heading = 0;
short g_test_cmdir = 0;
char g_test_direction;

#endif

/* GLOBAL VARIABLES */
int g_cmrotor_ticks, g_program_level = 0;

/********************** VISCA Commands *****************/

	      /* First digit is message length */
unsigned char visca_init[] = {5,0x88,0x01,0x00,0x01,0xff};
unsigned char visca_address[] = {4,0x88,0x30,0x01,0xff};
unsigned char visca_pwr_on[] = {6,0x81,0x01,0x02,0x00,0x02,0xff};
unsigned char visca_pwr_off[] = {6,0x81,0x01,0x02,0x00,0x03,0xff};
unsigned char vcr_record[] = {6,0x81,0x01,0x02,0x01,0x48,0xff};
unsigned char vcr_stop[] = {6,0x81,0x01,0x02,0x01,0x00,0xff};


/********************************************************
	     Auxilliary Port Routines
********************************************************

	   Auxilliary Output Port Routine

In: Port number (1-4), 1 or 0 to set or clear bits in port, output
    byte (0 to FF).
Return: Void

The routine first checks to see if the port is the high byte
of the 16 bit port word.  If it is, it adds FF to the byte.
The routine then sets or clears bits depending upon the variable
"setclear".
**********************************************************/

void outauxport(short port, short setclear, unsigned short outdata) {

  static short int port1data, port2data;

  if(port==2 || port==4) outdata = outdata << 8;

  if(port==1 || port==2) {
    if(setclear) port1data = port1data | outdata;
      else port1data = port1data & ~outdata;

#ifdef AZTEC_C
    *IOPORT1 = port1data;  /* Write 16 bit word to OUT0-OUT15 */
    return;

#else
    printf("\n");
    if(port1data & MOT1A && port1data & MOT2A) printf("\nMove Rov FWD"); /* OUT0-OUT15 */
    if(port1data & MOT1B && port1data & MOT2B) printf("\nMove Rov BACK");
    if(port1data & MOT1B && port1data & MOT2A) printf("\nMove Rov LEFT");
    if(port1data & MOT1A && port1data & MOT2B) printf("\nMove Rov RIGHT");

    if(port1data & 0x10) printf("\nFlood 1 on");
    if(port1data & 0x20) printf("\nFlood 2 on");
    if(port1data & 0x40) printf("\nAUX3 on");

    port1data >>= 8;

    if(port1data & MOT1A) printf("\nMove Rack UP");
    if(port1data & MOT1B) printf("\nMove Rack DOWN");
    if(port1data & MOT2B) printf("\nMove Current Meter DOWN");
    if(port1data & MOT2A) printf("\nMove Current Meter UP");

    if(port1data & 0x10) printf("\nStrobe on");
    if(port1data & 0x20) printf("\nAUX2 on");
    if(port1data & 0x40) printf("\nAUX3 on");

    if(port2data & POWER1) printf("\nBen. Cham. 1 power on"); /* OUT0-OUT15 */
    if(port2data & POWER2) printf("\nBen. Cham. 2 power on");
    if(port2data & POWER3) printf("\nMicroprofiler power on");
    if(port2data & POWER4) printf("\nTransponder power on");

    if(port2data & POWER5) printf("\nAux. power on");
    if(port2data & POWER6) printf("\nSensor power on");
      else printf("\nSensor power off");
    if(port2data & POWER7) printf("\nVideo 1 power on");
    if(port2data & POWER8) printf("\nVideo 2 power on");

    return;
#endif
  }

  if(port==3 || port==4) {
    if(setclear) port2data = port2data | outdata;
      else port2data = port2data & ~outdata;

#ifdef AZTEC_C
    *IOPORT2 = port2data;  /* Write 16 bit word to OUT16-OUT31 */

#endif
  }
  return;
}
/******************************************************/

void clear_auxports(void){

  outauxport(1, CLEARBIT, 0xFF);
  outauxport(2, CLEARBIT, 0xFF);
  outauxport(3, CLEARBIT, 0xFF);
  outauxport(4, CLEARBIT, 0xFF);

}


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

	 Auxilliary Input Port Routine

In: Number of port bit to be tested.
Return: Value of tested bit (0 or 1).

The routine clears all bits in "portword" except the
one set in "portbit" .  It then compares the two words:
if they are equal, then the bit of interest must have been a one.
*********************************************************/

int inauxport(unsigned short portbit) {

#ifdef AZTEC_C
  unsigned short portword;

  portword =  *IOPORT1;   /* Read 16 bit word from IN0-IN15 */
  portword = portword & portbit;
  if(portword == portbit) return 1;
    else return 0;

#else
  return 1;
#endif
}

/********************************************************
	    Analog Input Functions
********************************************************/

void setanachan(short chan) {
  if(chan/4){
    PSet(E,7);
    chan -= 4;
  }
  else PClear(E,7);

  if(chan/2){
    PSet(E,6);
    chan -= 2;
  }
  else PClear(E,6);

  if(chan) PSet(E,5);
    else PClear(E,5);
  return;
}

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

int get_ad_counts(chan) {

  setanachan(chan);
  DelayMilliSecs(10);
  return AtoDReadWord(0);
}


/**************************************
       RS-232 Input/Output Functions
***************************************/

void setcomchan(short chan) {

  if(chan/4) {
    PSet(D,5);
    chan -= 4;
  }
    else PClear(D,5);

  if(chan/2) {
    PSet(D,4);
    chan -= 2;
  }
    else PClear(D,4);

  if(chan) PSet(D,3);
    else PClear(D,3);
  return;
}

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

int TPUiorelay(char chan){

#ifdef AZTEC_C
  int inkey;

  setcomchan( (short)chan );
  while(1){
    if( SerByteAvail() ){
      inkey = (int)SerGetByte();
      if( inkey == 3 ) break;            /* Exit on Ctrl-C */
      TSerPutByte( TPUSERTX, inkey );
    }

    if( TSerByteAvail( TPUSERRX ) ){
      SerPutByte( (unsigned char)TSerGetByte( TPUSERRX ) );
    }
  }
  return 0;

#else
  printf( "\nTPUiorelay channel =  %d", chan );
  return 0;

#endif
}

/*******************************************/
  
int heading(void) {

#ifdef AZTEC_C
  char inbuf[20];
  char inchar;
  int i;

  setcomchan(7);
  TSerInFlush(13);
  TPuts("d1\r");

  for(i=0; i<=15; i++) {
    inchar = (char)TSerGetByteTimed(500);
    if( inchar == (-1) ) {
      printf("\nCompass communication timeout error");
      return -1;
    }
    else inbuf[i] = inchar;
  }
  inbuf[16] = '\0';

/*  printf("\n%s", inbuf); */

  return atoi(inbuf+3);

#else                           /* Simulator for Turbo C */
  switch(g_test_direction){
    case 'r':
      (g_test_heading += 5);
      break;
    case 'l':
      (g_test_heading -= 5);
      break;
  }
  g_test_heading = (g_test_heading + 360) % 360;
  printf("\nHeading = %d", g_test_heading);
  return g_test_heading;
#endif
}



/**************************************
	Input Capture Functions
***************************************/

int TPUGetPinES(short chan, short edge){

#ifdef AZTEC_C
  CHANPRIOR(chan, Disabled);
  FUNSEL(chan, DIO);
  PARAMRAM(chan)[CHANNEL_CONTROL] = InputChan | NoForceState | edge;
  HOSTSEQ(chan, UpdtOnTrans); /* need to enter transistion mode first ! */
  HOSTSERVREQ(chan, Initialize);
  CHANPRIOR(chan, LowPrior);
  while (HOSTSERVSTAT(chan))
    ;
  HOSTSEQ(chan, UpdtOnHSRInit);
  HOSTSERVREQ(chan, Initialize);
  while (HOSTSERVSTAT(chan))
    ;
  return ((PARAMRAM(chan)[PIN_LEVEL] & 0x8000) == 0x8000);

#else
  return;

#endif

}   /* TPUGetPinES() */


/************************************************
   DRIVE FUNCTIONS
*************************************************/

int move_rov(char direction) {

  switch(direction) {
    case 'f':
      outauxport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outauxport(1, SETBIT, MOT1A + MOT2A);           /* Turn on motor relays forward */
      break;
    case 'b':
      outauxport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outauxport(1, SETBIT, MOT1B + MOT2B);
      break;
    case 'l':
      outauxport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outauxport(1, SETBIT, MOT1B + MOT2A);
      break;
    case 'r':
      outauxport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B);
      outauxport(1, SETBIT, MOT1A + MOT2B);
      break;
    case 's':
      outauxport(1, CLEARBIT, MOT1A+MOT1B+MOT2A+MOT2B); /* Turn off both motor relays */
      break;
  }

#ifdef __TURBOC__
  g_test_direction = direction;
#endif

  return(0);
}


/************************************************
	   RACK FUNCTIONS
************************************************/

int move_rack(char direction)  {

  switch(direction) {
    case 'u':
      outauxport(2, CLEARBIT, MOT1A+MOT1B);
      outauxport(2, SETBIT, MOT1A);  /* Turn rack motor on upward */
      break;
    case 'd':
      outauxport(2, CLEARBIT, MOT1A+MOT1B);
      outauxport(2, SETBIT, MOT1B);
      break;
    case 's':
      outauxport(2, CLEARBIT, MOT1A+MOT1B);  /* Turn off rack motor */
      break;
  }
  return(0);
}


/************************************************
	 CURRENT METER FUNCTIONS
************************************************/

int move_currmet(char direction)  {

  switch(direction) {
    case 'u':
      outauxport(2, CLEARBIT, MOT2A+MOT2B);
      outauxport(2, SETBIT, MOT2A);  /* Turn current meter motor on upward */
      break;
    case 'd':
      outauxport(2, CLEARBIT, MOT2A+MOT2B);
      outauxport(2, SETBIT, MOT2B);
      break;
    case 's':
      outauxport(2, CLEARBIT, MOT2A+MOT2B);  /* Turn off current meter motor */
      break;
  }
  return(0);
}

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

short cmvane(void){

#ifdef AZTEC_C
  short dir;

  cmvane_pwr_on();
  DelayMilliSecs(50);
  dir = get_ad_counts(CMVANECHAN);
  cmvane_pwr_off();
  return dir;

#else
  return 0;
#endif
}

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

short cmvane_deg(void){

#ifdef AZTEC_C
  return (short)(cmvane()/CMVANECAL+180)%360;   /* Relative heading */

#else                                      /* Turbo C Simulation */
  g_test_cmdir = (g_test_cmdir + 5) % 360;
  printf("\ncmvane_deg = g_test_cmdir");
  return g_test_cmdir;
#endif
}

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

void cmrotor(void){

#ifdef AZTEC_C
  static int then;
  int now;

  TPUSetPin(CMROTOR_PWR, 1);   /* Turn on photo-diode */
  DelayMilliSecs(10);
  now = TPUGetPin(CMROTOR);
  TPUSetPin(CMROTOR_PWR, 0);   /* Turn off photo-diode */

  if(now != then){
    g_cmrotor_ticks++;
    then = now;
  }

/*  printf("\nChecking cmrotor"); */

#else
  g_cmrotor_ticks++;
/*  printf("\ng_cmrotor_ticks = %d", g_cmrotor_ticks); */
 
#endif
}

/******************************************************
		Video Functions
******************************************************/

int video(int command){

   switch(command){
     case PWR_ON_1:  /* Power Video 1 */
       outauxport(3, SETBIT, POWER7);
       delaysecs(1);
       setcomchan(5);
       viscapacket(VISCA_INIT);
       viscapacket(VISCA_ADDRESS);
       viscapacket(VISCA_PWR_ON);
       printf("\nVideo 1 power on");
       break;

     case PWR_ON_2:  /* Power Video 2 */
       outauxport(3, SETBIT, POWER8);
       delaysecs(1);
       setcomchan(6);
       viscapacket(VISCA_INIT);
       viscapacket(VISCA_ADDRESS);
       viscapacket(VISCA_PWR_ON);
       printf("\nVideo 2 power on");
       break;

     case RECORD_1:  /* Record Video 1 */
       setcomchan(5);
       viscapacket(VCR_RECORD);
       printf("\nVideo 1 record");
       break;

     case STOP_1:  /* Stop Video 1 */
       setcomchan(5);
       viscapacket(VCR_STOP);
       printf("\nVideo 1 stop");
       break;

     case RECORD_2:  /* Record Video 2 */
       setcomchan(6);
       viscapacket(VCR_RECORD);
       printf("\nVideo 2 record");
       break;

     case STOP_2:  /* Stop Video 2 */
       setcomchan(6);
       viscapacket(VCR_STOP);
       printf("\nVideo 2 stop");
       break;

     case SHUT_DOWN:  /* Shut down Both Videos */
       setcomchan(5);
       viscapacket(VCR_STOP);
       setcomchan(6);
       viscapacket(VCR_STOP);
       printf("\nBoth videos stopped, 10 seconds to power off...");
       delaysecs(10);
       outauxport(3, CLEARBIT, POWER7 + POWER8);
       break;
   }
   return 0;
}

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

void viscapacket(int command)  /* VISCA packets */
{
  int msglen;
  unsigned char *packptr;

  switch(command){
    case 0:
      packptr = visca_init;
      break;
    case 1:
      packptr = visca_address;
      break;
    case 2:
      packptr = visca_pwr_on;
      break;
    case 3:
      packptr = visca_pwr_off;
      break;
    case 4:
      packptr = vcr_record;
      break;
    case 5:
      packptr = vcr_stop;
      break;
  }

  /* Find the message length and move 'em out */
  msglen = *packptr;
  packptr++;

  while(msglen){
#ifdef AZTEC_C
    TSerPutByte( TPUSERTX, (int)*packptr);
#endif
/*    printf("%02x", *packptr);  */
    packptr++;
    msglen--;
  }

}  /* viscapacket() */

/******************************************************
	     Mission Functions
******************************************************/


int transit_rov(int transit_dist){
  long start_time, data_store_time, filmcam_time;
  short errcode = 0;

  sensor_pwr_on();
  delaysecs(1);

  start_time = TensMilliSecs();    /* Initialize the timer variables */
  data_store_time = start_time;
  filmcam_time = start_time;

  strobe_pwr_on();
  move_rov('f');                        /* Let's go! */
  tpurupt(ODOMETER, ON);                /* Odometer on */
  g_odometer_ticks = 0;                 /* Reset distance counter */
  SerInFlush();

  while(1){    /* Transit Loop */

	  /* Time to store data? */
    if( timer(data_store_time, TRANSIT_DATA_RATE ) ){
      g_heading = heading();
      store_data(TRANSIT);
      data_store_time = TensMilliSecs();    /* Reset timer variable */
    }

	     /* Too much time? */
    if( timer(start_time, TRANSIT_TIMEOUT) ){
      errcode = (TRANSIT_TIMEOUT_ERR);
      break;
    }

	     /* Take a picture ? */
    if( timer(filmcam_time, FILMCAM_RATE) ){
      filmcam_trig_on();                     /* Trigger strobe/camera */
      DelayMilliSecs(TRIGGERTIME);
      filmcam_trig_off();
      filmcam_time = TensMilliSecs();;       /* Reset timer variable */
    }

	   /* Far enough? */
    if( g_odometer_ticks > transit_dist ) break;   

	    /* Simulation function */
#ifdef __TURBOC__
    DelayMilliSecs(1000);
    g_odometer_ticks += 4;    /* Increment the odometer for simulation */
    printf("\nOdometer ticks = %d", g_odometer_ticks);
#endif

      /* "e" to get us out of here if needed */
    if(SerByteAvail()){
      if(SerGetByte() == 'e') break;
    }

  }  /* End of Transit Loop */

  strobe_pwr_off();
  tpurupt(ODOMETER, OFF);
  move_rov('s');         /* We're there--hopefully */
  store_data(errcode);  /* Store the errorcode just in case */
  return errcode;
}

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

int turn_rov(int new_heading){
  long starttime, data_store_time;
  short headingdiff, errcode = 0, turn_dir = 0;

  sensor_pwr_on();
  delaysecs(1);

  SerInFlush();
  starttime = TensMilliSecs();

      /* Find and correct heading difference */
  g_heading = heading();
  headingdiff = new_heading - g_heading;
  if(headingdiff > 180) headingdiff -= 360;
  if(headingdiff < (-180)) headingdiff += 360;

      /* Decide which way to turn */
  if(headingdiff > 0){
    move_rov('r');
    turn_dir = TURN_RIGHT;
  }
  else if(headingdiff < 0){
    move_rov('l');
    turn_dir = TURN_LEFT;
  }

  DelayMilliSecs(500);        /* Let Compass Settle */

  while(1){                   /* Turn loop */
    g_heading = heading();
    store_data(turn_dir);
    data_store_time = TensMilliSecs();

     /* Find and correct heading difference */
    headingdiff = new_heading - g_heading;
    if(headingdiff > 180) headingdiff -= 360;
    else if(headingdiff < (-180)) headingdiff += 360;

      /* Are we there yet? */
    if(headingdiff <= 0 && turn_dir == TURN_RIGHT) break;                         /* We're there */
    if(headingdiff >= 0 && turn_dir == TURN_LEFT) break;
    if(headingdiff == 0) break;

	    /* Too much time? */
    if( timer( starttime, TURN_TIMEOUT ) ){
      errcode = (TURN_TIMEOUT_ERR);
      break;
    }

	    /* Hang out here awhile */
    while( timer(data_store_time, TURN_DATA_RATE));

	   /* "e" to get out of here */
    if(SerByteAvail()){
      if(SerGetByte() == 'e') break;
    }

  }   /* End of turn loop */

  move_rov('s');        /* End of turn */
  store_data(errcode);  /* Store error code just in case */
  return errcode;
}

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

int rack_down_cycle(void){
  int errcode = 0, last_leadscrew_ticks, rackloops;
  long starttime, data_store_time;

  sensor_pwr_on();
  delaysecs(1);

  SerInFlush();
  starttime = TensMilliSecs();     /* Start time for timeout */
  data_store_time = TensMilliSecs();
  tpurupt(LEADSCREW1, ON);         /* Turn on interrupts */
  tpurupt(LEADSCREW2, ON);
  g_leadscrew1_ticks = 0;
  g_leadscrew2_ticks = 0;
  last_leadscrew_ticks = 0;

  move_rack('d');         /* Move rack down */
  
  while(1){

		  /* Store some data? */
    if( timer(data_store_time, RACK_DATA_RATE) ){
      g_heading = heading();
      store_data(RACK_DOWN);
      data_store_time = TensMilliSecs();

	    /* Are both leadscrews turning ? */
      if( g_leadscrew1_ticks == last_leadscrew_ticks ){
	errcode = RACK_JAM_ERR;
	break;
      }
      else last_leadscrew_ticks = g_leadscrew1_ticks;
    }

#ifdef __TURBOC__
  g_leadscrew1_ticks++;
  g_leadscrew2_ticks++;
  DelayMilliSecs(2000);
#endif

	 /* Watch for limit-switch */
      if( limit_dwn() ){                    
	errcode = (RACK_LIMITDWN_ERR);
	break;
      }
	       /* Leadscrew not turning? */
      if( abs(g_leadscrew1_ticks - g_leadscrew2_ticks) > LEADSCREW_DIFF_MAX ){
	errcode =  (RACK_LEADSCREW_ERR);
	break;
      }
	      /* Too much time? */
      if(timer(starttime, RACK_TIMEOUT)){
	errcode = (RACK_TIMEOUT_ERR);
	break;
      }
	    /* Watch the limit pads */
      if( limit_pad1() | limit_pad2() ) break;

	    /* Simulate rack down by pressing "e" */
      if(SerByteAvail()){
	if(SerGetByte() == 'e') break;
      }

  }         /* End of rack down main loop */

  move_rack('s');              /* Benthic chambers planted--hopefully */
  tpurupt(LEADSCREW1, OFF);
  tpurupt(LEADSCREW2, OFF);
  store_data(errcode);          /* Store error code just in case */
  return errcode;
}

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

int rack_up_cycle(void){
  int errcode = 0, last_leadscrew_ticks, rackloops;
  long starttime, data_store_time;

  sensor_pwr_on();
  delaysecs(1);

  SerInFlush();
  starttime = TensMilliSecs();    /* Start time in tens of milliseconds */
  data_store_time = TensMilliSecs();
  tpurupt(LEADSCREW1, ON);
  tpurupt(LEADSCREW2, ON);
  g_leadscrew1_ticks = 0;
  g_leadscrew2_ticks = 0;
  last_leadscrew_ticks = 0;

  move_rack('u');                              /* Move rack up */

	      /* Rack up loop */
  while(1){                                     

       /* Store data and check for rack jam */
    if( timer(data_store_time, RACK_DATA_RATE) ){
      g_heading = heading();
      store_data(RACK_UP);
      data_store_time = TensMilliSecs();

	 /* Are both leadscrews turning? */
      if( g_leadscrew1_ticks == last_leadscrew_ticks ){
	errcode = RACK_JAM_ERR;
	break;
      }
      else last_leadscrew_ticks = g_leadscrew1_ticks;
    }

#ifdef __TURBOC__
  g_leadscrew1_ticks++;
  g_leadscrew2_ticks++;
  DelayMilliSecs(2000);
#endif

	/* Watch for limit-switch */
      if( limit_up() ){
	errcode = (RACK_LIMITUP_ERR);
	break;
      }
	     /* Only one leadscrew turning? */
      if( abs(g_leadscrew1_ticks - g_leadscrew2_ticks) > LEADSCREW_DIFF_MAX ){
	errcode = (RACK_LEADSCREW_ERR);
	break;
      }
	      /* Too much time? */
      if(timer(starttime, RACK_TIMEOUT)){
	errcode = (RACK_TIMEOUT_ERR);
	break;
      }
	    /* Is the rack up? */
      if( rack_up() ) break;

	    /* Simulate rack up by pressing "e" */
      if(SerByteAvail()){
	if(SerGetByte() == 'e') break;
      }


  }   /* End of rack up main loop */

  move_rack('s');                /* Rack is up -- hopefully */
  tpurupt(LEADSCREW1, OFF);
  tpurupt(LEADSCREW2, OFF);
  store_data(errcode);       /* Store error code just in case */
  return errcode;
}
/************************************************/

int currmet_up(void){
  int errcode = 0, last_cmmot_shaft_ticks = 0, cmmot_loops;
  long starttime, data_store_time;

  sensor_pwr_on();
  delaysecs(1);

  starttime = TensMilliSecs();     /* Start time for timeout */
  data_store_time = TensMilliSecs();
  tpurupt(CMMOT_SHAFT, ON);         /* Turn on interrupt */
  g_cmmot_shaft_ticks = 0;

#ifdef __TURBOC__
  g_cmmot_shaft_ticks++;  /* Avoids a jam error on first loop */
#endif

  move_currmet('u');
  SerInFlush();

	    /* Current meter up main loop */
  while(1){

       /* Store Data and Check for Jam Error */
    if( timer(data_store_time, CMMOT_DATA_RATE) ){
      g_heading = heading();
      data_store_time = TensMilliSecs();
      store_data(CURRMET_UP);

	    /* Is the leadscrew turning ? */
      if( g_cmmot_shaft_ticks == last_cmmot_shaft_ticks ){
	errcode = CMMOT_JAM_ERR;
	break;
      }
      else last_cmmot_shaft_ticks = g_cmmot_shaft_ticks;


    }  /* End of sample if statement */

#ifdef __TURBOC__
      g_cmmot_shaft_ticks += 100;   /* Simulate shaft turning */
#endif


	      /* Too much time? */
      if(timer(starttime, CMMOT_TIMEOUT)){
	errcode = CMMOT_TIMEOUT_ERR;
	break;
      }

	    /* Is the current meter up? */
      if( g_cmmot_shaft_ticks > CURRMET_UP_POS ) break;

	  /* Get us out of here if needed */
      if(SerByteAvail()){
	if(SerGetByte() == 'e') break;
      }

  }   /* End of Current Meter up loop */

  move_currmet('s');              /* Current meter is up--hopefully */
  tpurupt(CMMOT_SHAFT, OFF);
  store_data(errcode);          /* Store error code just in case */

  return errcode;
}


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

int currmet_down(void){
  int errcode = 0, last_cmmot_shaft_ticks = 0, cmmot_loops;
  long starttime, data_store_time;

  sensor_pwr_on();
  delaysecs(1);

  starttime = TensMilliSecs();     /* Start time for timeout */
  data_store_time = TensMilliSecs();
  tpurupt(CMMOT_SHAFT, ON);         /* Turn on interrupt */
  g_cmmot_shaft_ticks = 0;

#ifdef __TURBOC__
  g_cmmot_shaft_ticks++;
#endif

  move_currmet('d');         /* Move current meter down */
  SerInFlush();

	    /* Current meter down loop */
  while(1){

       /* Store Data and Check for Jam Error */
    if( timer(data_store_time, CMMOT_DATA_RATE) ){
      g_heading = heading();
      data_store_time = TensMilliSecs();
      store_data(CURRMET_DOWN);

	    /* Is the leadscrew turning ? */
      if( g_cmmot_shaft_ticks == last_cmmot_shaft_ticks ){
	errcode = CMMOT_JAM_ERR;
	break;
      }
      else last_cmmot_shaft_ticks = g_cmmot_shaft_ticks;

#ifdef __TURBOC__
      g_cmmot_shaft_ticks++;
#endif

    }  /* End of Store Data if statement */

	      /* Too much time? */
      if(timer(starttime, CMMOT_TIMEOUT)){
	errcode = CMMOT_TIMEOUT_ERR;
	break;
      }

	    /* Is the current meter down? */
      if( cm_limit() ) break;

       /* "e" to get us out of here */
      if(SerByteAvail()){
	if(SerGetByte() == 'e') break;
      }

  }   /* End of Current Meter down loop */


  move_currmet('s');              /* Current meter is down--hopefully */
  tpurupt(CMMOT_SHAFT, OFF);
  store_data(errcode);          /* Store error code just in case */

  return errcode;
}


/*********************************************
	    Misc. Functions
*********************************************/

int timer(long starttime, long maxtime){
  long timediff;

/*
*   starttime is absolute time from RTC in tens of milliseconds.
*   maxtime is the timeout period in tens of milliseconds.
*   function returns 1 upon timeout.
*/

  timediff = TensMilliSecs() - starttime;
  if(timediff < 0) timediff += 8.64e6;   /* Correct for rollover at midnight*/
  if(timediff >= maxtime) return 1;
    else return 0;
}

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

void delaysecs(unsigned long delay_time){

  DelayMilliSecs(delay_time*1000);
}

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

void rov_sleep(unsigned long sleep_time){
  unsigned long t;

  Sleep(0);

  for(t=0; t<=sleep_time; t++){
    Sleep(TICK_RATE);   /* 1 second */
    transpond();
  }
}

/**************************************************
		Transpond Function
***************************************************/

void transpond(void){
  int x;
  static int transpond_in, pings;
  static long lock_time;

/*  printf("\nChecking transponder");  */

  if( TPUGetPin(TRANSPOND_IN) == 0 && transpond_in == 0 ){
    transpond_in = 1;
    lock_time = TensMilliSecs();
    printf("\nPing in!");
  }
  if( transpond_in ){
    if( !timer(lock_time, TRANSPOND_LOCK_TIME) && pings < g_program_level){
      TPUSetPin(TRANSPOND_OUT, 0);
      for(x=0; x<10; x++);           /* Do the ping thing */
      TPUSetPin(TRANSPOND_OUT, 1);
      pings++;
    }
    else{
      transpond_in = 0;
      pings = 0;
    }
  }
}