rp6robotbaselib.c

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

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/         >>> BASE CONTROLLER
 * ----------------------------------------------------------------------------
 * ------------------- [c]2006 / 2007 - AREXX ENGINEERING ---------------------
 * -------------------------- http://www.arexx.com/ ---------------------------
 * ****************************************************************************
 * File: RP6RobotBaseLib.c
 * Version: 1.3
 * Target: RP6 Base - ATMEGA32 @8.00MHz
 * Author(s): Dominik S. Herwald
 * ****************************************************************************
 * Description:
 *
 * This is the RP6 Robot Base Library - it contains the following functions:
 * - Processor initialisation
 * - LED Control
 * - Bumpers
 * - A/D Convertor (Motor Current, Light Sensors, Battery Voltage...)
 * - Encoder reading and speed measurement 
 * - Motor Control (Automatic speed control and PWM + Direction control
 *   + failsafe functions (overcurrent and defect motor/encoder detection))
 * - ACS (Anti Collision System) 
 * - IRCOMM (IR-Communication System, RC5 transmission and reception)
 * - Timing functions (Delays, Stopwatches etc.)
 *
 * PLEASE ALSO READ THE RP6MANUAL! THERE YOU WILL FIND EXAMPLE
 * CODE AND SOME FURTHER EXPLANATIONS!
 *
 * In other parts of this library (RP6uart.c, RP6I2CSlaceTWI.c and 
 * RP6I2CMasterTWI.c)
 * you can find UART communication and I睠 Bus communication routines.
 *
 * -----
 * Hint: You should better leave all this as it is if you just started with
 * C programming, but it is a very good idea to read the comments and review 
 * the code, it will help you to understand C programming for AVR better!
 * -----
 *
 * For the experienced users: 
 * This code works OK, but it is not optimal! There is a lot potential for 
 * tuning! 
 * Well, this leaves some tasks for you and this is what makes most 
 * fun: To improve the excisting!  
 *
 * Of course you are free to add new functions and improvements to this
 * library and make them available to the public on the Internet e.g. on
 * our Forum!
 * Please use the changelog at the end of this file to document your
 * changes! And add your name to any new function or modification you added! 
 * E.g. a "modified by <name> at <date>" is always a good idea to show 
 * other users where and WHAT you changed in the source code!
 *
 * It is a good idea to make your own includeable libraries instead of
 * changing this library - of course only if this is possible.
 *
 * Or create your own complete library with all specific functions you need.
 * This code is GPL'd - s. license at the end of this file!
 *
 * ****************************************************************************
 * CHANGELOG AND LICENSING INFORMATION CAN BE FOUND AT THE END OF THIS FILE!
 * ****************************************************************************
 */

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

#include "RP6RobotBaseLib.h"

/*****************************************************************************/

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

// -------------------------------
// Power on warning
// 
#ifdef POWER_ON_WARNING
	uint8_t leds_on;

	/**
	 * Enables the "led-blink" warning after some time with
	 * no active LEDs. This is to ensure that you don't forget
	 * to turn of the Robot if your program does not use
	 * any LEDs for a long time! 
	 */
	void enablePowerOnWarning(void) 
	{ 
		if(leds_on > 3)
			leds_on = 0; 
	}
	
	/**
	 * This disables the power on warning. 
	 * also see RP6Config.h for #define POWER_ON_WARNING
	 */
	void disablePowerOnWarning(void) 
	{ 
		leds_on = 4; 
	}
#endif

/**
 * Update status LEDs with current value from shadow register.
 *
 * Additional info:
 * This function ensures that the LED pins are not driven low to allow
 * other circuitry to be connected to the I/O pads on the mainboard! 
 * As long as external circuits only connect the I/O pads to VCC and not to 
 * GND, everything should work fine, but always connect a >= 470 Ohm 
 * series resistor to limit maximum current!
 *
 *
 * Example:
 *
 *			statusLEDs.byte=0b101001;
 *			updateStatusLEDs();
 *			// this clears all LEDs and sets the LEDs STATUS1,
 *			// STATUS6 and STATUS4!
 *
 *			// Other possibility:
 *			statusLEDs.LED2=true;
 *			updateStatusLEDs();
 *			// This sets LED2 and does not affect any other LED!
 */
