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
