"Vereinfachung" von radbruchs Servoansteuerung mög

[radbruch]

Hallo

Um nun endlich mal zu einem Erfolg zu kommen:

#include <avr/io.h>			// I/O Port definitions
//#include <avr/interrupt.h>	// Interrupt macros (für cli)

#define servo1_port PORTC
#define servo1_ddr  DDRC
#define servo1_pin  PC0
#define servo2_port PORTC
#define servo2_ddr  DDRC
#define servo2_pin  PC1

#define servo1_on servo1_port |= 1<<servo1_pin
#define servo2_on servo2_port |= 1<<servo2_pin
#define servo1_off servo1_port &= ~(1<<servo1_pin)
#define servo2_off servo2_port &= ~(1<<servo2_pin)

uint16_t i, stellzeit, dummy;

int main(void)
{
	servo1_ddr |= 1<<servo1_pin; // Pins als Ausgang definieren
	servo2_ddr |= 1<<servo2_pin;
	servo1_off;                  // und auf low schalten
	servo2_off;
	//cli();                       // keine Störungen erlauben
	while(1)                     // solange Saft im Akku...
	{
	   stellzeit=100;            // 100 mal den Impuls senden
	   while(stellzeit--)
	   {
	      servo1_on;             // Impuls für erstes Servo erzeugen
	      for(i=0; i<500; i++) dummy^=i;
	      servo1_off;

	      servo2_on;             // Impuls für zweites Servo erzeugen
	      for(i=0; i<500; i++) dummy^=i;
	      servo2_off;

	      for(i=0; i<20000; i++) dummy^=i; // Impulspause
		}

	   stellzeit=100;
	   while(stellzeit--)
	   {
	      servo1_on;
	      for(i=0; i<2500; i++) dummy^=i;
	      servo1_off;

	      servo2_on;
	      for(i=0; i<2500; i++) dummy^=i;
	      servo2_off;

	      for(i=0; i<20000; i++) dummy^=i;
		}
	}
	return(0);
}

Ohne Interrupts und ohne asuro-Lib nur mit Zählschleifen sind nun viele mögliche Störquellen ausgeschaltet. Das läuft auf meinem 8Mhz-Mega32, für 16Mhz sollte es ausreichen die Zählerwerte zu verdoppeln.

Gruß

mic

[pinsel120866]

Mahlzeit Radbruch,

Danke für den Code - ich habe dein Programm geflasht, leider ohne Erfolg.

Mein angepasster Code:

#include <avr/io.h>         // I/O Port definitions 
//#include <avr/interrupt.h>   // Interrupt macros (für cli) 

#define servo1_port PORTB 
#define servo1_ddr  DDRB 
#define servo1_pin  PB5 
#define servo2_port PORTB 
#define servo2_ddr  DDRB 
#define servo2_pin  PB6 
#define servo3_port PORTB 
#define servo3_ddr  DDRB 
#define servo3_pin  PB7 


#define servo1_on servo1_port |= 1<<servo1_pin 
#define servo2_on servo2_port |= 1<<servo2_pin 
#define servo3_on servo3_port |= 1<<servo3_pin 
#define servo1_off servo1_port &= ~(1<<servo1_pin) 
#define servo2_off servo2_port &= ~(1<<servo2_pin) 
#define servo3_off servo3_port &= ~(1<<servo3_pin)

uint16_t i, stellzeit, dummy; 

int main(void) 
{ 
   servo1_ddr |= 1<<servo1_pin; // Pins als Ausgang definieren 
   servo2_ddr |= 1<<servo2_pin;
   servo3_ddr |= 1<<servo3_pin;
   servo1_off;                  // und auf low schalten 
   servo2_off;
   servo3_off; 
   //cli();                       // keine Störungen erlauben 
   while(1)                     // solange Saft im Akku... 
   { 
      stellzeit=100;            // 100 mal den Impuls senden 
      while(stellzeit--) 
      { 
         servo1_on;             // Impuls für erstes Servo erzeugen 
         for(i=0; i<500; i++) dummy^=i; 
         servo1_off; 

         servo2_on;             // Impuls für zweites Servo erzeugen 
         for(i=0; i<500; i++) dummy^=i; 
         servo2_off; 

        servo3_on;             // Impuls für drittes Servo erzeugen 
         for(i=0; i<500; i++) dummy^=i; 
         servo3_off; 
 
         for(i=0; i<20000; i++) dummy^=i; // Impulspause 
      } 

      stellzeit=100; 
      while(stellzeit--) 
      { 
         servo1_on; 
         for(i=0; i<2500; i++) dummy^=i; 
         servo1_off; 

         servo2_on; 
         for(i=0; i<2500; i++) dummy^=i; 
         servo2_off;
		 
         servo3_on; 
         for(i=0; i<2500; i++) dummy^=i; 
         servo2_off;  

         for(i=0; i<20000; i++) dummy^=i; 
      } 
   } 
   return(0); 
}

