2 Servos gleichzeitig ansteuern geht das nur mit Stopwatches

**RobbyMartin** · 31.01.2010, 13:48

Ich will den Roboterarm mit 2 Servos steuern docjh wenn ein Servo nicht mehr angesteert wird verharrt er nicht in der Position sondern fällt nach unten (danke schwerkraft :P ) naja deswegen wäre es zimlich gut wenn ich beide Servos ansteuern könnte.

http://www.youtube.com/watch?v=ndtNgNb7sLM

nur wie soll ich das realisieren??

gruß martin

**radbruch** · 31.01.2010, 13:58

Hallo

Es ist schon möglich mehrere Servos gleichzeitig anzusteuern. Vielleicht zeigst du mal dein bisheriges Programm, dann können wir versuchen, es "aufzubohren".

Gruß

mic

**Thund3r** · 31.01.2010, 14:19

Hallo

Hier ich habe es mit folgendem Quellcode hinbekommen:
for(c=0;c<=50;c++)
{

Servo1(10);
mSleep(30);
Servo2(22);

}
Ruckelt etwas und geht gut auf die Batterie aber funktioniert (Ist noch keine Ideallösung)

http://www.youtube.com/watch?v=VPqT5MsCGLM

(0:10)

Gruß Thund3r

**RobbyMartin** · 31.01.2010, 15:24

Sorry ich weiß nicht wie ich den tex in die Box kriege

aber ist ja auch egal der Text ist noch nicht perfekt ,da es nur die example move1 datei ist die ich abgeändert habe

Code:

// Includes:

#include "RP6RobotBaseLib.h" 	// The RP6 Robot Base Library. 
								
void bumpersStateChanged(void)
{	
	DDRA |= (E_INT1);            // PA4 (IT1) als Ausgang definieren 
    DDRC |= (SCL | SDA);         // PC0, PC1 als Ausgänge definieren 
    uint8_t i;					  // Variable i 
	uint8_t a;					  // Variable a
	uint8_t c;					  // Variable c
    PORTA |= E_INT1;    
	
	if(bumper_left) 
	{
	 
	 for(a=0; a<5; a++) 
		{ 
	 for(i=0; i<70; i++) 
		{ 
	  PORTA |= E_INT1;           // PA4 high 
      sleep(7); 
      PORTA &= ~E_INT1;         // PA4 low 
	  mSleep(15);
		}
		
	 for(c=0; c<70; c++) 
		{ 
	  PORTA |= E_INT1;         // PA4 high 
      sleep(15); 
      PORTA &= ~E_INT1;         // PA4 low 
	  mSleep(15);
		}
		for(i=0; i<70; i++) 
		{ 
	  PORTA |= E_INT1;         // PA4 high 
      sleep(23); 
      PORTA &= ~E_INT1;         // PA4 low 
	  mSleep(15);
		}
		
	 
    }}
	else
		 for(a=0; a<5; a++) 
		{ 
	 for(i=0; i<70; i++) 
		{ 
	  PORTC |= SCL;           // PA4 high 
      sleep(7); 
      PORTC &= ~SCL;         // PA4 low 
	  mSleep(15);
		}
		
	 for(c=0; c<70; c++) 
		{ 
	  PORTC |= SCL;         // PA4 high 
      sleep(15); 
      PORTC &= ~SCL;         // PA4 low 
	  mSleep(15);
		}
		for(i=0; i<70; i++) 
		{ 
	  PORTC |= SCL;         // PA4 high 
      sleep(23); 
      PORTC &= ~SCL;         // PA4 low 
	  mSleep(15);
		}
		
	 
    
    
		
		
		
		
		

	}
	
	if(bumper_right) 
	{
		PORTC |= SDA;            // PC1 high
		mSleep(2000);
	    PORTC &= ~SDA;            // PC1 lo
	}
	
}

/**
 * This function checks Stopwatch1 all the time. If stopwatch 1 is
 * not running, the function does not do anything. As soon as the
 * stopwatch is started, two LEDs begin to blink!
 */
