2 Servos gleichzeitig ansteuern geht das nur mit Stopwatches

[RobbyMartin]

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]

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]

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]

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


// 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]

Hallo RobbyMartin,

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

Gruß Dirk

[radbruch]

Hallo

Mit zwei Servos könnte es etwa so funktionieren:

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]

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


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]

Hallo

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

// 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:

http://i3.ytimg.com/vi/fGHDkUlJuh0/default.jpg (http://www.youtube.com/watch?v=fGHDkUlJuh0)
http://www.youtube.com/watch?v=fGHDkUlJuh0

Gruß

mic

[RobbyMartin]

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

http://www.youtube.com/user/maddingoogol#p/u/0/QuIyrsbIIHs

gruß
martin

[Thund3r]

Hallo

Das sieht doch schon recht schick aus ;)

Nur weiter so!

Gruß Thund3r

[RobbyMartin]

Eine frage @ Radbruch

in deinem Programm verstehe ich leider nur den ersten teil ](*,) den komplexeren teil verstehe ich nicht, da die demo mit meinen roboterarm wesentlich besser lief würde ich gerne wissen wie ich es anpassen könnte

mfg
martin

[radbruch]

// Die schon gestarteten Stoppuhren werden auf null gesetzt, alle Demos bei 0 starten
setStopwatch1(0);
setStopwatch2(0);
setStopwatch3(0);

// Nach "for(c=0;c<6;c++)" hat c den Wert 6. Die folgende Schleife wird solange
// ausgeführt wie c einen Wert != 0 enthält.
while(c)
{
// Der Wert in c wird mit Hilfe der LEDs dargestellt.
setLEDs(1<<c);

// Sind seit dem letzten reseten der Stopwatch1 mehr als 1000ms vergangen?
if(getStopwatch1()>1000)
{
// Wenn ja, dann Stopwatch1 wieder auf null setzen und Demo für Servo1 ausführen
setStopwatch1(0);
// Unter Berücksichtigung der letzten Servoposition die nächste Position laden
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;
}
}

// Alle 100ms wird das Demo für Servo2 weitergetaktet
if(getStopwatch2()>100)
{
setStopwatch2(0);
// Servo2 weiterdrehen, wenn Ende Schwenkbereich erreicht ist, wieder neu starten
// Nach jedem Schwenk wird hier c um eins verkleinert bis schlieslich "while(c)"
// nicht mehr erfüült ist und die Demoschleife abgebrochen wird.
servo2++;
if (servo2 > 38) { servo2=14; c--; }
}

if(getStopwatch3()>300)
{
setStopwatch3(0);
// Alle 300ms soll das Servo3 eine Endlage anfahren. Das ist im Video de Beeper
if (servo3 == 10) servo3=50; else servo3=10;
}
task_RP6System(); // Verträglichkeitstest ;)
}
Verständlicher? Ich befürchte nein :(

[RobbyMartin]

So nachdem ich wieder zeit hatte etwas herumzubasteln und programmieren stoße ich auf das nächste problem das demoprogramm von radbruch bewegt im zweiten teil den arm ganz langsam in eine position und schlägt danach ruckartig in die ausgangsposition

siehe hier=> http://www.youtube.com/user/maddingoogol#p/u/1/QuIyrsbIIHs

wie kann man dies vermeiden , sodass der arm sich die ganze zeit langsam und sanft bewegt??

lg
martin

[Thund3r]

Hallo

Du könntest zwischen die Ansteuerung der Servos Pausen machen.

...
for(i=0;i<50;i++)
{
Servo(23);
mSleep(40);
}
...

Je größer die Zeit bei mSleep ist desto langsamer bewegt sich der Servo. Allerdings wird bei zu hoher Sleeptime die Bewegung etwas abgehackt aussehen also immer mit kleinem mSleep arbeiten.

Gruß Thund3r

[RobbyMartin]

das verstehe ich ja nur guck mal in den text von radbruch und mein video an die bewegung nach oben klappt langsam nur nach unten haut der arm

[Thund3r]

Hallo

Ah jezt ich glaub ich weiß was du meinst.
Ähnliches Prob hatte ich bei meinem Arm genauso.
Bei mir lag es schlicht und einfach an der Schwerkraft. Die Servos schafften es mit Kraft den Arm hoch zu hieven und wenn es dann runterging saußte dieser nach unten da kein Widerstand da war und sich der verhältnismäßig schwere Arm nach unten drückte (Schwerkraft).
Ich hab das mit folgender Methode versucht zu kompensieren:

...
int c;
for(c=0;c<=50;c++)
{
Servo3(10);
mSleep(50-c);
}
...
Hier im Video von 0:10 bis 0:12 zu erkennen.
Ich hoffe das ist das was du meintest :D

Gruß Thund3r

[radbruch]

Hallo

Ich habe das nun noch nicht getestet, aber es sollte funktionieren:

int8_t drehrichtung=1;

if(getStopwatch2()>100)
{
setStopwatch2(0);
servo2 += drehrichtung;
if (servo2 > 38) { drehrichtung= -1; c--; }
if (servo2 < 14) { drehrichtung= +1; }
}


Für eine schnellere Bewegung könnte man die Aufrufe häufiger machen, z.B.:
if(getStopwatch2()>50)

oder die Schrittweite erhöhen:
drehrichtung = +2; bzw. drehrichtung = -2;

@Thund3r:
Hier geht es um die nichtblockierende Ansteuerung der Servos mit Timer1-Interrupt und den Stopwatches...

Gruß

mic

[RobbyMartin]

an welche stelle sollte ich den text jetz einfügen ??