Das Blinkprogramm von gestern hat ja gepasst - damit müsste eigentlich auch der Prozessortakt passen, oder?

[pinsel120866]

Kommando zurück, ich habe jetzt 3 neue PINs genommen (PB0,PB1,PB2), damit reagieren die Servos. Nach dem Einschalten des Asuros drehen die Servos bis zum Anschlag im Gegenuhrzeigersinn und Ticken dan weiter.

[radbruch]

Die Zählwerte in den Schleifen müssen noch verdoppelt werden. Jetzt senden wir nur halbe Impulszeiten deshalb gehen die Servos auf Anschlag. Schön dass sich was dreht:)

[pinsel120866]

Hilf mir bitte mal, wo ich verdoppeln muss:

#include <avr/io.h>         // I/O Port definitions 
//#include <avr/interrupt.h>   // Interrupt macros (für cli) 

#define servo1_port PORTB 
#define servo1_ddr  DDRB 
#define servo1_pin  PB0 
#define servo2_port PORTB 
#define servo2_ddr  DDRB 
#define servo2_pin  PB1 
#define servo3_port PORTB 
#define servo3_ddr  DDRB 
#define servo3_pin  PB2 


#define servo1_on servo1_port |= 1<<servo1_pin 
#define servo2_on servo2_port |= 1<<servo2_pin 
#define servo3_on servo3_port |= 1<<servo3_pin 
#define servo1_off servo1_port &= ~(1<<servo1_pin) 
#define servo2_off servo2_port &= ~(1<<servo2_pin) 
#define servo3_off servo3_port &= ~(1<<servo3_pin) 

uint16_t i, stellzeit, dummy; 

int main(void) 
{ 
   servo1_ddr |= 1<<servo1_pin; // Pins als Ausgang definieren 
   servo2_ddr |= 1<<servo2_pin; 
   servo3_ddr |= 1<<servo3_pin; 
   servo1_off;                  // und auf low schalten 
   servo2_off; 
   servo3_off; 
   //cli();                       // keine Störungen erlauben 
   while(1)                     // solange Saft im Akku... 
   { 
      stellzeit=100;            // 100 mal den Impuls senden 
      while(stellzeit--) 
      { 
         servo1_on;             // Impuls für erstes Servo erzeugen 
         for(i=0; i<1000; i++) dummy^=i; 
         servo1_off; 

         servo2_on;             // Impuls für zweites Servo erzeugen 
         for(i=0; i<1000; i++) dummy^=i; 
         servo2_off; 

        servo3_on;             // Impuls für drittes Servo erzeugen 
         for(i=0; i<1000; i++) dummy^=i; 
         servo3_off; 
  
         for(i=0; i<40000; i++) dummy^=i; // Impulspause 
      } 

      stellzeit=100; 
      while(stellzeit--) 
      { 
         servo1_on; 
         for(i=0; i<5000; i++) dummy^=i; 
         servo1_off; 

         servo2_on; 
         for(i=0; i<5000; i++) dummy^=i; 
         servo2_off; 
       
         servo3_on; 
         for(i=0; i<5000; i++) dummy^=i; 
         servo2_off;  

         for(i=0; i<40000; i++) dummy^=i; 
      } 
   } 
   return(0); 
}

Damit drehen die Servos

[radbruch]

Na prima. 1000/5000 sind die Werte für die Impulse min/max, 40000 die 20ms-Wiederholung. So könntest du knapp deine 12 Servos ansteuern wenn du pausenlos Impulse senden würdest. Das ist zwar ein Weg der vermutlich in die Sackgasse führt, aber du brauchst erst etwas Übung mit Servos. Das Ansteuern kannst du auch mit deinen drei Servo lernen.