void blink(void)
{
	if(getStopwatch1() > 500) // 500ms
	{
		statusLEDs.LED2 = !statusLEDs.LED2; // Toggle LED bit in LED shadow register... 
		statusLEDs.LED5 = !statusLEDs.LED5;
		updateStatusLEDs();
		setStopwatch1(0);
	}
}

/*****************************************************************************/
// Main:

int main(void)
{
	initRobotBase();
	setLEDs(0b111111);
	mSleep(1500);
	setLEDs(0b100100);

	// Set Bumpers state changed event handler:
	BUMPERS_setStateChangedHandler(bumpersStateChanged);

	
	powerON(); 	// Turn Encoders, Motor Current Sensors 
				// (and ACS IR Receiver and PWRON LED) on.
				// ATTENTION: Automatic Motor control will not work without this!
	
	// -------------------------
	// With the following two commands, the RP6 will start to move in a circle.
	// We simply set the direction to forwards:
	
	

    // and afterwards we set the speed of the left and right motors to different 
	// values (80 for left, and 30 for right in this example):
	
	 PORTA |= E_INT1;   
 
	// The function 
	// void moveAtSpeed(uint8_t desired_speed_left, uint8_t desired_speed_right)
	// sets the desired speeds. The automatic motor control will try to maintain
	// this speed even if the motors get blocked or the robot has to move up a
	// ramp or drive over a small obstacle. 
	// At least this will be the case if you always call task_RP6System() frequently
	// out of the main loop!
	// Maximum speed is 200. And if you execute the command:
	// moveAtSpeed(0,0);
	// the Robot will stop and turn motors off.
	// Values below 8 will not work properly as this is just too slow to regulate
	// accurately. 
	// ATTENTION: If you use high speeds all the time, this will result in shorter
	// gears and motors lifetime. It is recommended to use only speed values
	// between 10 and 160! It is no problem to use 200 for some seconds, but 
	// you should not let the Robot drive at such high speed all the time!

	// Main loop:
	while(true) 
	{
		// If we hit an obstacle, the bumperStateChanged Handler will start
		// Stopwatch 1 and then this task will let two LEDs blink:
		blink();
	
		// In the main loop we need to call task_RP6System() all the time! 
		// This function calls the Motor control functions that automatically
		// regulate the motor speed:
		
		task_RP6System();
	}
	return 0;
}

**Dirk** · 31.01.2010, 15:44

Hallo RobbyMartin,

hiermit:
https://www.roboternetz.de/phpBB2/viewtopic.php?t=45180
... wäre das ziemlich einfach.

Gruß Dirk

**radbruch** · 31.01.2010, 16:08

Hallo

Mit zwei Servos könnte es etwa so funktionieren:

Code:

