Code:
// 18.facher Servotester 13.9.2011 mic
// 18 Servos ansteuern mit 16MHz-Mega16 (cat16) und 16-Bit Timer1
// https://www.roboternetz.de/community/threads/54583-Code-Optimierung-f%C3%BCr-Interrupt-m%C3%B6glich
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdlib.h> // für itoa() in writeInteger()
// Servoausgänge A 1-9
// S1 bis S8 und Speaker als Dummy (PB3)
#define servoainit {DDRB |= 0b1011; PORTB &= ~0b1011; DDRC |= 0b11111100; PORTC &= ~0b11111100; DDRD |= (1<<PD3); PORTD &= ~(1<<PD3);}
#define servoa1on PORTD |= (1<<PD3) // INT1
#define servoa1off PORTD &= ~(1<<PD3)
#define servoa2on PORTB |= (1<<PB0) // S1
#define servoa2off PORTB &= ~(1<<PB0)
#define servoa3on PORTB |= (1<<PB1) // S2
#define servoa3off PORTB &= ~(1<<PB1)
#define servoa4on PORTC |= (1<<PC2) // S3
#define servoa4off PORTC &= ~(1<<PC2)
#define servoa5on PORTC |= (1<<PC3) // S4
#define servoa5off PORTC &= ~(1<<PC3)
#define servoa6on PORTC |= (1<<PC4) // S5
#define servoa6off PORTC &= ~(1<<PC4)
#define servoa7on PORTC |= (1<<PC5) // S6
#define servoa7off PORTC &= ~(1<<PC5)
#define servoa8on PORTC |= (1<<PC6) // S7
#define servoa8off PORTC &= ~(1<<PC6)
#define servoa9on PORTC |= (1<<PC7) // S8
#define servoa9off PORTC &= ~(1<<PC7)
// Servoausgänge B 1-9
// J1 PD2/RC5, J2 PA0/Roll, J5 PA6, J6 PA7, J7 PA7-PD7/OCR2, J8 PA4/HeadL, J9 PA2/Tail, J10 PA1/Rang
#define servobinit {DDRA |= 0b11011111; PORTA &= ~0b11011111; DDRD |= 0b10000100; PORTD &= ~0b10000100;}
#define servob1on PORTD |= (1<<PD2) // J1 PD2/RC5
#define servob1off PORTD &= ~(1<<PD2)
#define servob2on PORTA |= (1<<PA0) // J2 PA0/Roll
#define servob2off PORTA &= ~(1<<PA0)
#define servob3on PORTA |= (1<<PA6) // J5 PA6/ADC6
#define servob3off PORTA &= ~(1<<PA6)
#define servob4on PORTA |= (1<<PA7) // J6 PA7/ADC7
#define servob4off PORTA &= ~(1<<PA7)
#define servob5on PORTD |= (1<<PD7) // J7 PD7/OCR2 (Pin4, PA7 ist auf Pin3!)
#define servob5off PORTD &= ~(1<<PD7)
#define servob6on PORTA |= (1<<PA4) // J8 PA4/HeadL
#define servob6off PORTA &= ~(1<<PA4)
#define servob7on PORTA |= (1<<PA3) // J8 PA3/HeadR auf Pin4
#define servob7off PORTA &= ~(1<<PA3)
#define servob8on PORTA |= (1<<PA2) // J9 PA2/Tail
#define servob8off PORTA &= ~(1<<PA2)
#define servob9on PORTA |= (1<<PA1) // J10 PA1/Rang
#define servob9off PORTA &= ~(1<<PA1)
#define off 0
#define green 0x10
#define red 0x20
#define yellow 0x30
volatile uint8_t p; // 20ms-Timer
volatile uint16_t servo_pos_a[10]={5000, 0,0,0, 0,0,0, 0,0,0}; // Pos[0] ist Impulspause
volatile uint16_t servo_pos_b[10]={0, 0,0,0, 0,0,0, 0,0,0}; // Position=0 bedeutet kein Impuls
ISR (TIMER1_COMPA_vect)
{
static uint8_t servo_nr=1;
uint16_t temp=0;
switch(servo_nr)
{
// Beim ersten Servo das Zählregister reseten und Interrupt für B-ISR initialisieren
// Wenn das Ergebniss der Zuweisung ungleich 0 dann Impuls starten und switch() verlassen
// sonst nächtes Servo, was bedeutet, das aktuelle Servo wird übersprungen (auch mehrfach!)
// (( temp=servo_pos_a[] )) doppelte Klammerung wegen Kompilerwarnung!
case 1: TCNT1=0; OCR1B=50; if((temp=servo_pos_a[1])) {servoa1on; break;} else servo_nr++; // ;)
case 2: servoa1off; if((temp=servo_pos_a[2])) {servoa2on; break;} else servo_nr++;
case 3: servoa2off; if((temp=servo_pos_a[3])) {servoa3on; break;} else servo_nr++;
case 4: servoa3off; if((temp=servo_pos_a[4])) {servoa4on; break;} else servo_nr++;
case 5: servoa4off; if((temp=servo_pos_a[5])) {servoa5on; break;} else servo_nr++;
case 6: servoa5off; if((temp=servo_pos_a[6])) {servoa6on; break;} else servo_nr++;
case 7: servoa6off; if((temp=servo_pos_a[7])) {servoa7on; break;} else servo_nr++;
case 8: servoa7off; if((temp=servo_pos_a[8])) {servoa8on; break;} else servo_nr++;
case 9: servoa8off; if((temp=servo_pos_a[9])) {servoa9on; break;} else servo_nr++;
case 10: servoa9off; servo_nr=0; OCR1A = servo_pos_a[0]; if(p) p--; break; // Impulspause
//case 10: servoa9off; servo_nr=0; OCR1A = TCNT1+500; if(p) p--; break; // Impulspause (Test)
}
if(servo_nr) OCR1A = temp + TCNT1; // nächsten Interruptzeitpunkt berechnen
/*
else
{
writeInteger(TCNT1, 10);
writeChar('A');
writeInteger(OCR1A, 10);
writeChar('\n');
}
*/
servo_nr++;
}
ISR (TIMER1_COMPB_vect)
{
static uint8_t servo_nr=1;
uint16_t temp=0;
switch(servo_nr)
{
case 1: if((temp=servo_pos_b[1])) {servob1on; break;} else servo_nr++;
case 2: servob1off; if((temp=servo_pos_b[2])) {servob2on; break;} else servo_nr++;
case 3: servob2off; if((temp=servo_pos_b[3])) {servob3on; break;} else servo_nr++;
case 4: servob3off; if((temp=servo_pos_b[4])) {servob4on; break;} else servo_nr++;
case 5: servob4off; if((temp=servo_pos_b[5])) {servob5on; break;} else servo_nr++;
case 6: servob5off; if((temp=servo_pos_b[6])) {servob6on; break;} else servo_nr++;
case 7: servob6off; if((temp=servo_pos_b[7])) {servob7on; break;} else servo_nr++;
case 8: servob7off; if((temp=servo_pos_b[8])) {servob8on; break;} else servo_nr++;
case 9: servob8off; if((temp=servo_pos_b[9])) {servob9on; break;} else servo_nr++;
case 10: servob9off; servo_nr=0; break; // nach dem letzten Impuls warten auf A-ISR!
}
if(servo_nr) OCR1B = temp + TCNT1; // nächsten Interruptzeitpunkt berechnen
/*
else
{
writeInteger(TCNT1, 10);
writeChar('B');
writeInteger(OCR1B, 10);
writeChar('\n');
}
*/
servo_nr++;
}
/************************* Ausgabe an Terminal ********************************/
void writeChar(char ch) {while (!(UCSRA & (1<<UDRE))); UDR = (uint8_t)ch;}
void writeString(char *string) {while(*string) writeChar(*string++);}
void writeInteger(int16_t number, uint8_t base)
{char buffer[17]; itoa(number, &buffer[0], base); writeString(&buffer[0]);}
/******************************************************************************/
volatile uint8_t usart_puffer[20], usart_write=0, usart_read=0, eingabe=0;
uint8_t beinnr, servonr, temp8;
uint16_t posa, posb, posc, temp16;
SIGNAL (SIG_UART_RECV)
{
usart_puffer[usart_write]=UDR;
if(usart_puffer[usart_write++] == 13) eingabe=1; // CR empfangen
if(usart_write > 19) {usart_write=19; eingabe=2;} // Pufferüberlauf!
}
void sleep(uint8_t pause) // 1/50 Sekunde blockierende Pause
{
p=pause;
while(p);
}
void leds(uint8_t mask)
{
SPCR = ( (1<<SPE)|(1<<MSTR) | (1<<SPR1) |(1<<SPR0));
SPDR = mask;
while(!(SPSR & (1<<SPIF))); // Wait for transmission complete!
SPCR = ( (0<<SPE)|(1<<MSTR) | (1<<SPR1) |(1<<SPR0));
PORTD |= (1<<6); // STRB high
PORTD &= ~(1<<6); // STRB low
}
void sleep(uint8_t pause); // 1/50 Sekunde blockierende Pause
void leds(uint8_t mask); // green, red, yellow or off
void c(uint8_t nr, uint16_t a, uint16_t b, uint16_t c, uint8_t d)
{
if(a>=200 && a<=500 && b>=200 && b<=500 && c>=200 && c<=500) // Bereichsprüfung
{
sleep(1); // Positionswerte in der Impulspause ändern, Synchronisationg nur mit A-ISR!
if(nr<2)
{
servo_pos_a[nr*3+1]=a;
sleep(d); // gemeinsamen Anlauf verhindern!
servo_pos_a[nr*3+2]=b;
sleep(d);
servo_pos_a[nr*3+3]=c;
}
else if(nr<3)
{
servo_pos_b[3]=a; // ??? Testbelegung!
sleep(d);
servo_pos_b[4]=b;
sleep(d);
servo_pos_b[5]=c;
}
else writeString("Fehler: Beinnummer!\n");
}
else writeString("Fehler: Drehbereich!\n");
}
int main(void)
{
cli();
//MCUCSR |= 128; // JTAG ausschalten
//MCUCSR |= 128;
/************************ UART-Setup für cat16 *******************************/
#define BAUD_LOW 38400 //Low speed - 38.4 kBaud
#define UBRR_BAUD_LOW ((F_CPU/(16*BAUD_LOW))-1)
UBRRH = UBRR_BAUD_LOW >> 8; // Baudrate is Low Speed
UBRRL = (uint8_t) UBRR_BAUD_LOW;
UCSRA = 0x00;
UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<<UCSZ0);
UCSRB = (1 << TXEN) | (1 << RXEN) | (1 << RXCIE); // Senden, Empfangen und Empfang-ISR einschalten
/***************************************************************************/
//Init SPI für leds()
SPCR = ( (0<<SPE)|(1<<MSTR) | (1<<SPR1) |(1<<SPR0)); // Disable SPI, Master, set clock rate fck/128
DDRB = 0b10110000; // SCK: PB7, MoSi: PB5, SS: PB4 (!)?
DDRD = 0b01000000; // PD6 ist STRB fürs 4094
//Timer1 Initialisierung
TCCR1A = 0;
TCCR1B = (0<<CS12) | (1<<CS11) | (1<<CS10); // Prescaler /64
//TCCR1B|= (1<<WGM12); // CTC-Mode 23.8.11 NormalMode!
TCNT1=0; // Zählregister initialisieren (vorsichtshalber)
OCR1A=5000; // 20ms bis zum ersten Interrupt
OCR1B=5000;
TIMSK |= (1 << OCIE1A);
TIMSK |= (1 << OCIE1B);
servoainit; // Datenrichtung der Servopins A einstellen
servobinit; // Datenrichtung der Servopins B einstellen
sei(); // ... und los!
leds(red);
writeString("\nHallo\n");
sleep(50);
while(1)
{
for(temp8=1; temp8<10;temp8++) servo_pos_a[temp8]=servo_pos_b[temp8]=250;
sleep(50);
for(temp8=1; temp8<10;temp8++) servo_pos_a[temp8]=servo_pos_b[temp8]=450;
sleep(50);
}
while(1)
{
if(eingabe)
{
leds(green);
/*
for(temp8=usart_read; temp8<usart_write; temp8++)
{
writeChar(usart_puffer[temp8]);
}
writeChar('\n');
*/
if(usart_puffer[0] == 'a')
{
servonr=(usart_puffer[1]-'0')*10+usart_puffer[2]-'0';
servo_pos_a[servonr]=(usart_puffer[3]-'0')*100+(usart_puffer[4]-'0')*10+usart_puffer[5]-'0';
writeString("Servo: A");
writeInteger(servonr, 10);
writeString(" = ");
writeInteger(servo_pos_a[servonr],10);
writeChar('\n');
}
else if(usart_puffer[0] == 'b')
{
servonr=(usart_puffer[1]-'0')*10+usart_puffer[2]-'0';
servo_pos_b[servonr]=(usart_puffer[3]-'0')*100+(usart_puffer[4]-'0')*10+usart_puffer[5]-'0';
writeString("Servo: B");
writeInteger(servonr, 10);
writeString(" = ");
writeInteger(servo_pos_b[servonr],10);
writeChar('\n');
}
else if(usart_puffer[0] == 'c')
{
// CB,aaa,bbb,ccc
// 01234567890123
if((usart_puffer[2]+usart_puffer[6]+usart_puffer[10]) == (3*',')) // alle Kommas richtig gesetzt?
{
beinnr=usart_puffer[1]-'0';
posa=(usart_puffer[3]-'0')*100+(usart_puffer[4]-'0')*10+usart_puffer[5]-'0';
posb=(usart_puffer[7]-'0')*100+(usart_puffer[8]-'0')*10+usart_puffer[9]-'0';
posc=(usart_puffer[11]-'0')*100+(usart_puffer[12]-'0')*10+usart_puffer[13]-'0';
c(beinnr, posa, posb, posc, 10);
writeString("Bein: ");
writeInteger(beinnr, 10);
writeString(" = ");
writeInteger(posa,10);
writeChar(' ');
writeInteger(posb,10);
writeChar(' ');
writeInteger(posc,10);
writeChar('\n');
}
else writeString("Fehler: Kommas im Datensatz!\n");
}
else writeString("ERROR!\n");
usart_read=0;
usart_write=0;
eingabe=0;
leds(red);
}
}
return(0);
}
Zitieren
Lesezeichen