rp6controllib.c

RP6机器人范例程序。包括移动

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/             >>> RP6 CONTROL
 * ----------------------------------------------------------------------------
 * ------------------- [c]2006 / 2007 - AREXX ENGINEERING ---------------------
 * -------------------------- http://www.arexx.com/ ---------------------------
 * ****************************************************************************
 * File: RP6ControlLib.c
 * Version: 1.1
 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz
 * Author(s): Dominik S. Herwald
 * ****************************************************************************
 * Description:
 * The RP6 CONTROL M32 function Library.
 *
 * ****************************************************************************
 * CHANGELOG AND LICENSING INFORMATION CAN BE FOUND AT THE END OF THIS FILE!
 * ****************************************************************************
 */

/*****************************************************************************/
// Includes:

#include "RP6ControlLib.h"


/*****************************************************************************/
// External Interrupts and Event Handlers.
// To allow common usage with the RP6Control library, you need to use
// Event Handlers. 
// Please make sure that you keep the Event Handler Functions as
// short as possible! 
// --> You usually don't need this stuff. 

void INT0_event_DUMMY(void){}
static void (*INT0_eventHandler)(void) = INT0_event_DUMMY;
void INT0_setEventHandler(void (*i0eventHandler)(void))
{
	INT0_eventHandler = i0eventHandler;
}

/**
 * External Interrupt 0 ISR
 */
ISR (INT0_vect)
{
	INT0_eventHandler();
}

void INT1_event_DUMMY(void){}
static void (*INT1_eventHandler)(void) = INT1_event_DUMMY;
void INT1_setEventHandler(void (*i1eventHandler)(void))
{
	INT1_eventHandler = i1eventHandler;
}

/**
 * External Interrupt 1 ISR
 */
ISR (INT1_vect)
{
	INT1_eventHandler();
}

void INT2_event_DUMMY(void){}
static void (*INT2_eventHandler)(void) = INT2_event_DUMMY;
void INT2_setEventHandler(void (*i2eventHandler)(void))
{
	INT2_eventHandler = i2eventHandler;
}

/**
 * External Interrupt 2 ISR
 */
ISR (INT2_vect)
{
	INT2_eventHandler();
}

/*****************************************************************************/
// ADC:

/**
 * Read ADC channel (10 bit -> result is an integer from 0 to 1023).
 * The channels (ADC_BAT etc.) are defined in the RP6Control.h file.
 *
 * This is a blocking function, which means it waits until the conversion
 * is complete. 
 *
 */
uint16_t readADC(uint8_t channel)
{
	if((ADCSRA & (1<<ADSC))) return 0; // check if ADC is buisy...
	ADMUX = (1<<REFS0) | (0<<REFS1) | (channel<<MUX0);
	ADCSRA = (0<<ADIE) | (1<<ADSC) | (1<<ADEN) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADIF);
	while ((ADCSRA & (1<<ADSC))); 
	ADCSRA |= (1<<ADIF);
	return ADC;
}

/*****************************************************************************/
// Hardware SPI Interface:

/**
 * Writes a single Databyte to the SPI Interface.
 */
void writeSPI(uint8_t data) 
{     
	SPDR = data;    
	while(!(SPSR & (1<<SPIF)));
}

/**
 * Reads a single Databyte from the SPI Interface.
 */
uint8_t readSPI(void)
{
	writeSPI(0xFF);
	return SPDR;
}

/**
 * Reads TWO Bytes from the SPI Interface and returns them as
 * a 16 Bit value with first byte read in the upper 8 bits.
 */
uint16_t readWordSPI(void)
{
	uint16_t data = 0;
	data = readSPI() << 8;
	data |= readSPI();
	return data;
}

/**
 * Writes two Bytes contained in the 16 Bit parameter "data".
 * The first byte to be written needs to be in the upper 8 Bits.
 */
void writeWordSPI(uint16_t data)
{
	writeSPI((uint8_t)(data >> 8));
	writeSPI((uint8_t)data);
}

/** 
 * This function writes up to 255 Bytes to the SPI Interface.
 * The numer of bytes in the Buffer that shall be written is given 
 * by the parameter length.
 */
void writeBufferSPI(uint8_t *buffer, uint8_t length)
{
	uint8_t i = 0;
	for(; i < length; i++) {
		SPDR = buffer[i];    
		while(!(SPSR & (1<<SPIF)));
	}
}

/** 
 * Reads "length" Bytes from SPI Interface into the buffer.
 */