void bumpersStateChanged(void)
{
	DDRA |= (E_INT1);            // PA4 (IT1) als Ausgang definieren
   DDRC |= (SCL | SDA);         // PC0, PC1 als Ausgänge definieren
   uint8_t i;                 // Variable i
   uint8_t a;                 // Variable a
   uint8_t c;                 // Variable c
   //PORTA |= E_INT1;

	if(bumper_left)
   {
   	for(a=0; a<5; a++)
   	{
    		for(i=0; i<70; i++)
      	{
     			PORTA |= E_INT1;           // Impuls Servo 1 an
      		sleep(7);
      		PORTA &= ~E_INT1;         	// Impuls Servo 1 aus

      		PORTC |= SCL;         		// Impuls Servo 2 an (Position 23 halten)
      		sleep(23);
      		PORTC &= ~SCL;         		// Impuls Servo 2 aus
     			mSleep(15);
      	}

    		for(c=0; c<70; c++)
      	{
     			PORTA |= E_INT1;        	// Impuls Servo 1 an
      		sleep(15);
      		PORTA &= ~E_INT1;         	// Impuls Servo 1 aus

      		PORTC |= SCL;         		// Impuls Servo 2 an (Position 23 halten)
      		sleep(23);
      		PORTC &= ~SCL;         		// Impuls Servo 2 aus
     			mSleep(15);
      	}
      	for(i=0; i<70; i++)
      	{
     			PORTA |= E_INT1;         	// Impuls Servo 1 an
      		sleep(23);
      		PORTA &= ~E_INT1;         	// Impuls Servo 1 aus

      		PORTC |= SCL;         		// Impuls Servo 2 an (Position 23 halten)
      		sleep(23);
      		PORTC &= ~SCL;         		// Impuls Servo 2 aus
     			mSleep(15);
      	}
		}
	}
   else
   {
   	for(a=0; a<5; a++)
      {
    		for(i=0; i<70; i++)
      	{
     			PORTC |= SCL;           	// Impuls Servo 2 an
      		sleep(7);
      		PORTC &= ~SCL;         		// Impuls Servo 2 aus

     			PORTA |= E_INT1;         	// Impuls Servo 1 an (Position 23 halten)
      		sleep(23);
      		PORTA &= ~E_INT1;         	// Impuls Servo 1 aus
     			mSleep(15);
      	}
    		for(c=0; c<70; c++)
      	{
     			PORTC |= SCL;         		// Impuls Servo 2 an
      		sleep(15);
      		PORTC &= ~SCL;         		// Impuls Servo 2 aus

     			PORTA |= E_INT1;         	// Impuls Servo 1 an (Position 23 halten)
      		sleep(23);
      		PORTA &= ~E_INT1;         	// Impuls Servo 1 aus
     			mSleep(15);
      	}
      	for(i=0; i<70; i++)
      	{
     			PORTC |= SCL;         		// Impuls Servo 2 an
      		sleep(23);
      		PORTC &= ~SCL;         		// Impuls Servo 2 aus

     			PORTA |= E_INT1;         	// Impuls Servo 1 an (Position 23 halten)
      		sleep(23);
      		PORTA &= ~E_INT1;         	// Impuls Servo 1 aus
     			mSleep(15);
      	}
	   }
	}
	
   if(bumper_right) // ???
   {
      PORTC |= SDA;            // PC1 high
      mSleep(2000);
      PORTC &= ~SDA;            // PC1 lo
   }

}

Allerdings werden die Servos nur angesteuert, wenn ein Bumper betätigt wird. Und während sich die Servos bewegen ist das gesamte restliche System blockiert. Langfristig wirst du wohl auf eine interruptgesteuerte Servoansteuerung umsteigen müssen. Anregungen dazu findest du z.b. hier:

https://www.roboternetz.de/phpBB2/viewtopic.php?t=34407

Gruß

mic

**suicide** · 31.01.2010, 19:16

Man könnte alle 2/256ms einen Zähler hochzählen und ihn mit den gewünschten Positionen der Servos vergleichen. Ist der Zähler kleiner als die Servoposition, wird der Ausgang für den Servo geschaltet, ansonsten halt nicht.

Als Pseudocode

Code:

servo0 = 100
servo1 = 200

solange(1) {

   solange (zähler<2ms) {
      erhöhe zähler um 1
      wenn zähler < servo0 dann aktiviere ausgang servo0 sonst deaktiviere ausgang servo0
      wenn zähler < servo1 dann aktiviere ausgang servo1 sonst deaktiviere ausgang servo1
   }
   deaktiviere alle ausgänge für 18ms
   starte von vorn;
}

Statt die Ausgänge hintereinander zu setzen ist es natürlich auch möglich, die Bits in einem Byte entsprechend der gesetzten Ausgänge zu ändern und das Byte an den Port zu schicken.

Damit wäre der Controller vermutlich etwas überlastet oder? Wenn man keine Auflösung von 256 Schritten benötigt, könnte man die Auflösung auch kleiner wählen und hätte zwischen den einzelnen Zählervergleichen mehr Zeit für andere Dinge.

**radbruch** · 31.01.2010, 23:48

