/*
By Andrew Chanler - Fall 2003
some general handyboard routines for use with GCC and GEL
this file provides some functions that do similar things to Interactive C's built in functions
please refer to http://www.cs.uml.edu/~achanler/robotics/
*/

#include <stdio.h>
#include <stdlib.h>
#include <sys/sio.h>



int digital(int);
int stop_button(void);
int start_button(void);
void stop_press(void);
void start_press(void);
unsigned char knob(void);
unsigned char read_analog(unsigned char port);
unsigned char analog(unsigned char port);
void init_analog(void);
void set_digital_out(unsigned char x);
void clear_digital_out(unsigned char x);


unsigned char digital_out_data =0;

void set_digital_out(unsigned char x)
{
	if(x > 8)  return;
	unsigned char t = 1;
	while(x--) t = t << 1;
	*(unsigned char *)0x5000 = digital_out_data = digital_out_data | t;
}

void clear_digital_out(unsigned char x)
{
	if(x > 8) return;
	unsigned char t = 1;
	while(x--) t = t << 1;
	*(unsigned char *)0x5000 = digital_out_data = digital_out_data & (!t);
}


unsigned char analog(unsigned char port)
{
	if((port >  1) && (port <  7)) return read_analog(port);
/*
	if((port > 15) && (port < 24))
	{
		// setup exp board multiplexer
		port = (port - 16) << 5;
		(*(unsigned char *) 0x4000) = port;
		return read_analog(0);
	}
	if((port > 23) && (port < 32))
	{
		// setup exp board multiplexer
		port = (port - 24) << 5;
		(*(unsigned char *) 0x4000) = port;
		return read_analog(1);
	}
*/
	return 255;
}

unsigned char knob()
{
	return read_analog(7);
}



unsigned char read_analog(unsigned char port)
{
  if(port > 3)
  {
	_io_ports[M6811_ADCTL] = 0x34;
	port-=4;
  }else _io_ports[M6811_ADCTL] = 0x30;

  while( !(_io_ports[M6811_ADCTL] & 0x80) );

  return (_io_ports[M6811_ADR1 + port]);
}


void init_analog()
{
  /* enable A/D converter */
  _io_ports[M6811_OPTION] |= 0x80;

  /* set A/D system for continous scan, multiple channel */
  _io_ports[M6811_ADCTL] = 0x30;
}




int digital(int x)
{
	// digital 7, 8 and 9 inputs are not implemented
	unsigned char temp = *((unsigned char *)0x7000);
	
	return(((x == 10)&& ((temp & 0x01)) == 0 ) ||
		 ((x == 11)&& ((temp & 0x02)) == 0 ) ||
		 ((x == 12)&& ((temp & 0x04)) == 0 ) ||
		 ((x == 13)&& ((temp & 0x08)) == 0 ) ||
		 ((x == 14)&& ((temp & 0x10)) == 0 ) ||
		 ((x == 15)&& ((temp & 0x20)) == 0 ) );
}

int stop_button()
{
	return ((*((unsigned char *)0x7000) & 0x40) == 0 ) ;
}

int start_button()
{
	return ((*((unsigned char *)0x7000) & 0x80) == 0 ) ;
}

void stop_press()
{
	while(!stop_button());
	while(stop_button());
}


void start_press()
{
	while(!start_button());
	while(start_button());
}