void readBufferSPI(uint8_t *buffer, uint8_t length)
{
	uint8_t i = 0;
	for(; i < length; i++) {
		SPDR = 0xFF;    
		while(!(SPSR & (1<<SPIF)));
		buffer[i] = SPDR;
	}
}

/*****************************************************************************/
// External 32KB SPI EEPROM:

/**
 * Reads a single Byte from the external EEPROM.
 */
uint8_t SPI_EEPROM_readByte(uint16_t memAddr)
{
	uint8_t data;
	PORTB &= ~MEM_CS;
	writeSPI(SPI_EEPROM_READ);
	writeWordSPI(memAddr);
	data = readSPI();
	PORTB |= MEM_CS;
	return data;
}

/**
 * Reads "length" Bytes into the Buffer "buffer" from startAdr on. 
 * You can read the complete EEPROM into a buffer at once - if it is large enough. 
 * (But you only have 2KB SRAM on a MEGA32 ;) )
 */
void SPI_EEPROM_readBytes(uint16_t startAddr, uint8_t *buffer, uint8_t length)
{
	PORTB &= ~MEM_CS;
	writeSPI(SPI_EEPROM_READ);
	writeWordSPI(startAddr);
	readBufferSPI(&buffer[0], length);
	PORTB |= MEM_CS;
}

/**
 * Enable Write Mode
 */
void SPI_EEPROM_enableWrite(void)
{
	PORTB &= ~MEM_CS;
	writeSPI(SPI_EEPROM_WREN);
	PORTB |= MEM_CS;
}

/**
 * Disable Write Mode
 */
void SPI_EEPROM_disableWrite(void)
{
	PORTB &= ~MEM_CS;
	writeSPI(SPI_EEPROM_WRDI);
	PORTB |= MEM_CS;
}

/**
 * Write a single data byte to the specified EEPROM address.
 */
void SPI_EEPROM_writeByte(uint16_t memAddr, uint8_t data)
{
	while(SPI_EEPROM_getStatus() & SPI_EEPROM_STAT_WIP);
	SPI_EEPROM_enableWrite();
	PORTB &= ~MEM_CS;
	writeSPI(SPI_EEPROM_WRITE);
	writeWordSPI(memAddr);
	writeSPI(data);
	PORTB |= MEM_CS;
}

/**
 * Write "length" Bytes from the Buffer to the EEPROM. 
 * YOU CAN ONLY WRITE MAXIMAL 64 BYTES AT ONCE!!! This is the Pagesize!
 * You can NOT cross a page boundary! For example when you write 20 Bytes 
 * starting at address 54, you will not write up to Byte address 74, but
 * instead only up to 63 and then it continues at Byte 0 and writes 
 * the rest up to Byte 10!
 *
 */
void SPI_EEPROM_writeBytes(uint16_t startAddr, uint8_t *buffer, uint8_t length)
{
	while(SPI_EEPROM_getStatus() & SPI_EEPROM_STAT_WIP);
	SPI_EEPROM_enableWrite();
	PORTB &= ~MEM_CS;
	writeSPI(SPI_EEPROM_WRITE);
	writeWordSPI(startAddr);
	writeBufferSPI(&buffer[0], length);
	PORTB |= MEM_CS;
}

/**
 * Returns EEPROM Status register - for checking if EEPROM is buisy. 
 * Writing takes about 5ms. 
 */
uint8_t SPI_EEPROM_getStatus(void)
{
	uint8_t status;
	PORTB &= ~MEM_CS;
	writeSPI(SPI_EEPROM_RDSR);
	status = readSPI();
	PORTB |= MEM_CS;
	return status;
}

/*****************************************************************************/
// External Port - LEDs and LCD:

// A shadow register for the external shift register:
externalPort_t externalPort;

/**
 * This routine outputs the value in the shadow register "externalPort" 
 * to the external 8bit shift register on the RP6 Control board with the 
 * hardware SPI module.
 * The lower four bits are connected to the four LEDs, and the upper 
 * to the 4bit LCD databus. (The LCD can be configured to operate
 * in 4 or 8bit mode - we use 4bit mode to save portpins) 
 *
 */
void outputExt(void)
{      
  writeSPI(externalPort.byte);   
  PORTD |= STR;
  nop();
  nop();
  PORTD &= ~STR;        
}

/*****************************************************************************/
// LEDs:

/**
 * Sets the LEDs - this does not affect the other four port lines that
 * are connected to the LCD! 
 *
 */
void setLEDs(uint8_t leds)
{
	externalPort.LEDS = leds;
	outputExt();
}

