Hallo Leute,

hier meine einfache Lib für den RP6, um max. 8 Servos betreiben zu können.

Have fun!

Demoprogramm:
Code:
// Uncommented Version of RP6BaseServo.c
// written by Dirk
// ------------------------------------------------------------------------------------------

#include "RP6BaseServoLib.h"

uint16_t pos = 0;

int main(void)
{
	initRobotBase();
	
	writeString_P("\n\n   _______________________\n");
	writeString_P("   \\| RP6  ROBOT SYSTEM |/\n");
	writeString_P("    \\_-_-_-_-_-_-_-_-_-_/\n\n");

	writeString_P("################\n");
	writeString_P("<<RP6     Base>>\n");
	writeString_P(" Servo - Test 1 \n");
	writeString_P("  Version 1.20  \n");
	writeString_P("################\n\n");
	mSleep(2500);

	setLEDs(0b111111);
	mSleep(500);

	initSERVO(SERVO1 | SERVO2);

	startStopwatch2();
	
	while(true)  
	{
		if (getStopwatch2() > 48) {
			servo1_position = pos;
			servo2_position = pos;
			pos++;
			if (pos > RIGHT_TOUCH) {pos = 0;}
			setStopwatch2(0);
		}

		task_SERVO();
		task_RP6System();
	}
	return 0;
}
Header-Datei (RP6BaseServoLib.h):
Code:
/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/         >>> BASE CONTROLLER
 * ----------------------------------------------------------------------------
 * ------------------------ [c]2008 - Dirk ------------------------------------
 * ****************************************************************************
 * File: RP6BaseServoLib.h
 * Version: 1.2
 * Target: RP6 Base - ATMEGA32 @8.00MHz
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * This is the RP6BaseServoLib header file.
 * You have to include this file, if you want to use the library
 * RP6BaseServoLib.c in your own projects.
 *
 * ****************************************************************************
 * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE!
 * ****************************************************************************
 */
 
/*****************************************************************************/
// Includes:

#include "RP6RobotBaseLib.h" 	// The RP6 Robot Base Library.
								// Always needs to be included!

/*****************************************************************************/
// Defines:

// Servo constants:
#define SERVO1				0b00000001
#define SERVO2				0b00000010
#define SERVO3				0b00000100
#define SERVO4				0b00001000
#define SERVO5				0b00010000
#define SERVO6				0b00100000
#define SERVO7				0b01000000
#define SERVO8				0b10000000

// Servo movement limits (depending on servo type):
// Standard Servos need an impulse every 20ms (50Hz). This impulse must have
// a length of 1ms (0.7 .. 1ms) to move the servo lever to the left touch
// and a length of 2ms (2 .. 2.3ms) for moving it to the right touch. In the
// middle position the servo needs an impulse length of 1.5ms (1.3 .. 1.6ms).
// If you want to modify the following constants for a certain servo type,
// you must adapt the LEFT_TOUCH constant first (values ~690 .. 990 = ~0.7 ..
// 1ms) by using a servo position value (servoX_position) of zero.
// After that you have two "screws" to adjust the servo movement limits:
// First you may change the RIGHT_TOUCH constant. If you choose a higher
// value than 255, you will use 16-bit values. Higher values mean a longer
// impulse length, but longer impulses than 2.3ms do not make sense.
// Second you may alter the number of nop() instructions in the pulseSERVO()
// function. Less nop()s lead to smaller steps of the servo movement. This of
// course reduces the impulse length and may be compensated again by a higher
// RIGHT_TOUCH constant.
#define LEFT_TOUCH			692			// Left servo touch      (~0.7ms)
#define RIGHT_TOUCH			254			// Right servo touch     (~2.3ms)
#define MIDDLE_POSITION		(RIGHT_TOUCH / 2) // Middle position (~1.5ms)
#define PULSE_REPETITION	17			// Pulse repetition freq. (~50Hz)

// Servo ports:
#define SERVO1_PULSE_ON		(PORTC |= SL1)		// PC4
#define SERVO1_PULSE_OFF	(PORTC &= ~SL1)
#define SERVO2_PULSE_ON		(PORTC |= SL2)		// PC5
#define SERVO2_PULSE_OFF	(PORTC &= ~SL2)
#define SERVO3_PULSE_ON		(PORTC |= SL3)		// PC6
#define SERVO3_PULSE_OFF	(PORTC &= ~SL3)
#define SERVO4_PULSE_ON		(PORTB |= SL4)		// PB7
#define SERVO4_PULSE_OFF	(PORTB &= ~SL4)
#define SERVO5_PULSE_ON		(PORTB |= SL5)		// PB1
#define SERVO5_PULSE_OFF	(PORTB &= ~SL5)
#define SERVO6_PULSE_ON		(PORTB |= SL6)		// PB0
#define SERVO6_PULSE_OFF	(PORTB &= ~SL6)
#define SERVO7_PULSE_ON		(PORTC |= SCL)		// PC0
#define SERVO7_PULSE_OFF	(PORTC &= ~SCL)
#define SERVO8_PULSE_ON		(PORTC |= SDA)		// PC1
#define SERVO8_PULSE_OFF	(PORTC &= ~SDA)
// -----------------------------------------------------------
// Other possible ports for connecting Servos to RP6Base:
//#define SERVOx_PULSE_ON		(PORTA |= ADC0)		// PA0
//#define SERVOx_PULSE_OFF	(PORTA &= ~ADC0)
//#define SERVOx_PULSE_ON		(PORTA |= ADC1)		// PA1
//#define SERVOx_PULSE_OFF	(PORTA &= ~ADC1)
//#define SERVOx_PULSE_ON		(PORTA |= E_INT1)	// PA4
//#define SERVOx_PULSE_OFF	(PORTA &= ~E_INT1)
// -----------------------------------------------------------

