Hallo Leute,

nach meiner Servo-Library für die RP6Base hier:
https://www.roboternetz.de/phpBB2/viewtopic.php?t=45180
... jetzt auch eine einfache Version für max. 8 Servos für die RP6Control M32.

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

#include "RP6ControlServoLib.h"

uint16_t pos = 0;

int main(void)
{  
	initRP6Control();

	initLCD();

	showScreenLCD("################", "################");
	mSleep(1500);
	showScreenLCD("<<RP6  Control>>", "<<LC - DISPLAY>>");
	mSleep(2500); 
	showScreenLCD(" Servo - Test 1 ", "  Version 1.00  ");
	mSleep(2500);
	clearLCD();

	setLEDs(0b111111);
	mSleep(500);
	setLEDs(0b000000);
	
	initSERVO(SERVO1 | SERVO2);

	startStopwatch2();
	
	while(true)  
	{

		if (getStopwatch2() > 48) {
			servo1_position = pos;
			servo2_position = pos;
			setCursorPosLCD(0, 0);
			writeStringLCD_P("Servopos.: ");
			writeIntegerLCD(pos, DEC);
			writeStringLCD_P("   ");

			pos++;
			if (pos > RIGHT_TOUCH) {pos = 0;}
			setStopwatch2(0);
		}

		task_SERVO();

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

#include "RP6ControlLib.h" 		// The RP6 Control 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 ~70 .. 100 = ~0.7 ..
// 1ms at 100kHz) 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 Timer 1 frequency constant (F_TIMER1).
// A higher frequency leads 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. As a possible range of Timer 1 frequency values you
// may use 50kHz (20us) .. 105.263kHz (9.5us).
// HINT: If you alter F_TIMER1, you'll have to adapt LEFT_TOUCH and
//       RIGHT_TOUCH again as you can see in the following table!
//     Steps ->		9.5		10		12.5	15		17.5	20    [us]
//  ------------------------------------------------------------------
//  LEFT_TOUCH		74		71		57		47		41		35
//  RIGHT_TOUCH		169		162		129		107		92		80
//  F_TIMER1		105263	100000	80000	66667	57143	50000 [Hz]
#define LEFT_TOUCH			71			// Left servo touch      (~0.7ms)
#define RIGHT_TOUCH			162			// Right servo touch     (~2.3ms)
#define MIDDLE_POSITION		(RIGHT_TOUCH / 2) // Middle position (~1.5ms)
#define PULSE_REPETITION	17			// Pulse repetition freq. (~50Hz)
#define F_TIMER1			100000		// Timer 1 frequency     (100kHz)

// Servo ports:
#define SERVO1_PULSE_ON		(PORTC |= IO_PC2)	// PC2
#define SERVO1_PULSE_OFF	(PORTC &= ~IO_PC2)
#define SERVO2_PULSE_ON		(PORTC |= IO_PC3)	// PC3
#define SERVO2_PULSE_OFF	(PORTC &= ~IO_PC3)
#define SERVO3_PULSE_ON		(PORTC |= IO_PC4)	// PC4
#define SERVO3_PULSE_OFF	(PORTC &= ~IO_PC4)
#define SERVO4_PULSE_ON		(PORTC |= IO_PC5)	// PC5
#define SERVO4_PULSE_OFF	(PORTC &= ~IO_PC5)
#define SERVO5_PULSE_ON		(PORTC |= IO_PC6)	// PC6
#define SERVO5_PULSE_OFF	(PORTC &= ~IO_PC6)
#define SERVO6_PULSE_ON		(PORTC |= IO_PC7)	// PC7
#define SERVO6_PULSE_OFF	(PORTC &= ~IO_PC7)
#define SERVO7_PULSE_ON		(PORTD |= IO_PD5)	// PD5
#define SERVO7_PULSE_OFF	(PORTD &= ~IO_PD5)
#define SERVO8_PULSE_ON		(PORTD |= IO_PD6)	// PD6
#define SERVO8_PULSE_OFF	(PORTD &= ~IO_PD6)
// -----------------------------------------------------------
// Other possible ports for connecting Servos to RP6Control:
//#define SERVOx_PULSE_ON		(PORTA |= ADC6)		// PA6
//#define SERVOx_PULSE_OFF	(PORTA &= ~ADC6)
//#define SERVOx_PULSE_ON		(PORTA |= ADC7)		// PA7
//#define SERVOx_PULSE_OFF	(PORTA &= ~ADC7)
// -----------------------------------------------------------

/*****************************************************************************/
// 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) 31.12.2008 by Dirk
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF
Bibliothek (RP6ControlServoLib.c):
Code:
/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/             >>> RP6 CONTROL
 * ----------------------------------------------------------------------------
 * ------------------------ [c]2008 - Dirk ------------------------------------
 * ****************************************************************************
 * File: RP6ControlServoLib.c
 * Version: 1.0
 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * This is my simple RP6 Control 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 -> I/O Pin 7 (IO_PC2)		SERVO5 -> I/O Pin 4 (IO_PC6)
 *         SERVO2 -> I/O Pin 5 (IO_PC3)		SERVO6 -> I/O Pin 1 (IO_PC7)
 *         SERVO3 -> I/O Pin 6 (IO_PC4)		SERVO7 -> I/O Pin 9 (IO_PD5)
 *         SERVO4 -> I/O Pin 3 (IO_PC5)		SERVO8 -> I/O Pin 8 (IO_PD6)
 *
 * ****************************************************************************
 * 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 "RP6ControlServoLib.h"

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

uint8_t usedservos;
uint8_t servo_on = FALSE;

uint16_t impulselength1 = 0;
uint16_t impulselength2 = 0;
uint16_t impulselength3 = 0;
uint16_t impulselength4 = 0;
uint16_t impulselength5 = 0;
uint16_t impulselength6 = 0;
uint16_t impulselength7 = 0;
uint16_t impulselength8 = 0;

volatile uint16_t intcounter = 0;

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

/**
 * INIT SERVO
 *
 * Call this once before using the servo function.
 * Timer 1 is configured to work in "Clear Timer On
 * Compare Match Mode" (CTC). So no PWM is generated!
 * The timer runs on a fixed frequency (100kHz).
 *
 * 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)
{
	usedservos = servos;				// Save used Servos
	impulselength1 = 0;
	impulselength2 = 0;
	impulselength3 = 0;
	impulselength4 = 0;
	impulselength5 = 0;
	impulselength6 = 0;
	impulselength7 = 0;
	impulselength8 = 0;
	if (servos & SERVO1) {DDRC |= IO_PC2; PORTC &= ~IO_PC2;}
	if (servos & SERVO2) {DDRC |= IO_PC3; PORTC &= ~IO_PC3;}
	if (servos & SERVO3) {DDRC |= IO_PC4; PORTC &= ~IO_PC4;}
	if (servos & SERVO4) {DDRC |= IO_PC5; PORTC &= ~IO_PC5;}
	if (servos & SERVO5) {DDRC |= IO_PC6; PORTC &= ~IO_PC6;}
	if (servos & SERVO6) {DDRC |= IO_PC7; PORTC &= ~IO_PC7;}
	if (servos & SERVO7) {DDRD |= IO_PD5; PORTD &= ~IO_PD5;}
	if (servos & SERVO8) {DDRD |= IO_PD6; PORTD &= ~IO_PD6;}
// -----------------------------------------------------------
// Other possible ports for connecting Servos to RP6Control:
//	if (servos & SERVOx) {DDRA |= ADC6; PORTA &= ~ADC6;}
//	if (servos & SERVOx) {DDRA |= ADC7; PORTA &= ~ADC7;}
// -----------------------------------------------------------
	cli();
	// Timer 1: Normal port operation, mode 4 (CTC), clk/8
	TCCR1A =  (0 << COM1A1) 
			| (0 << COM1A0) 
			| (0 << COM1B1) 
			| (0 << COM1B0) 
			| (0 << FOC1A) 
			| (0 << FOC1B) 
			| (0 << WGM11) 
			| (0 << WGM10);
	TCCR1B =  (0 << ICNC1) 
			| (0 << ICES1) 
			| (0 << WGM13) 
			| (1 << WGM12) 
			| (0 << CS12) 
			| (1 << CS11) 
			| (0 << CS10);
	OCR1A = ((F_CPU/8/F_TIMER1)-1);	// 19 at 100kHz
// ------------------------------------------------------
// Possible OCR1A values (F_CPU = 16000000):
//	OCR1A = 2000000 / F_TIMER1 - 1	// F_TIMER1  (Steps)
//	OCR1A = 18;						// 105263Hz  (9.5us)
//	OCR1A = 19;						// 100000Hz   (10us)
//	OCR1A = 24;						//  80000Hz (12.5us)
//	OCR1A = 29;						//  66667Hz   (15us)
//	OCR1A = 34;						//  57143Hz (17.5us)
//	OCR1A = 39;						//  50000Hz   (20us)
// ------------------------------------------------------
	// Enable output compare A match interrupts:
	startSERVO();
	sei();
	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)
{
	TIMSK |= (1 << OCIE1A);
	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 Timer 1 interrupt
 * can be stopped with this function.
 *
 */
void stopSERVO(void)
{
	TIMSK &= ~(1 << OCIE1A);
	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
 *
 * ! Please make sure in your main program, that the !
 * ! servo position values (servoX_position) don't   !
 * ! exceed RIGHT_TOUCH!!!                           !
 *
 * COMMENT: The pulses are only started here!
 *          The pulses end in the Timer 1 ISR!
 *
 */
void pulseSERVO(void)
{
	if (servo_on) {
		intcounter = RIGHT_TOUCH;		// Avoid interference of Timer 1 ISR!
								// (Only necessary, if pulseSERVO() is called
								//  from outside of this library!)
		if (usedservos & SERVO1) {
			SERVO1_PULSE_ON; impulselength1 = LEFT_TOUCH + servo1_position;}
		if (usedservos & SERVO2) {
			SERVO2_PULSE_ON; impulselength2 = LEFT_TOUCH + servo2_position;}
		if (usedservos & SERVO3) {
			SERVO3_PULSE_ON; impulselength3 = LEFT_TOUCH + servo3_position;}
		if (usedservos & SERVO4) {
			SERVO4_PULSE_ON; impulselength4 = LEFT_TOUCH + servo4_position;}
		if (usedservos & SERVO5) {
			SERVO5_PULSE_ON; impulselength5 = LEFT_TOUCH + servo5_position;}
		if (usedservos & SERVO6) {
			SERVO6_PULSE_ON; impulselength6 = LEFT_TOUCH + servo6_position;}
		if (usedservos & SERVO7) {
			SERVO7_PULSE_ON; impulselength7 = LEFT_TOUCH + servo7_position;}
		if (usedservos & SERVO8) {
			SERVO8_PULSE_ON; impulselength8 = LEFT_TOUCH + servo8_position;}
		intcounter = 0;
	}
}

/**
 * TIMER1 ISR
 *
 * In this ISR the servo pulses are finished, if the
 * correct pulse length of each servo is reached.
 *
 */
ISR (TIMER1_COMPA_vect)
{
	intcounter++;
	if (intcounter == impulselength1) {SERVO1_PULSE_OFF;}
	if (intcounter == impulselength2) {SERVO2_PULSE_OFF;}
	if (intcounter == impulselength3) {SERVO3_PULSE_OFF;}
	if (intcounter == impulselength4) {SERVO4_PULSE_OFF;}
	if (intcounter == impulselength5) {SERVO5_PULSE_OFF;}
	if (intcounter == impulselength6) {SERVO6_PULSE_OFF;}
	if (intcounter == impulselength7) {SERVO7_PULSE_OFF;}
	if (intcounter == impulselength8) {SERVO8_PULSE_OFF;}
}

/**
 * SERVO TASK
 *
 * This is the servo task. The task performes the pulse repetition
 * with the help of a stopwatch.
 * At the next call of the servo task (earliest about 3ms after the
 * last servo pulse generation) the compare A match interrupt will
 * be disabled to reduce the interrupt load. It will be enabled
 * again after the next pulseSERVO() function call.
 *
 */
void task_SERVO(void)
{
	if (getStopwatch1() > 2) {TIMSK &= ~(1 << OCIE1A);}
	if (getStopwatch1() > PULSE_REPETITION) { // Pulse every ~20ms
		pulseSERVO();					// Servo pulse generation
		if (servo_on) {TIMSK |= (1 << OCIE1A);}
		setStopwatch1(0);
	}
}

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * - v. 1.0 (initial release) 31.12.2008 by Dirk
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF
Euch allen einen guten Rutsch! Und denkt dran: um 0.00 Uhr nicht zu früh die Korken knallen lassen! Da wird an 2008 noch eine Schaltsekunde angehängt! Das ist eine gute Chance, eigene DCF-Decoder wie z.B. diesen hier für den RP6:
https://www.roboternetz.de/phpBB2/viewtopic.php?t=34240
... 'mal auf ihre Schaltsekunden-Tauglichkeit zu überprüfen. Man kann ja danach auch noch böllern!

Dirk