Robocup ball sense code with added LFSR and debug stuff

ball_sense.c

#include <board.h>

#include "main.h"
#include "status.h"
#include "timer.h"
#include "ball_sense.h"
#include "adc.h"

//How much ambient IR light we can handle
//FIXME This number is arbitrary and needs to be chosen experimentally
static const int Dazzle_Threshold = 500;


// How many consecutive cycles the detector input must be stuck at full scale for the
// detector to be declared open-circuited.
static const int Det_Open_Time = 100;

static const int Det_Open_Threshold = 3;

int have_ball;
int ball_sense_light, ball_sense_dark;
int lfsr = 0xACE1u; //starting seed for LFSR
int last_bit = 0;
unsigned bit;
unsigned period = 0;


// This is incremented for each cycle in which the detector input is at full scale.
// If the input stays at full scale, the detector has probably failed open.  A working detector normally passes some current.
int det_open_count;

void update_ball_sensor()
{
	static int wait = 0;

	// Only run every other cycle.
	// ADC conversions are started just before this is called,
	// and we want to wait long enough after toggling the LED to let the detector
	// input settle (due to the large pull-up resistor).
	wait = !wait;
	if (wait)
	{
		return;
	}


	//see if what we just read from the photodiode is what we sent.
	printf('ADC is currently %i\n', adc[4]);
	if(adc[4]==last_bit){
		have_ball = 1;
	} else {
	have_ball = 0;
	}



	//LFSR to modulate the IR LED to try to reduce false positives from reflections on ball
	bit  = ((lfsr >> 0) ^ (lfsr >> 2) ^ (lfsr >> 3) ^ (lfsr >> 5) ) & 1;
	lfsr =  (lfsr >> 1) | (bit << 15);
//end of period

printf('Last bit was %i', last_bit);
printf('\n');
printf('Current bit is %', bit);
if(last_bit){
	//LED was on
	ball_sense_light = 0x3ff - adc[4];
} else {
	//LED was off
	ball_sense_dark = 0x3ff - adc[4];
}



	// Update the ball sensor and toggle its LED.

		//Toggle LED with LFSR
		if(!bit){
		LED_OFF(BALL_LED);
		// Check for emitter failure
				//
				// While the output is low, switch it to input.  If the LED is connected, it will
				// quickly pull the pin high.  If the LED is open, the pin will stay low.
				AT91C_BASE_PIOA->PIO_CODR = BALL_LED;
				AT91C_BASE_PIOA->PIO_ODR = BALL_LED;
				if (!(AT91C_BASE_PIOA->PIO_PDSR & BALL_LED))
				{
					failures |= Fail_Ball_LED_Open;
				} else {
					failures &= ~Fail_Ball_LED_Open;
				}
		// Make the pin output again
				AT91C_BASE_PIOA->PIO_OER = BALL_LED;
		}



		if(bit){
		LED_ON(BALL_LED);
		// Check for detector failures and excessive ambient light
			failures &= ~(Fail_Ball_Det_Open | Fail_Ball_Det_Short | Fail_Ball_Dazzled);
			if (ball_sense_dark <= Det_Open_Threshold && ball_sense_light <= Det_Open_Threshold)
			{
				// Detector may be open (the pullup resistor pulled the ADC input to 3.3V)
				if (det_open_count >= Det_Open_Time)
				{
					failures |= Fail_Ball_Det_Open;
				} else {
					++det_open_count;
				}
			} else {
				// The detector is working
				det_open_count = 0;
			}
		}



	if (ball_sense_dark == 0x3ff && ball_sense_light == 0x3ff)
	{
		// Detector is shorted so the ADC input is fixed at GND
		failures |= Fail_Ball_Det_Short;
	} else if (ball_sense_dark > Dazzle_Threshold)
	{
		// Too much outside light
		failures |= Fail_Ball_Dazzled;
	}

	// Update have_ball and the ball status LED
	if (failures & Fail_Ball)
	{
		// The ball sensor is broken
		have_ball = 0;
	} else {
		// The ball sensor works, so determine if we have the ball
		have_ball = (ball_sense_light - ball_sense_dark) < Unbroken_Beam;
	}


	}

last_bit = bit;

}