/*
 *	Uncomment this line to enable debug mode. In this mode, 
 *	motors are disabled and printouts can be made to the 
 *	it68 development environment. 
 */ 
/*#define DEBUG*/

/*
 *	Uncomment this line to disable the distance sensor and 
 *	the servo. 
 */ 
#define SENSE

/* Define addresses for I/O */
#define ADDR_DIODES 0xC0000
#define ADDR_MOTORS 0x80000
#define ADDR_AD 0xE0000
#define ADDR_EEPROM 0x20000
#define PAGE_SIZE 128

/* Motor modes */
#define LEFT 0x38
#define RIGHT 0x32
#define FREE_RUNNING 0x00

/* Number of motor pulses (resolution of motor speed) */
#define NBR_PULSES 10

/* History size */
#define NBR_STORED_OFFSETS 45

/* Number of interrupts for each step in a transition */
#define TIME_CONSTANT 20

/* Circular list structure used to store offset history */
typedef struct listnode listnode;
struct listnode 
{
	listnode *next;
	listnode *prev;
	signed short val;
};

/* Global variables and data structures */
unsigned short *motors;
unsigned short *diodes;
unsigned short *ad;
unsigned short *eeprom;
unsigned short pulses[NBR_PULSES];
listnode history[NBR_STORED_OFFSETS];
listnode *history_head;
unsigned short last_offset;
unsigned short latch_val;
unsigned short pulse_counter;
unsigned int power_counter;
unsigned int servo_counter;
unsigned int servo_pulser;
unsigned short power_left;
unsigned short power_right;
unsigned short motor_pause;
unsigned int pause_counter;
unsigned short page_entries[PAGE_SIZE];
unsigned short curr_column;
unsigned int curr_page;
unsigned short page_full;
unsigned short replay_mode;
signed short last_side;

/* Help functions */
void init(void);
void pulse_servo(void);
void step_playback(void);
void step_normal(void);
void push_to_history(signed short);
signed int calc_derivative(void);
void regulate_PD(signed short, signed int);
signed short read_offset(void);
void set_pulse_array(void);
void set_motors(unsigned short);
void set_diode(unsigned short);
void set_servo(unsigned short);
void write_to_motor_latch(void);
void store_value(unsigned short);
void write_to_eeprom(void);

#ifdef DEBUG
unsigned short sum_history(void);
void print_short(signed short);
#endif