/*****************************************************************************/
// Variables:

uint16_t servo1_position;		// Servo 1 position [0..RIGHT_TOUCH]
uint16_t servo2_position;		// Servo 2 position [0..RIGHT_TOUCH]
uint16_t servo3_position;		// Servo 3 position [0..RIGHT_TOUCH]
uint16_t servo4_position;		// Servo 4 position [0..RIGHT_TOUCH]
uint16_t servo5_position;		// Servo 5 position [0..RIGHT_TOUCH]
uint16_t servo6_position;		// Servo 6 position [0..RIGHT_TOUCH]
uint16_t servo7_position;		// Servo 7 position [0..RIGHT_TOUCH]
uint16_t servo8_position;		// Servo 8 position [0..RIGHT_TOUCH]

/*****************************************************************************/
// Functions:

void initSERVO(uint8_t servos);
void startSERVO(void);
void stopSERVO(void);
void pulseSERVO(void);
void task_SERVO(void);

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * - v. 1.0 (initial release) 03.05.2008 by Dirk
 * - v. 1.1 (bugs fixed) 21.12.2008 by Dirk
 * - v. 1.2 (pulse generation improved) 26.12.2008 by Dirk
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF
Bibliothek (RP6BaseServoLib.c):
Code:
/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/         >>> BASE CONTROLLER
 * ----------------------------------------------------------------------------
 * ------------------------ [c]2008 - Dirk ------------------------------------
 * ****************************************************************************
 * File: RP6BaseServoLib.c
 * Version: 1.2
 * Target: RP6 Base - ATMEGA32 @8.00MHz
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * This is my simple RP6 Base Servo Library for up to 8 Servos.
 *
 * COMMENT: It is a good idea to use a separate power supply for the servos!
 *
 * Servo connections:
 *         SERVO1 -> Status LED 1 (SL1)		SERVO5 -> Status LED 5 (SL5)
 *         SERVO2 -> Status LED 2 (SL2)		SERVO6 -> Status LED 6 (SL6)
 *         SERVO3 -> Status LED 3 (SL3)		SERVO7 -> PC0 (I2C bus: SCL)
 *         SERVO4 -> Status LED 4 (SL4)		SERVO8 -> PC1 (I2C bus: SDA)
 *   If you want to use the RP6 bumpers in your program, you should not
 *   connect Servos to SL3 and SL6.
 *   It is not possible to use the I2C bus, if Servos are connected to SCL
 *   and/or SDA. Instead of SCL/SDA you could use ADC0/ADC1 for servo 7/8,
 *   if you want to use the I2C bus.
 *
 * ****************************************************************************
 * ATTENTION: Stopwatch 1 is used for the servo task! Please do
 *            not use this stopwatch elsewhere in your program!
 *
 * ****************************************************************************
 * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE!
 * ****************************************************************************
 */
 
/*****************************************************************************/
// Includes:

#include "RP6BaseServoLib.h"

/*****************************************************************************/
// Variables:

uint8_t usedservos;
uint8_t servo_on = FALSE;

/*****************************************************************************/
// Functions:

/**
 * INIT SERVO
 *
 * Call this once before using the servo library.
 *
 * Input:  Servos -> Used Servos
 *         Examples:
 *         - initSERVO(SERVO1 | SERVO2) -> Use only Servos 1 and 2
 *         - initSERVO(SERVO1 | SERVO6) -> Use only Servos 1 and 6
 *         - initSERVO(SERVO1 | SERVO2 | SERVO8) -> Use Servos 1, 2 and 8
 *
 */
