RP6Base: Library für 8 Servos
Hallo Leute,
hier meine einfache Lib für den RP6, um max. 8 Servos betreiben zu können.
Have fun! O:)
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
Servos und IRCOMM - geht das zusammen?
Hallo,
wenn ich den Quelltext richtig verstehe, dann beschäftigt sich der Mikrocontroller über 10 Prozent seiner Zeit mit nichts anderem als abzuwarten bis er mal wieder einen Servoport umschalten muss. Interrupts sind in dieser Zeit gesperrt. Gibt es da nicht Probleme z.B. mit der IRCOMM ?