/*****************************************************************************/
// LCD
// All LCD routines are prepared to control a 2x16 character LCD.
// If you want to connect a bigger LCD you need to change some things in 
// these routines! (especially in the initLCD, setCursorLCD and showScreenLCD
// routines)

char lcd_tmp_buffer[17];

/**
 * Sets the LCD ports without affecting the LEDs and also pulses the
 * enable line of the LCD to 'inform' the LCD about the new data.
 *
 */
void setLCDD(uint8_t lcdd)
{
	externalPort.LCDD = lcdd;
	outputExt();
	PORTB |= LCD_EN;
	delayCycles(50);
	PORTB &= ~LCD_EN;
}

/**
 * Initialize the LCD. Always call this before using the LCD! 
 *
 */
void initLCD(void)
{
	//delayCycles(34000); No need for Power ON delay as usually the
	// Bootloader should have been executed before...
	setLCDD(0b0011);
	delayCycles(18000);
	setLCDD(0b0011);
	delayCycles(5500);
	setLCDD(0b0011);
	delayCycles(5500);
	setLCDD(0b0010);
	delayCycles(5500);
	writeLCDCommand(0b00101000);
	delayCycles(5500);
	writeLCDCommand(0b00001000);
	delayCycles(5500);
	writeLCDCommand(0b00000001);
	delayCycles(5500);
	writeLCDCommand(0b00000010);
	delayCycles(5500);
	writeLCDCommand(0b00001100);
	delayCycles(5500);
}

/**
 * Write a 8bit-byte in two nibbles of 4bit to the LCD.
 */
void write4BitLCDData(uint8_t data)
{
	setLCDD(data >> 4);
	setLCDD(data);
	delayCycles(150);
}

/**
 * Write a command to the LCD.
 */
void writeLCDCommand(uint8_t cmd)
{
	PORTB &= ~LCD_RS;
	write4BitLCDData(cmd);
	delayCycles(150);
}

/**
 * Clears the whole LCD!
 */
void clearLCD(void)
{
	writeLCDCommand(0b00000001);
	delayCycles(5500);
}


/**
 * Write a single character to the LCD.
 *
 * Example:
 *
 *			writeCharLCD('R');
 *			writeCharLCD('P');
 *			writeCharLCD('6');
 *			writeCharLCD(' ');
 *			writeCharLCD('0');
 *			writeCharLCD(48); // 48 is ASCII code for '0'
 *			writeCharLCD(49); // '1'
 *			writeCharLCD(50); // '2'
 *			writeCharLCD(51); // '3'
 *			//...
 *
 *			would output:
 *			RP6 00123
 *			at the current cursor position!
 *			use setCursorPos function to move the cursor to a 
 *			different location!
 */
void writeCharLCD(uint8_t ch)
{
	PORTB |= LCD_RS;
	write4BitLCDData(ch);
	delayCycles(50);
}

/**
 * Writes a null terminated string from flash program memory to the LCD.
 * You can use the macro writeStringLCD_P(STRING); instead, this macro
 * ensures that the String is stored in program memory only!
 *
 * Example:
 *
 *			writeNStringLCD_P(PSTR("RP6 Control"));
 *
 *			// There is also a Macro that makes life easier and
 *			// you can simply write:
 *			writeStringLCD_P("RP6 Control");
 *
 */
void writeNStringLCD_P(const char *pstring)
{
    uint8_t c;
    for (;(c = pgm_read_byte_near(pstring++));writeCharLCD(c));
}

/**
 * Writes a String from SRAM to the LCD.
 */
void writeStringLCD(char *string)
{
	while(*string)
		writeCharLCD(*string++);
}

/**
 * Writes a string with specified length and offset from SRAM to the LCD.
 * If it is a null terminated string, output will be stopped at the
 * end. It does not need to be null terminated, but it is recommended
 * to use only null terminated strings/buffers, otherwise the function could
 * output any SRAM memory data stored after the string until it reaches a 0
 * or the specified length!
 *
 * Example:
 *
 *			writeStringLength("RP6 Robot Sytem",16,0);
 *			// would output: "RP6 Robot Sytem\n"
 *			writeStringLength("RP6 Robot Sytem",11,4);
 *			// would output: "Robot System"
 * 			writeStringLength("RP6 Robot Sytem",40,4);
 *			// would output: "Robot System"
 *			// No matter if the specified length is 40 characters!
 *
 */
void writeStringLengthLCD(char *string, uint8_t length, uint8_t offset)
{
	for(string = &string[offset]; *string && length; length--)
		writeCharLCD(*string++);
}

/**
 * Write a number (with specified base) to the LCD.
 *