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
Zitieren
Berechtigungen
Lesezeichen