void initSERVO(uint8_t servos)
{
	setLEDs(0b000000);					// All LEDs off!
	usedservos = servos;				// Save used Servos
	if (servos & SERVO1) {DDRC |= SL1; PORTC &= ~SL1;}
	if (servos & SERVO2) {DDRC |= SL2; PORTC &= ~SL2;}
	if (servos & SERVO3) {DDRC |= SL3; PORTC &= ~SL3;}
	if (servos & SERVO4) {DDRB |= SL4; PORTB &= ~SL4;}
	if (servos & SERVO5) {DDRB |= SL5; PORTB &= ~SL5;}
	if (servos & SERVO6) {DDRB |= SL6; PORTB &= ~SL6;}
	if (servos & SERVO7) {DDRC |= SCL; PORTC &= ~SCL;}
	if (servos & SERVO8) {DDRC |= SDA; PORTC &= ~SDA;}
// -----------------------------------------------------------
// Other possible ports for connecting Servos to RP6Base:
//	if (servos & SERVOx) {DDRA |= ADC0; PORTA &= ~ADC0;}
//	if (servos & SERVOx) {DDRA |= ADC1; PORTA &= ~ADC1;}
//	if (servos & SERVOx) {DDRA |= E_INT1; PORTA &= ~E_INT1;}
// -----------------------------------------------------------
	startSERVO();
	startStopwatch1();					// Needed for 20ms pulse repetition
}

/**
 * START SERVO
 *
 * If the servo function was stopped with the
 * function stopSERVO() before, it can be
 * started again with this function.
 *
 */
void startSERVO(void)
{
	servo_on = TRUE;
}

/**
 * STOP SERVO
 *
 * The servo function uses a certain amount of the
 * processor's calculating time. If the Servos are
 * not moving for a while, the servo pulse
 * generation can be stopped with this function.
 *
 */
void stopSERVO(void)
{
	servo_on = FALSE;
}

/**
 * PULSE SERVO
 *
 * This is the servo pulse generation. This function
 * must be called every 20ms (pulse repetition).
 *
 * position = 0               : Left touch
 * position = RIGHT_TOUCH     : Right touch
 * position = MIDDLE_POSITION : Middle position
 *
 * ATTENTION: ! This function is BLOCKING all other activities for up to 2   !
 *            ! ms!!!                                                        !
 *            ! If you generate a pulse every 20ms <= 10% of the processor's !
 *            ! calculating time is wasted by this kind of pulse generation. !
 *            ! If this is a problem for the rest of your program, you       !
 *            ! cannot use this method.                                      !
 *            ! You will need an interrupt- or PWM-based solution instead.   !
 *            
 */
void pulseSERVO(void)
{uint16_t position = 0; uint8_t pulse_active = usedservos;
	if (servo_on) {
		cli();
		if (usedservos & SERVO1) {SERVO1_PULSE_ON;}
		if (usedservos & SERVO2) {SERVO2_PULSE_ON;}
		if (usedservos & SERVO3) {SERVO3_PULSE_ON;}
		if (usedservos & SERVO4) {SERVO4_PULSE_ON;}
		if (usedservos & SERVO5) {SERVO5_PULSE_ON;}
		if (usedservos & SERVO6) {SERVO6_PULSE_ON;}
		if (usedservos & SERVO7) {SERVO7_PULSE_ON;}
		if (usedservos & SERVO8) {SERVO8_PULSE_ON;}
		delayCycles(LEFT_TOUCH);			// Delay ~0.7ms (left touch)
		while (position <= RIGHT_TOUCH) {
			if ((usedservos & SERVO1) && (position == servo1_position)) {
				SERVO1_PULSE_OFF; pulse_active &= ~SERVO1;}
			if ((usedservos & SERVO2) && (position == servo2_position)) {
				SERVO2_PULSE_OFF; pulse_active &= ~SERVO2;}
			if ((usedservos & SERVO3) && (position == servo3_position)) {
				SERVO3_PULSE_OFF; pulse_active &= ~SERVO3;}
			if ((usedservos & SERVO4) && (position == servo4_position)) {
				SERVO4_PULSE_OFF; pulse_active &= ~SERVO4;}
			if ((usedservos & SERVO5) && (position == servo5_position)) {
				SERVO5_PULSE_OFF; pulse_active &= ~SERVO5;}
			if ((usedservos & SERVO6) && (position == servo6_position)) {
				SERVO6_PULSE_OFF; pulse_active &= ~SERVO6;}
			if ((usedservos & SERVO7) && (position == servo7_position)) {
				SERVO7_PULSE_OFF; pulse_active &= ~SERVO7;}
			if ((usedservos & SERVO8) && (position == servo8_position)) {
				SERVO8_PULSE_OFF; pulse_active &= ~SERVO8;}
			if (!pulse_active) break;		// Longest pulse done: Break!
			// Delay 9 cycles for an 8-bit servo movement range [0..255]:
			nop(); nop(); nop(); nop(); nop(); nop(); nop(); nop(); nop();
			position++;
		}
		sei();
	}
}

/**
 * SERVO TASK
 *
 * This is the servo task. The task performes the pulse repetition
 * with the help of a stopwatch.
 *
 */
void task_SERVO(void)
{
	if (getStopwatch1() > PULSE_REPETITION) { // Pulse every ~20ms
		pulseSERVO();					// Servo pulse generation
		setStopwatch1(0);
	}
}

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * - v. 1.0 (initial release) 03.05.2008 by Dirk
 * - v. 1.1 (bugs fixed) 21.12.2008 by Dirk
 * - v. 1.2 (pulse generation improved) 26.12.2008 by Dirk
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF
Dirk