Zuerst solltest du den Wert für die Pause optimieren. Wenn er zu groß ist zuckeln die Servos. Die Impulspause ist nun auch der Platz im Programm zum Berechen neuer Servopositionen. Dazu verwendest du dann Variablen für die Positionen und nach der Pause, noch vor dem Start der nächsten Impulse, berechnest du die neuen Positionen und änderst die entsprechenden Variablen. Die Zeit die das Programm dafür benötigt gewinnst du durch Verkürzung der Pausenzeiten. (Wenn du dich damit beschäftigst: die Schwankungen der Pausezeiten durch unterschiedliche Impulsdauern sollte man auch berücksichtigen. Impulse+Pause sollte ca. 20ms sein)

#include <avr/io.h>         // I/O Port definitions
//#include <avr/interrupt.h>   // Interrupt macros (für cli)

#define servo1_port PORTB
#define servo1_ddr  DDRB
#define servo1_pin  PB0
#define servo2_port PORTB
#define servo2_ddr  DDRB
#define servo2_pin  PB1
#define servo3_port PORTB
#define servo3_ddr  DDRB
#define servo3_pin  PB2


#define servo1_on servo1_port |= 1<<servo1_pin
#define servo2_on servo2_port |= 1<<servo2_pin
#define servo3_on servo3_port |= 1<<servo3_pin
#define servo1_off servo1_port &= ~(1<<servo1_pin)
#define servo2_off servo2_port &= ~(1<<servo2_pin)
#define servo3_off servo3_port &= ~(1<<servo3_pin)

uint16_t i, stellzeit, dummy;
uint16_t servo1_position, servo2_position, servo3_position;

int main(void)
{
   servo1_ddr |= 1<<servo1_pin; // Pins als Ausgang definieren
   servo2_ddr |= 1<<servo2_pin;
   servo3_ddr |= 1<<servo3_pin;
   servo1_off;                  // und auf low schalten
   servo2_off;
   servo3_off;
   servo1_position=1000;
   //cli();                       // keine Störungen erlauben
   while(1)                     // solange Saft im Akku...
   {
      stellzeit=100;            // 100 mal den Impuls senden
      while(stellzeit--)
      {
         servo1_on;             // Impuls für erstes Servo erzeugen
         for(i=0; i<servo1_position; i++) dummy^=i;
         servo1_off;

         for(i=0; i<40000; i++) dummy^=i; // Impulspause
      }
		// neue Position berechen
		if(servo1_position == 1000) servo1_position=5000; else servo1_position=1000;
   }
   return(0);
}

Das wird dann auch zum Ziel führen wenn man schrittweise optimiert. Servoansteuerung als Funktion gestalten und zyklisch aufrufen, dann als ISR im Hintergrund. Aber das ist eher ein Ausblick auf die nächsten Hürden.

Gruß

mic

[pinsel120866]

Nun habe ich dieses Programm gefasht:

#include "asuro.h" 

unsigned char i, servo_stellzeit; 

void servo(unsigned char winkel)
{ 
unsigned int count=0; 
do{ 
   count++; 
   if (ON || ON || ON) { 
      DDRB |= (1 << PB0) | (1 << PB1) | (1 << PB2); 
      PORTB |= (1 << PB0) | (1 << PB1) | (1 << PB2); 
   } 
   FrontLED(ON); 
   Sleep(winkel); 
   if (!OFF) PORTB &= ~(1 << PB2); 
   if (!OFF) PORTB &= ~(1 << PB1); 
   if (!OFF) PORTB &= ~(1 << PA0); 
   FrontLED(OFF); 
   Sleep(255); Sleep(255); Sleep(255); 
}while (count<servo_stellzeit); 
} 


int main(void) { 

Init(); 
BackLED(OFF,OFF); 
do{ 
servo_stellzeit=35; 
servo(51); 
servo(90); 
servo(51); 
servo(15); 
servo_stellzeit=2; 
for (i=15; i<88; i+=2) servo(i); 
for (i=90; i>17; i-=2) servo(i); 
}while (1); 
return 0;  
}

Die FrontLED flackert ganz schnell, der Servo dreht: 90° Rechts - 90° Rechts - 90° Links - 90° Links - 180° Rechts - 180° Links

Wenn ich allerdings einen 2. Servo anhänge stimmt der Ablauf nicht mehr, bei 3 angehängten Servos dreht gar keiner mehr.

Wie muss ich vorgehen um alle 3 Parallel betreiben zu können?

