//Ultrasound Pulse and detect routine
//Returns distance in 100um (or tenths of mm)

#include "ses.h"
#include "timer.h"
//#include "ad_lib.h"
#include "shiftreg.h"
//#include "kbhit.h"

/*---------------- Module Defines ---------------------------*/
//#define ULTRASONIC_PULSE_PERIOD 52 //in desired mS
//#define ULTRASONIC_TIMEOUT 10  // in 4mS ticks.  10 ticks=40mS
//#define SAMPLES_PER_POINT 1 //number of chirp cycles to average 
		  					  //to get the distance.  if commented out it
							  //must be now a passed variable. 955am Sunday BR
#define DEBUG 0
/*---------------- Global Variables -------------------------*/
//static unsigned int wallDistance[ROTATIONAL_SAMPLE_POINTS];
static unsigned int	samplePointNum;
//static unsigned int sampleArrayForAveraging[SAMPLES_PER_POINT];
static unsigned int sampleArrayForAveraging[1];

/*---------------- Function Prototypes ----------------------*/
void sendGoPulse(void);
unsigned int averageSampleDistances (unsigned int samplesToAverage);
unsigned int doSample (void);

/*---------------- Module Code -----------------------------*/

unsigned int getUltrasoundSample (unsigned int samplesToAverage) {
//unsigned char getUltrasonicDistance (unsigned char motorIndex) { 
//TMR_Init() 4mS must have been called prior to this routine!  but not in this routine!
unsigned int thisSampleDistance;  //this single-sampled distance
unsigned int theDistance; 	   //post-averaging distance
unsigned int s; 	 		   //counter
//unsigned char motorIndex; 	   //for testing--remove once modularized
//motorIndex = 77; //for testing--remove once modularized
if (DEBUG) printf("\nstarting getUltrasoundSample, averaging %u", samplesToAverage);

for (s=0; s 1) {
        //stick it in array sampleArrayForAveraging[s]
        sampleArrayForAveraging[s] = thisSampleDistance;
if (samplesToAverage > 1) {
    theDistance = averageSampleDistances(samplesToAverage);
	if (DEBUG) printf("\n Avg %d: %u", samplesToAverage, theDistance);
else {
    theDistance = thisSampleDistance;
	if (DEBUG) printf("\n 1 sample: %u", theDistance);
return theDistance;
//else wallDistance[sample point number] = thisSampleDistance;
//stick it in array wallDistance[sample point number]


void sendGoPulse(void)
    //send out as quick a pulse as possible on G0.  This seems to be ~100uS.
uchar initialPORTGdata;
uchar pulseUp;
uchar pulseDown;
	// WritePORTG(0xE3);  //Test with alternate values in it.  Should go to e2 after 1st cycle
initialPORTGdata = ReadPORTG();
pulseUp = 0x01 | initialPORTGdata;
/*	if (DEBUG) { 
    	pulseDown = 0xFE & pulseUp;
    	printf("\n chirp request pulse going out."); 
    	}  */
WritePORTG(0x01 | initialPORTGdata);
WritePORTG(0xFE & pulseUp);			//this up-down thing takes about 100uS
	//if (DEBUG) printf("\n %x %x %x", initialPORTGdata, pulseUp, pulseDown);
	//if (DEBUG) printf("\n pulse out, now: %u", ReadPORTG());

unsigned int doSample (void)
unsigned int echoPulseStart;
unsigned int echoPulseEnd;
unsigned int interval;
unsigned int distance100um;  //in tenths of mm for bit filling

    //send the pulse
	if (DEBUG) printf("\n calling sendGoPulse");
    //wait for the return pulse to go high
	//Temp commented out
/*while (!(ReadPORTF() & 0x01)){     //empty loop   
	  printf("\n RPF hi"); 
    //time the return
echoPulseStart = TCNT;
while (ReadPORTF() & 0x01) {  		  //F0! while it is 1 or pulse returning
   //if (DEBUG) printf("\n This Echo Pulse end TCNT count %u ", echoPulseEnd);
echoPulseEnd = TCNT;	   			  //set the timer value, after it is off, you'll have it

    interval = (echoPulseEnd - echoPulseStart) / 2;
    //if (DEBUG) printf("\n %u uS", interval);

distance100um = (echoPulseEnd - echoPulseStart) * .86;
    if (DEBUG) printf("\n %u ", distance100um);

return distance100um;

unsigned int averageSampleDistances (unsigned int samplesToAverage) 
    double summationOfDistances;
	unsigned int thisSample;
    double averageDistance;
    unsigned int i;
    summationOfDistances = 0;
    for (i=0; i