void updateStatusLEDs(void)
{
	DDRB &= ~0x83;
	PORTB &= ~0x83;
	if(statusLEDs.LED4){ DDRB |= SL4; PORTB |= SL4; }
	if(statusLEDs.LED5){ DDRB |= SL5; PORTB |= SL5; }
	if(statusLEDs.LED6){ DDRB |= SL6; PORTB |= SL6; }
	DDRC &= ~0x70;
	PORTC &= ~0x70;
	DDRC |= ((statusLEDs.byte << 4) & 0x70);
	PORTC |= ((statusLEDs.byte << 4) & 0x70);
	#ifdef POWER_ON_WARNING
		leds_on = (leds_on ? leds_on : (statusLEDs.byte && 1));
	#endif
}

/**
 * Set status LEDs - this is very handy if you want to set all LEDs.
 *
 * Example:
 *
 *			setLEDs(0b101001);
 *			// this clears all LEDs and sets the LEDs STATUS1,
 *			// STATUS6 and STATUS4!
 */
inline void setLEDs(uint8_t leds)
{
	statusLEDs.byte = leds;
	updateStatusLEDs();
}


/*****************************************************************************/
// Bumpers:

/**
 * Returns true if the the left Bumper is hit. 
 * This function turns off the LED connected to the port, reads the Bumper 
 * value and restores previous LED state afterwards!
 *
 * Example:
 *
 *		if(getBumperLeft())
 *			// do something
 */
uint8_t getBumperLeft(void)
{ 
	PORTB &= ~SL6;
	DDRB &= ~SL6; 
	nop();
	uint8_t tmp = PINB & SL6;
	if(statusLEDs.LED6) { 
		DDRB |= SL6; 
		PORTB |= SL6; 
	}
	return tmp;
}

/**
 * Returns true if the the right Bumper is hit. 
 * This function turns off the LED connected to the port, reads the Bumper 
 * value and restores previous LED state afterwards!
 *
 * Example:
 *
 *		if(getBumperRight())
 *			// do something
 */
uint8_t getBumperRight(void)
{
	PORTC &= ~SL3;
	DDRC &= ~SL3; 
	nop();
	uint8_t tmp = PINC & SL3;
	if(statusLEDs.LED3) { 
		DDRC |= SL3; 
		PORTC |= SL3; 
	}
	return tmp;
}

// -------------------------------
// Bumpers State changed handler:

void BUMPERS_stateChanged_DUMMY(void){}
static void (*BUMPERS_stateChangedHandler)(void) = BUMPERS_stateChanged_DUMMY;
/**
 * Use this function to set the Bumpers state change handler. 
 * 
 */
void BUMPERS_setStateChangedHandler(void (*bumperHandler)(void)) 
{
	BUMPERS_stateChangedHandler = bumperHandler;
}
// -------------------------------

volatile uint8_t bumper_timer;
uint8_t bumper_left;
uint8_t bumper_right;

/**
 * If you call this frequently out of the mainloop (or use task_RP6System which
 * calls this routine for you), the global bumper_left and bumper_right
 * variables are updated automatically every 50ms and can be used everywhere
 * in your program. It can also call an event handler routine, that you
 * need to register with BUMPERS_setStateChangedHandler before.
 */
void task_Bumpers(void)
{
	if(bumper_timer > 50) { // 50ms
		uint8_t left = getBumperLeft();
		uint8_t right = getBumperRight();
		if(bumper_left != left || bumper_right != right) {
			bumper_left = left;
			bumper_right = right;
			BUMPERS_stateChangedHandler();
		}
		bumper_timer = 0;
	}
}

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

/**
 * Read ADC channel (10 bit -> result is an integer from 0 to 1023).
 * The channels (ADC_BAT etc.) are defined in the RP6RobotBase.h file!
 *
 * This is a blocking function, which means it waits until the conversion
 * is complete. There is a more complicated alternative that frequently 
 * checks all channels (s. below).
 *
 * This function returns 0 if the ADC is buisy! This has been done to
 * prevents problems when the automatical function is used.
 * You should usually NOT use this function if you use the automatic one!
 *
 * Example:
 *
 *			uint16_t uBat = readADC(ADC_BAT);
 *			if(uBat < 600)
 *				writeString("WARNING: BAT IS LOW!\n");
 *
 */
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;
}