Hallo

Jetzt bin ich endlich über eine einfache Servoansteuerung gestolpert die auch noch libverträglich zu sein scheint:

Code:

// Servoansteuerung mit Timer1                                         31.1.2010 mic

// Einfach und elegant, warum finde ich das erst jetzt? Timer1 (RP6-Motoransteuerung)
// läuft ja sowieso im Hintergrund. Deshalb kann man die "klassische" Servoansteuerung
// in die Overflow-ISR einbauen und mit ca. 19kHz aufrufen lassen. Timersetup der Lib:
// Mode 10, Phase Correct mit ICR1 als Top ergibt bei ICR1=210 ca. 8MHz/420=19047,6Hz ;)

// Drehbereich meiner RS-2-Servos ist von ca. 14 bis ca. 38

#include "RP6RobotBaseLib.h"

volatile uint8_t servo1, servo2, servo3, p; 	// Servopositionen und Impulszähler
uint8_t c; 												// ein Char zur freien Verfügung

int main(void)
{
   initRobotBase();
   servo1=servo2=servo3=26;	// Servomitte?
   TIMSK |= (1 << TOIE1); 		// Die Timer1 Overflow-ISR zur Servoansteuerung
	DDRA |= (E_INT1);          // Servopins auf Ausgang setzen
   DDRC |= (SCL | SDA);
   setLEDs(1);                // und los!
   startStopwatch1();
   startStopwatch2();
   startStopwatch3();
	while(1)
   {
   	for(c=0;c<6;c++)            // 6 mal ein einfaches Demo....
		{
		   setLEDs(1<<c);
			servo1=servo2=servo3=26; // mitte
   		p=50; while(p);          // warten bis 50 Impulse gesendet (ca. 1 Sek.)
   		servo1=servo2=servo3=14; // links
   		p=50; while(p);
   		servo1=servo2=servo3=38; // rechts
   		p=50; while(p);
		}
		setStopwatch1(0);
		setStopwatch2(0);
		setStopwatch3(0);
		while(c)                    // und 6 mal ein etwas anspruchsvolleres Demo
		{
		   setLEDs(1<<c);
		   if(getStopwatch1()>1000)
		   {
		      setStopwatch1(0);
				switch(servo1)
		      {
		         case 38: servo1=30; break;
		         case 30: servo1=20; break;
		         case 20: servo1=14; break;
		         case 14: servo1=21; break;
		         case 21: servo1=31; break;
		         case 31: servo1=38; break;
				}
			}
		   if(getStopwatch2()>100)
		   {
		      setStopwatch2(0);
		      servo2++;
		      if (servo2 > 38) { servo2=14; c--; }
			}
		   if(getStopwatch3()>300)
		   {
		      setStopwatch3(0);
		      if (servo3 == 10) servo3=50; else servo3=10;
			}
			task_RP6System(); // Verträglichkeitstest ;)
		}
   }
   return 0;
}
ISR (TIMER1_OVF_vect)
{
	static uint16_t servocount=1;
	if(servocount > servo1) PORTA &= ~E_INT1; else PORTA |= E_INT1; 		// PA4 XBUS 8
	if(servocount > servo2) PORTC &= ~SCL; else PORTC |= SCL; 				// PC0 XBUS 10
	if(servocount > servo3) PORTC &= ~SDA; else PORTC |= SDA; 				// PC1 XBUS 12
	if(servocount < 400) servocount++; else {servocount=1; if(p) p--;} 	// p = 1/50 Sek
}

Blöderweise finde ich grad nicht genug Servos zum Testen:

Bild hier
http://www.youtube.com/watch?v=fGHDkUlJuh0

Gruß

mic

**RobbyMartin** · 12.02.2010, 13:41

So habe es jetzt endlich geschafft den roboterarm halbwegs zu vollenden das mit den Servos klappt auch ganz gut

http://www.youtube.com/user/maddingo.../0/QuIyrsbIIHs

gruß
martin