[pinsel120866]

Ich habe den Code auf 3 Servos erweitert:

#include "asuro.h" 

unsigned char i, j, k, servo_stellzeit; 

void servo1(unsigned char winkel)
{ 
unsigned int count=0; 
do{ 
   count++; 
   if (ON)  { 
      DDRB |= (1 << PB0); 
      PORTB |= (1 << PB0); 
   } 
   Sleep(winkel); 
   if (!OFF) PORTB &= ~(1 << PB0); 
   Sleep(255); Sleep(255); Sleep(255); 
}while (count<servo_stellzeit); 
} 

void servo2(unsigned char winkel)
{ 
unsigned int count=0; 
do{ 
   count++; 
   if (ON)  { 
      DDRB |= (1 << PB1); 
      PORTB |= (1 << PB1); 
   } 
   Sleep(winkel); 
   if (!OFF) PORTB &= ~(1 << PB1); 
   Sleep(255); Sleep(255); Sleep(255); 
}while (count<servo_stellzeit); 
} 

void servo3(unsigned char winkel)
{ 
unsigned int count=0; 
do{ 
   count++; 
   if (ON)  { 
      DDRB |= (1 << PB2); 
      PORTB |= (1 << PB2); 
   } 
   Sleep(winkel); 
   if (!OFF) PORTB &= ~(1 << PB2); 
   Sleep(255); Sleep(255); Sleep(255); 
}while (count<servo_stellzeit); 
} 

int main(void) { 

Init(); 
do{ 
servo_stellzeit=35; 
servo1(51); 
servo2(51);
servo3(51);
servo1(90);
servo2(90);
servo3(90); 
servo1(51);
servo2(51);
servo3(51); 
servo1(15);
servo2(15);
servo3(15);
servo_stellzeit=2; 
for (i=15; i<88; i+=2) servo1(i); 
for (j=15; j<88; j+=2) servo2(j);
for (k=15; k<88; k+=2) servo3(k);
for (i=90; i>17; i-=2) servo1(i); 
for (j=90; j>17; j-=2) servo2(j);
for (k=90; k>17; k-=2) servo3(k);
}while (1); 
return 0; 
}

Nun arbeiten alle 3 Servos brav nacheinander ihre Drehpostitionen ab. Einen Weg dass z.B. 2 Servos nahezu gleichzeitig drehen habe ich noch keinen gefunden.

[pinsel120866]

Hallo Radbruch,

bei meinem "Servomultitasking" komme ich nicht weiter, kannst du mir sagen wie ich den Code anpassen muss damit mehrere Servos gleichzeitig arbeiten?

[radbruch]

Mit asuro.h läuft es nicht mehr auf meinem RP6, deshalb ungetestet:

#include "asuro.h"

unsigned char i, j, k, servo_stellzeit;

void servo(unsigned char winkel0, unsigned char winkel1, unsigned char winkel2)
{
	unsigned int count=0;
	do{
	   count++;

	   if(winkel0){
		PORTB |= (1 << PB0);
	   Sleep(winkel0);
	   }
	   PORTB &= ~(1 << PB0);

	   if(winkel1){
	   PORTB |= (1 << PB1);
	   Sleep(winkel1);
	   }
	   PORTB &= ~(1 << PB1);

	   if(winkel1){
	   PORTB |= (1 << PB2);
	   Sleep(winkel2);
	   }
 	   PORTB &= ~(1 << PB2);

   	Sleep(255-winkel0); Sleep(255-winkel1); Sleep(255-winkel2);
	}while (count<servo_stellzeit);
}

int main(void) {

	Init();
	DDRB |= (1 << PB2) | (1 << PB1) | (1 << PB0);

	do{
		servo_stellzeit=35;
		servo(51, 51, 51);
		servo(90, 90, 90);
		servo(51, 51, 51);
		servo(15, 15, 15);

		servo_stellzeit=2;
		for (i=15; i<88; i+=2) servo(i, 0, 0);
		for (j=15; j<88; j+=2) servo(0, j, 0);
		for (k=15; k<88; k+=2) servo(0, 0, k);
		for (i=90; i>17; i-=2) servo(i, 0, 0);
		for (j=90; j>17; j-=2) servo(0, j, 0);
		for (k=90; k>17; k-=2) servo(0, 0, k);
	}while (1);
return 0;
}