/**
 * This function starts an ADC conversion - it does not return the
 * read value! You need to poll if the conversion is complete somewhere
 * else and then read it from the ADC result register.
 * (s. task_ADC function below)
 */
void startADC(uint8_t channel)
{
	ADMUX = (1<<REFS0) | (0<<REFS1) | (channel<<MUX0);
	ADCSRA = (0<<ADIE) | (1<<ADSC) | (1<<ADEN) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADIF);
}

// -----------------------

uint16_t adcBat;
uint16_t adcMotorCurrentLeft;
uint16_t adcMotorCurrentRight;
uint16_t adcLSL;
uint16_t adcLSR;
uint16_t adc0;
uint16_t adc1;

/**
 * This functions checks all ADC channels sequentially in the Background!
 * It can save a lot of time, if the ADC channels are checked like this, because
 * each A/D conversion takes some time. With this function you don't need to
 * wait until the A/D conversion is finished and you can do other things in the 
 * meanwhile.
 * If you use this function (this is also the case if you use task_RP6System 
 * because it calls this function), you can NOT use readADC anymore!
 *
 * Instead you can use the seven global variables you see above to
 * get the ADC values!
 */
void task_ADC(void)
{
	static uint8_t current_adc_channel = 0;
	if(!(ADCSRA & (1<<ADSC))) {
	//	ADCSRA |= (1<<ADIF);
		switch(current_adc_channel) {
			case 0: adcBat = ADC; startADC(ADC_MCURRENT_L); break;
			case 1: adcMotorCurrentLeft = ADC; startADC(ADC_MCURRENT_R); break;
			case 2: adcMotorCurrentRight = ADC; startADC(ADC_LS_L); break;
			case 3: adcLSL = ADC; startADC(ADC_LS_R); break;
			case 4: adcLSR = ADC; startADC(ADC_ADC0); break;
			case 5: adc0 = ADC; startADC(ADC_ADC1); break;
			case 6: adc1 = ADC; startADC(ADC_BAT); break;
		}
		if(current_adc_channel == 6)
			current_adc_channel = 0;
		else
			current_adc_channel++;
	}
}

/*****************************************************************************/
// Encoders

// Timing variable used for speed calculation:
volatile uint8_t speed_timer;

// Speed measurement variables
volatile uint16_t mleft_counter;
volatile uint16_t mright_counter;
volatile uint16_t mleft_speed;
volatile uint16_t mright_speed;

// Distance
volatile uint16_t mleft_dist;
volatile uint16_t mright_dist;

// This is only used for the selftest program.
// You don't need this for your own programs!
#ifdef DEBUG_MEASURE_DUTY_CYCLE
	volatile uint16_t cycle_h_l;
	volatile uint16_t cycle_l_l;
	volatile uint16_t cycle_h_r;
	volatile uint16_t cycle_l_r;
	
	volatile uint8_t cycle_h_l_tmp;
	volatile uint8_t cycle_l_l_tmp;
	volatile uint8_t cycle_h_r_tmp;
	volatile uint8_t cycle_l_r_tmp;
#endif

/**
 * External Interrupt 0 ISR
 * (ENCL)
 *
 */
ISR (INT0_vect)
{
	mleft_dist++;
	mleft_counter++;	
	
	// Only used for selftest program:
	#ifdef DEBUG_MEASURE_DUTY_CYCLE
		if(isEncoderLeft()) {
			cycle_l_l = cycle_l_l_tmp;
			cycle_l_l_tmp = 0;
		}
		else {
			cycle_h_l = cycle_h_l_tmp;
			cycle_h_l_tmp = 0;
		}
	#endif
}

/**
 * External Interrupt 1 ISR
 * (ENCR)
 *
 */
ISR (INT1_vect)
{
	mright_dist++;
	mright_counter++;
	
	// Only used for selftest program:
	#ifdef DEBUG_MEASURE_DUTY_CYCLE
		if(isEncoderLeft()) {
			cycle_l_r += cycle_l_r_tmp;
			cycle_l_r >>=1;
			cycle_l_r_tmp = 0;
		}
		else {
			cycle_h_r += cycle_h_r_tmp;
			cycle_h_r >>=1;
			cycle_h_r_tmp = 0;
		}
	#endif
}