Code:
/* Code fuer ATMega8, CPU-Frequenz 8MHz: Ansteuerung von mehreren Servos nacheinander innerhalb der 20ms-Periode */
/* Bei drei Servos wird zum Beispiel alle 7ms eines bearbeitet, so dass jedes eine Periode von ~21ms hat */
#include <avr\io.h>
#include <avr\signal.h>
#include <avr\interrupt.h>
//#include <math.h>
#include <avr\eeprom.h>
#define PORT PORTD
#define n_servos 3
#define S1L 7500 // Linker Anschlag (-90deg) fuer Servo 1
#define S1R 20000 // Rechter Anschlag (90deg) fuer Servo 1
#define S2L 6700 // Linker Anschlag (-90deg) fuer Servo 2
#define S2R 19000 // Rechter Anschlag (90deg) fuer Servo 2
#define S3L 6700 // Linker Anschlag (-90deg) fuer Servo 3
#define S3R 18200 // Rechter Anschlag (90deg) fuer Servo 3
typedef struct {
int sw; // Stellungssollwert in Grad (-90..90)
int puls; // Stellungssollwert umgerechnet in Timer-Takte (Rechnung in main())
unsigned char bit; // Auf welchem Bit des verwendeten Ports liegt dieses Servo...
unsigned int ll; // Linker Anschlag (wird vom #define bzw aus dem EEPROM uebernommen)
unsigned int lr; // Rechter Anschlag (wird vom #define bzw aus dem EEPROM uebernommen)
} servo_struct;
volatile servo_struct servo[n_servos]; // Volatile, weil Verwendung in main() und in den Interrupt-Routinen!
volatile unsigned char i=0, k=0; // Zaehler
SIGNAL(SIG_OVERFLOW0) { // 8bit-Timer: alle 7ms, jeweils eins der drei Servos (Periode 21ms)
sbi(PORT,servo[i].bit); // Pin fuer dieses Servo auf High setzen
TCNT1 = 0xFFFF-servo[i].puls; // Bei 0xFFFF laeuft der Timer ueber (laeuft immer aufwaerts)
TCCR1B = 0x01; // Prescaler des 16bit-Timers gleich CPU-Takt, Timer starten
if(i++>n_servos-1) i=0; // Bei jedem Erreichen dieser Routine das naechste Servo vorwaehlen (i++)
TCNT0 = 0xFF-(156.0/n_servos); // 156 Takte waeren 20ms, werden aufgeteilt auf die Anzahl der Servos
}
SIGNAL(SIG_OVERFLOW1) { // 16bit-Timer
PORT = 0x00; // Alle Pins auf Low
TCCR1B = 0x00; // 16bit-Timer stoppen
}
void init(void) {
servo[0].bit=1; // Erstes Servo auf Bit 1 von Port D
servo[1].bit=3; // Zweites Servo auf Bit 3 von Port D
servo[2].bit=5; // Drittes Servo auf Bit 5 von Port D
servo[0].sw=0; // Sollwert fuer Servo 1 (Test)
servo[1].sw=0; // Sollwert fuer Servo 2 (Test)
servo[2].sw=0; // Sollwert fuer Servo 3 (Test)
/* Dieser Block wird durch das Einlesen aus dem EEPROM ersetzt...*/
servo[0].ll=S1L; // Linker Anschlag Servo 1
servo[0].lr=S1R; // Rechter Anschlag Servo 1
servo[1].ll=S2L; // Linker Anschlag Servo 2
servo[1].lr=S2R; // Rechter Anschlag Servo 2
servo[2].ll=S3L; // Linker Anschlag Servo 3
servo[2].lr=S3R; // Rechter Anschlag Servo 3
/* Wenn die Werte aus dem EEPROM gelesen werden, kommt dieser Block natuerlich wieder raus...*/
eeprom_write_word(0x00,servo[0].ll);
eeprom_write_word(0x02,servo[0].lr);
eeprom_write_word(0x04,servo[1].ll);
eeprom_write_word(0x06,servo[1].lr);
eeprom_write_word(0x08,servo[2].ll);
eeprom_write_word(0x0A,servo[2].lr);
DDRD = 0xFF; // Alle 8 bits von Port D als Ausgaenge
TCCR0 = 0x05; // Prescaler des 8bit-Timers auf CPU-Takt/1024
TIMSK = 0x05; // Overflow-Interrupt beider Timer aktivieren (Bit 0 = Timer0, Bit 2 = Timer1)
}
int main(void) {
init(); // Servostructs initialisieren, Timer und Ports einstellen
sei(); // Interrupts global freigeben
while(1) {
for(k=0;k<n_servos;k++) // Berechnung der benoetigten Pulse aus dem Stellungssollwert fuer jedes Servo
servo[k].puls = servo[k].ll + ( (servo[k].sw+90.0)*(servo[k].lr-servo[k].ll)/180.0 );
}
}
Die normale Bildung der Sollwerte fehlt natuerlich noch, die sollen bei mir ueber USART von einem anderem AVR empfangen werden.
Die Pins 1,3 und 5 verwende ich, weil sie alle auf der gleichen Seite des zehnpoligen Steckers auf dem STK500 liegen, dadurch war die Testschaltung einfacher zu loeten. Hab naemlich aus 2,54mm-Steckerleisten drei dreipolige Anschluesse gemacht, sodass ich die normalen Servostecker benutzen kann. Allerdings faellt auf, dass die Stromversorgung des STK500 recht schwach ist. Laesst man eines der Servos gegen eine Kraft arbeiten, steigt der gemessene Strom auf 0,5A und die Spannung bricht deutlich ein (von 5V auf 3V), was dann auch bei den anderen Servos die Elektronik irritiert und ihre Stellung veraendert. Also kriegen die Dinger in der geplanten Anwendung einen eigenen Spannungsregler, der 1A abkann...
Nils
Zitieren
Lesezeichen