Irgendwie sind alle Servocontrollerprogramme, die ich bis jetzt gerfunden habe, einigermassen umständlich programmiert. Deshalb habe ich beschlossen, das ganze selber zu machen. Hier das Ergebnis:
Code:
/************************************************************************
SERVO Controller for up to 10 Servos
controlled by serial line
default: 2400 Baud 8N1
Processor: ATMEGA 8
CLOCK: 8MHZ, no prescaler set config bits of Atmega 8 correctly !
Compiler: AVR-GCC
This code is licensed under the GPL.
You may modify, redistribute the code .. blabla, you know what I mean ...
Copyright stochri (c.hab@gmx.net) Nov.2005
***************************************************************************/
#include <stdlib.h>
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
typedef unsigned char byte;
#define SYSCLK 8000000 // timer clock 8Mhz
#define MAXPULSFREQ 500 // 2ms => 500HZ
#define TIMER_MAXPULS SYSCLK/MAXPULSFREQ // Timer1 value for a 2ms Puls
#define MINPULS TIMER_MAXPULS/4 // min pulslength = 0.5ms
#define MAXPULS TIMER_MAXPULS // max pulslength=2ms
// port pin definitions
// you may redefine the pins to suit your application
// tale a look at the interrupt routine and enable the cases for your servo
#define LOW_SERVO0 PORTD&=~(1<<6)
#define HIGH_SERVO0 PORTD|=(1<<6)
#define LOW_SERVO1 PORTB&=~(1<<0)
#define HIGH_SERVO1 PORTB|=(1<<0)
#define LOW_SERVO2 PORTB&=~(1<<1)
#define HIGH_SERVO2 PORTB|=(1<<1)
#define LOW_SERVO3
#define HIGH_SERVO3
#define LOW_SERVO4
#define HIGH_SERVO4
#define LOW_SERVO5
#define HIGH_SERVO5
#define LOW_SERVO6
#define HIGH_SERVO6
#define LOW_SERVO7
#define HIGH_SERVO7
#define LOW_SERVO8
#define HIGH_SERVO8
#define LOW_SERVO9
#define HIGH_SERVO9
uint16_t Pulslength[20]; // array for all delays
/************************************************************************
SIGNAL(SIG_OVERFLOW1)
timer1 interrupt, generates the high and low pulses for each servo
***************************************************************************/
SIGNAL(SIG_OVERFLOW1)
{
static byte servoindex_half=0;
switch (servoindex_half)
{
case 0: HIGH_SERVO0; break;
case 1: LOW_SERVO0; break;
case 2: HIGH_SERVO1; break;
case 3: LOW_SERVO1; break;
case 4: HIGH_SERVO2; break;
case 5: LOW_SERVO2; break;
// case 6: HIGH_SERVO3; break;
// case 7: LOW_SERVO3; break;
// case 8: HIGH_SERVO4; break;
// case 9: LOW_SERVO4; break;
// case 10: HIGH_SERVO5; break;
// case 11: LOW_SERVO5; break;
// case 12: HIGH_SERVO6; break;
// case 13: LOW_SERVO6; break;
// case 14: HIGH_SERVO7; break;
// case 15: LOW_SERVO7; break;
// case 16: HIGH_SERVO8; break;
// case 17: LOW_SERVO8; break;
// case 18: HIGH_SERVO9; break;
// case 19: LOW_SERVO9; break;
}
TCNT1 =Pulslength[servoindex_half]; // set time for next interrupt
servoindex_half++; // increment timervalue index
if(servoindex_half==20)servoindex_half=0; // reset index
}
/************************************************************************
void setservo(byte index, byte value)
Set servo position
value: 0..255
***************************************************************************/
void setservo(byte index, byte value)
{
uint16_t wert;
wert=MINPULS+(MAXPULS-MINPULS)/256*value;
// callculate hightime
Pulslength[index<<1]=0-wert;
// sume of low and hightime for one servo is 2ms
Pulslength[(index<<1)+1]=0-(TIMER_MAXPULS-wert);
// 10 Servos give you 10*2ms=20ms total cycle time
}
/************************************************************************
void init_servos()
initialize all Servos to the start position
***************************************************************************/
void init_servos()
{
byte n;
for(n=0;n<10;n++) setservo(n,128);
}
/************************************************************************
void init(void)
initialize the prozessor registers
***************************************************************************/
void init(void)
{
// prepare RS232
UCSRA = 0x00;
UCSRB = 0x00;
UCSRC = 0x86; // No Parity | 1 Stop Bit | 8 Data Bit
UBRRL = 0xCF; // 2400bps @ 8.00MHz
// UBRRL = 51; // 9600bps @ 8.00MHz
// UBRRL = 25; // 19200bps @ 8.00MHz
/* initialize ports */
DDRB = 0xFF;
DDRC = 0xFF;
DDRD = 0xFF;
PORTB = 0x00;
PORTC = 0x00;
PORTD = 0x00;
// init timer1
TCNT1 = 0-16000;
TCCR1A=0;
TCCR1B=0x01;
TIMSK |= _BV(TOIE2) | _BV(TOIE1);
/* allow interrupts */
sei();
}
/************************************************************************
serial communication
***************************************************************************/
byte chgetchar()
{
UCSRB = 0x10; // enable receiver
while(!(UCSRA & 0x80)); // wait for received byte
return UDR;
}
void chputchar(byte zeichen)
{
UCSRB = 0x08; // enable transmitter
UCSRA|=0x40; // clear transmitter flag
while (!(UCSRA & 0x20)); // wait for empty transmit buffer
UDR = zeichen;
while (!(UCSRA & 0x40)); // Wait for transmit complete flac (TXC)
}
void chSerPrint(unsigned char *data)
{
unsigned char i = 0;
while(data[i]!=0x00) chputchar(data[i++]);
}
/************************************************************************
main programm
servo controll by RS232 interface
***************************************************************************/
int main(void)
{
char c;
byte n;
byte servos[10];
init();
init_servos();
chSerPrint("\n\r-- Atmega8 servo controller V1.0 --\n\r");
chSerPrint("Connection OK\n\r");
while(1)
{
// get key from terminal
c=chgetchar();
if(c=='q') servos[0] += 10;
if(c=='w') servos[0] -= 10;
if(c=='a') servos[1] += 10;
if(c=='s') servos[1] -= 10;
if(c=='y') servos[2] += 10;
if(c=='x') servos[2] -= 10;
// set to default position, if space is pressed
if(c==' ')
{
servos[0] = 128;
servos[1] = 128;
servos[2] = 128;
}
for(n=0;n<10;n++) setservo(n,servos[n]);
}
}
Maximale Zahl Servos: 10
Steuerung: serielle Schnittstelle 2400Baud
Prozessor: Atmega8 8Mhz
Es wird nur Timer 1 verwendet.
Zitieren
Berechtigungen
Lesezeichen