/* "ROVLIB"  ROVER FUNCTION LIBRARY */

#include <tt7.h>
#include <stdlib.h>
#include "rovsys.h"
#include "rovlib.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 port12data, port34data;

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

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

#ifdef AZTEC_C
    *IOPORT1 = port12data;  /* Write 16 bit word to OUT0-OUT15 */
#endif
  }

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

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

#ifdef AZTEC_C
#else
    printf("\n\nPower status:");
    if(port12data & MOT1A && port12data & MOT2A) printf("\nMove Rov FWD"); //OUT0-OUT7
    if(port12data & MOT1B && port12data & MOT2B) printf("\nMove Rov BACK");
    if(port12data & MOT1B && port12data & MOT2A) printf("\nMove Rov LEFT");
    if(port12data & MOT1A && port12data & MOT2B) printf("\nMove Rov RIGHT");

    if(port12data & AUX1) printf("\nStbd flood on");
    if(port12data & AUX2) printf("\nPort flood on");
    if(port12data & AUX3) printf("\nAUX3A on");

    port12data >>= 8;       	//Shift right to test upper eight bits

    if(port12data & MOT1A) printf("\nMove Rack UP");           //OUT8-OUT15
    if(port12data & MOT1B) printf("\nMove Rack DOWN");
    if(port12data & MOT2B) printf("\nMove Current Meter DOWN");
    if(port12data & MOT2A) printf("\nMove Current Meter UP");

    if(port12data & AUX1) printf("\nStrobe on");
    if(port12data & AUX2) printf("\nAUX2B on");
    if(port12data & AUX3) printf("\nAUX3B on");

    if(port34data & POWER1) printf("\nStbd bencham power on"); //OUT16-OUT23
    if(port34data & POWER2) printf("\nPort bencham power on");
    if(port34data & POWER3) printf("\nMicroprofiler power on");
    if(port34data & POWER4) printf("\nTransponder power on");

    if(port34data & POWER5) printf("\nAux. power on");
    if(port34data & POWER6) printf("\nSensor power on");
      else printf("\nSensor power off");
    if(port34data & POWER7) printf("\nStbd video power on");
    if(port34data & POWER8) printf("\nPort video power on");
#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);
}


/************************************************************************
   TPU Serial I/O Functions
*************************************************************************/

/*************************************************************************
**	TSerGetByteTimed		Timed Input Byte Routine
**
**	Return next serial input byte or -1 if timeout
**************************************************************************/

int TSerGetByteTimed(unsigned long mS) {
  long start_time, time_diff;

  start_time = MilliSecs();

  while(1){
    time_diff = MilliSecs() - start_time;
    if(time_diff < 0) time_diff += 8.64e7;   /* Correct for rollover at midnight*/
    if(time_diff >= mS) return -1;
    else if(TSerByteAvail(13)) return TSerGetByte(13);
  }
}

/**************************************
      TGetsTimed

Input: Pointer to input 80 character buffer
Returns: 0 on success, -1 on error

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

int TGetsTimed(ulong mS, char *str) {
  char ch;
  int t=0;

  while(1){
    ch = (char)TSerGetByteTimed(mS);
    if(ch == '\n') break;
    else if(ch == '\r') break;
    else if(ch == (-1)) return (-1);
    else str[t] = ch;
    t++;
  }
  str[t] = '\0';
  return 0;
}

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

void TPuts(char *str) {
  while(*str) TSerPutByte(12, (int)*str++);
  return;
}

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

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);

  DelayMilliSecs(10);       /* Wait for output glitch to clear */
  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 transition 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
************************************************/


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

int video(int command){

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

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

     case RECORD_STBD:  /* Record Stbd Video */
       setcomchan(5);
       viscapacket(VCR_RECORD);
       printf("\nStbd video record");
       break;

     case STOP_STBD:  /* Stop Stbd Video */
       setcomchan(5);
       viscapacket(VCR_STOP);
       printf("\nStbd video stop");
       break;

     case RECORD_PORT:  /* Record Port Video */
       setcomchan(6);
       viscapacket(VCR_RECORD);
       printf("\nPort video record");
       break;

     case STOP_PORT:  /* Stop Port Video */
       setcomchan(6);
       viscapacket(VCR_STOP);
       printf("\nPort video stop");
       break;

     case SHUT_DOWN:  /* Shut down Both Videos */
       setcomchan(5);
       viscapacket(VCR_STOP);
       delaysecs(1);
       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(STBD_LEADSCREW, ON);         /* Turn on interrupts */
  tpurupt(PORT_LEADSCREW, ON);
  g_stbd_leadscrew_ticks = 0;
  g_port_leadscrew_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_stbd_leadscrew_ticks == last_leadscrew_ticks ){
	errcode = RACK_JAM_ERR;
	break;
      }
      else last_leadscrew_ticks = g_stbd_leadscrew_ticks;
    }

#ifdef __TURBOC__
  g_stbd_leadscrew_ticks++;
  g_port_leadscrew_ticks++;
  DelayMilliSecs(2000);
#endif

	 /* Watch for limit-switch */
      if( limit_dwn() ){                    
	errcode = (RACK_LIMITDWN_ERR);
	break;
      }
	       /* Leadscrew not turning? */
      if( abs(g_stbd_leadscrew_ticks - g_port_leadscrew_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( stbd_limit_pad() | port_limit_pad() ) 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(STBD_LEADSCREW, OFF);
  tpurupt(PORT_LEADSCREW, 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(STBD_LEADSCREW, ON);
  tpurupt(PORT_LEADSCREW, ON);
  g_stbd_leadscrew_ticks = 0;
  g_port_leadscrew_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_stbd_leadscrew_ticks == last_leadscrew_ticks ){
	errcode = RACK_JAM_ERR;
	break;
      }
      else last_leadscrew_ticks = g_stbd_leadscrew_ticks;
    }

#ifdef __TURBOC__
  g_stbd_leadscrew_ticks++;
  g_port_leadscrew_ticks++;
  DelayMilliSecs(2000);
#endif

	/* Watch for limit-switch */
      if( limit_up() ){
	errcode = (RACK_LIMITUP_ERR);
	break;
      }
	     /* Only one leadscrew turning? */
      if( abs(g_stbd_leadscrew_ticks - g_port_leadscrew_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(STBD_LEADSCREW, OFF);
  tpurupt(PORT_LEADSCREW, 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){     /* Sleep time in mins. */
  unsigned long t;

  sleep_time *= 60;  	/* Convert to seconds */
  Sleep(0);

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

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

void transpond(void){
  int ping_in, x;
  static int transpond_in_flag, pings;

  if( transpond_in_flag == 0 && TPUGetPin(TRANSPOND_IN) == 0 ){
    transpond_in_flag = 1;

  }
  if( transpond_in_flag ){
    if( pings < g_program_level ){
      TPUSetPin(TRANSPOND_OUT, 0);
      for(x=0; x<10; x++);           /* Do the ping thing */
      TPUSetPin(TRANSPOND_OUT, 1);
      pings++;
      printf("\nPing!");
    }
    else{
      transpond_in_flag = 0;
      pings = 0;
    }
  }
}

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

void filmcam(short num_pictures){
  short x;

  strobe_pwr_on();

  for(x=0;x<num_pictures;x++){
    delaysecs(5);               //Let strobe charge
    filmcam_trig_on();          //Trigger strobe
    DelayMilliSecs(TRIGGERTIME);
    filmcam_trig_off();      	//Advance film
    printf("\nTime-lapse picture");
    delaysecs(2);		//Let film motor finish
  }

  strobe_pwr_off();
}