RP6 Servo Ansteuerung

[RP6-Besitzer]

Hallo,
habe jetzt einen Servo an meinen RP6 gebaut und mich ans programmieren gemacht ;)
Hab mich eingelesen und alles grundsätzlich verstanden, hätte aber ein Problem:
Mein Programm geht immer vom aktuellen Winkel aus, d.h. wenn ich das Programm starte dreht er sich 30 grad nach links und 30 grad nach rechts. Verdreh ich den Servo manuell und starte das Programm dreht er sich wieder 30 grad nach links und 30 grad nach rechts.

Mein Programm:

#include "RP6RobotBaseLib.h"

int main(void)
{
initRobotBase();
DDRA |= 1;
while(1)
{
PORTA |= 1;
sleep(5);
PORTA &= ~1;
mSleep(500);
PORTA |= 1;
sleep(30);
PORTA &= ~1;
mSleep(5000);
}


return(0);
}

Eigentlich will ich, dass er sich auf einen bestimmten Winkel einstellt, eine halbe Sekunde wartet und sich dann auf einen anderen Winkel dreht, 5 Sekunden wartet und von vorne...
...nur leider führt er das Programm immer von der aktuellen Postition aus
...und durch ändern der sleep-Zahlen dreht sich der Servo um keinen anderen Winkel, als vorher

was mache ich falsch?
Danke für eure Antworten

[radbruch]

Hallo

So vielleicht:

#include "RP6RobotBaseLib.h"

uint8_t i; //Stellzeit für das Servo

int main(void)
{
initRobotBase();
DDRA |= 1;
while(1)
{
setLEDs(1);
for(i=0; i<50; i++) // 50 mal Servosignal für Position 1 senden
{
PORTA |= 1;
sleep(5); // Impulslänge Position 1 (0,5ms)
PORTA &= ~1;
sleep(200-5); // Impulspause
}

setLEDs(2);
mSleep(500); // Pause 1

setLEDs(4);
for(i=0; i<50; i++) // und 50 mal Servosignal für Position 2 senden
{
PORTA |= 1;
sleep(30); // Impulslänge Position 2 (3ms?)
PORTA &= ~1;
sleep(200-30); // Impulspause
}

setLEDs(8);
mSleep(2000); // Pause 2
}
return(0);
}
(ungetestet)

Das Servosignal muss solange wiederholt werden, bis das Servo seine Position nicht mehr ändert. Da wir keine Positionsrückmeldung erhalten, kann das nur über eine Zeitsteuerung realisiert werden. Günstig ist hierbei, dass der Impuls alle 20ms erzeugt werden sollte, 50 Impulse dauern dann ca. eine Sekunde.


Mein Programm geht immer vom aktuellen Winkel aus...Das Servo fährt immer absolut die zur Impulsdauer gehörende Position an. Wenn du relativ von der jeweiligen Startposition schwenken willst, kannst du das ohne zusätzliche Positionsrückmeldung vergessen.

Die Belegung von ADC0 passt nicht zu den Servosteckern. Das hast du aber sicher beachtet ;)

Gruß

mic

P.S. Bitte Code-Tags verwenden.

[RP6-Besitzer]

Danke Radbruch
Hast mir sehr weitergeholfen, jetzt funktionierts ;)
Ich weiß, das die Belegung von ADC0 nicht zu den Servosteckern passt, hab bei einem Servo die Konstakte umgesteckt. Welche Belegung wäre besser?
Die von XBUS1 oder XBUS2?
Schöne Videos auf YouTube übrigens

[radbruch]

Hallo

Schön, dass es gleich auf Anhieb funktioniert.

Mit "Welche Belegung wäre besser? " meinst vermutlich den USRBUS. Der XBUS ist schon belegt. Ich habe den USRBUS1 verwendet, weil der näher am Mega32 sitzt und die Signalleitungen dadurch kürzer und störungunempfindlicher sind.

Nicht alle meine veröffentlichten Videos sind "schön" ;)

Gruß

mic

[RP6-Besitzer]

Entschuldigung, aber wie erstelle ich solche Code Tags?

[TobiKa]

Klick auf "Antwort erstellen", dann siehst du die dazugehörigen Button.
Man kann übrigens auch ausversehen doppelt abgeschickte Posts selbst löschen!

[RP6-Besitzer]

Danke
Will jetzt das Rudermaschinenansteuerungsprogramm in mein normales "Gerade Fahren und Hindernissen ausweichen" Programm intigrieren.
Hatte auch grundsätzlich gut geklappt, nur mein ACS funktioniert dann nicht mehr.
Hier meine Programme:


#include "RP6RobotBaseLib.h"

void acsStateChanged(void)
{
if(obstacle_left)
rotate(80, RIGHT, 50, true);
if(obstacle_right)
rotate(80, LEFT, 50, true);
if(obstacle_right && obstacle_left)
rotate(60, RIGHT, 100, true);
}
void bumpersStateChanged(void)
{
if(bumper_left || bumper_right)
{
changeDirection(BWD);
move(100, BWD, DIST_MM(200), true);
rotate(50, RIGHT, 120, true);


}
}

int main(void)
{
initRobotBase();
setLEDs(0b111111);
mSleep(500);
setLEDs(0b001001);
BUMPERS_setStateChangedHandler(bumpersStateChanged );
ACS_setStateChangedHandler(acsStateChanged);
powerON();
setACSPwrLow();




while(true)
{
task_RP6System();
changeDirection(FWD);
moveAtSpeed(80,80);
}
return 0;
}


mein geradeausfahr und hindernissen ausweichen programm



#include "RP6RobotBaseLib.h"
uint8_t i;
int main(void)
{
initRobotBase();
DDRA |= 1;
setLEDs(0b010101);
mSleep(1000);
setLEDs(0b101010);

while(1)
{
for (i=0; i<50; i++)
{
PORTA |= 1;
sleep(8);
PORTA &= ~1;
sleep(200-10);
}
mSleep(1000);
for (i=0; i<50; i++)
{
PORTA |= 1;
sleep(23);
PORTA &= ~1;
sleep(200-23);
}

}

return(0);
}


mein Rudermaschinenansteuerungsprogramm



#include "RP6RobotBaseLib.h"

uint8_t i;

void acsStateChanged(void)
{
if(obstacle_left)
rotate(80, RIGHT, 50, true);
if(obstacle_right)
rotate(80, LEFT, 50, true);
if(obstacle_right && obstacle_left)
rotate(60, RIGHT, 100, true);
}
void bumpersStateChanged(void)
{
if(bumper_left || bumper_right)
{
changeDirection(BWD);
move(100, BWD, DIST_MM(200), true);
rotate(50, RIGHT, 120, true);


}
}

int main(void)
{
initRobotBase();
setLEDs(0b111111);
mSleep(500);
setLEDs(0b001001);
DDRA |= 1;
BUMPERS_setStateChangedHandler(bumpersStateChanged );
ACS_setStateChangedHandler(acsStateChanged);
powerON();
setACSPwrLow();




while(1)
{
task_RP6System();
changeDirection(FWD);
moveAtSpeed(100,100);
setLEDs(1);
for(i=0; i<50; i++) // 50 mal Servosignal für Position 1 senden
{
PORTA |= 1;
sleep(8); // Impulslänge Position 1 (0,5ms)
PORTA &= ~1;
sleep(200-5); // Impulspause
}

setLEDs(2);
mSleep(1000); // Pause 1

setLEDs(4);
for(i=0; i<50; i++) // und 50 mal Servosignal für Position 2 senden
{
PORTA |= 1;
sleep(23); // Impulslänge Position 2 (3ms?)
PORTA &= ~1;
sleep(200-30); // Impulspause
}

setLEDs(8);
mSleep(1000); // Pause 2
}
return(0);
}


mein Versuch


Was muss ich ändern, damit das ACS auch funktioniert?
Danke ;)


[/code]-Tags eingefügt von radbruch

[TobiKa]

Du musst die Code-Tags auch wieder schliessen..

[RP6-Besitzer]

Danke, jetzt weiß ich wie's geht ;)
Jemand ne Lösung?

[radbruch]

Hallo

StateChanged-Funktionen und das ACS werden vom Tasksystem aufgerufen. Deine Programmversion ruft das Tasksystem aber nur alle vier Sekunden auf. Fährt der RP6 so überhaupt? Das ist der Punkt warum sich die blockierende Servoansteuerung nicht gut für den RP6 mit seinem Tasksystem eignet.

Eine mögliche Lösung wäre vielleicht, das Tasksystem zusätzlich während der Impulspause des Servos aufzurufen und die sleep()-Pause anzupassen. Blöderweise schwankt aber die Ausführungszeit von task_RP6System() und damit auch die Länge der Impulspause des Servos. Hier muss man wohl etwas mit der Länge des sleep() spielen.

Wenn man das Tasksystem verwendet, sollte man auf gleichzeitige sleep()- und mSleep()-Befehle verzichten. Hier eine ungetestete Version deines Programmes mit einer Stopwatch:


#include "RP6RobotBaseLib.h"

uint8_t i, servoposition;

void acsStateChanged(void)
{
setLEDs(8);
if(obstacle_right && obstacle_left) // beide Seiten?
rotate(60, RIGHT, 100, true);
else if(obstacle_left) // nur links
rotate(80, RIGHT, 50, true);
else if(obstacle_right) // bleibt nur noch rechts übrig
rotate(80, LEFT, 50, true);

changeDirection(FWD); // weiterfahren
moveAtSpeed(100,100);
}

void bumpersStateChanged(void)
{
setLEDs(1);
if(bumper_left || bumper_right)
{
changeDirection(BWD);
move(100, BWD, DIST_MM(200), true);
rotate(50, RIGHT, 120, true);
}
changeDirection(FWD);
moveAtSpeed(100,100);
}

int main(void)
{
initRobotBase();
setLEDs(0b111111);
mSleep(500);
setLEDs(0b001001);
DDRA |= 1;
BUMPERS_setStateChangedHandler(bumpersStateChanged );
ACS_setStateChangedHandler(acsStateChanged);
powerON();
setACSPwrLow();

changeDirection(FWD);
moveAtSpeed(100,100);
servoposition=0; // 0 ist sleep(8), 1 ist sleep(23)
startStopwatch1(); // Stoppuhr starten

while(1)
{
setLEDs(0b100100);
task_RP6System();
sleep(50); // das Tasksystem muss man nicht so häufig aufzurufen

if(getStopwatch1() > 2000) // zwei Sekunden sind vorbei
{
setStopwatch1(0); // Stoppuhr zurücksetzen
setLEDs(0b010010);
for(i=0; i<50; i++) // ein Sekunde lang Servo bewegen
{
PORTA |= 1;
if(servoposition) sleep(23); else sleep(8);
PORTA &= ~1;
sleep(100); // Impulspause ist sleep() und Task zusammen!
task_RP6System(); // beim Servoschwenken weiterfahren
}
if(servoposition) servoposition=0; // andere Servoposition wählen
else servoposition=1;
}
}
return(0);
}


Eine zittrige Servoansteuerung nur mit Stopwatches (und einer Zählschleife für die Impulslängen) hatte ich hier mal versucht:


#include "RP6RobotBaseLib.h"

#define pos1 800
#define pos2 1600

extern uint8_t acs_state;

uint16_t servo_pos, servo_timer, servo_dummy;

int main(void) {

initRobotBase();
DDRA |= 1;

startStopwatch1();
startStopwatch2();
servo_pos = pos1;

while (1)
{
if ((getStopwatch1() > 20) && (acs_state < 2)) // alle 20ms einen Servoimpuls erzeugen
{
setStopwatch1(0);
servo_timer = servo_pos;
PORTA|=1;
while(servo_timer--) servo_dummy ^= servo_timer;
PORTA&=~1;
}

if (getStopwatch2() > 2000) // alle 2000ms Servo auf andere Seite fahren
{
if (servo_pos == pos1) servo_pos=pos2; else servo_pos=pos1;
setStopwatch2(0);
}
//task_RP6System();
task_ADC();
task_ACS();
task_Bumpers();
task_motionControl();
}
return 0;
}Aus https://www.roboternetz.de/phpBB2/zeigebeitrag.php?p=479370#479370

Gruß

mic

[RP6-Besitzer]

Vielen Dank für Deine Antwort
Leider geht beim ersten Programm das ACS garnicht. Bei meinem Programm hat es wie, du schon gesagt hast das ACS nur immerwieder mal aufgerufen.
Woran könnte es liegen?
Also Roboter fährt ganz normal geradeaus, Rudermaschine dreht sich jedoch reagieren weder Bumper noch ACS

[radbruch]

Seltsam, dass es überhaupt mal funktioniert hatte:


powerON();
enableACS();
setACSPwrHigh();


Ich habe meinen RP6 jetzt auch mal aktiviert. Mit der Änderung oben sehe ich mit der Digicam die ACS-LEDs blinken. Wenn ich mit der TV-Fernbedienung auf den IR-Empfänger ziele, dann startet er auch das ACS-Ausweichprogramm. Aber sonst tut sich nichts. Selbst so nicht:


while(1)
{
//setLEDs(0b100100);
task_ACS();
if(obstacle_right || obstacle_left) setLEDs(63); else setLEDs(0b100100);
task_RP6System();


Wenn ich nun die Bumpers betätige, startet er das Bumpersprogramm, wenn ich loslasse, wechselt er zum ACS-Programm?! setLEDs(63) kommt gar nicht.

Muss ich mal forschen, was da nicht funzt. Bin wohl etwas aus der Übung :)

Gruß

mic

[radbruch]

Oje:
#include "RP6RobotBaseLib.h"

uint8_t i, servoposition;

void acsStateChanged(void)
{
setLEDs(8);
if(obstacle_right && obstacle_left) // beide Seiten?
rotate(60, RIGHT, 100, true);
else if(obstacle_left) // nur links
rotate(80, RIGHT, 50, true);
else if(obstacle_right) // bleibt nur noch rechts übrig
rotate(80, LEFT, 50, true);

changeDirection(FWD); // weiterfahren
moveAtSpeed(100,100);
}

void bumpersStateChanged(void)
{
setLEDs(1);
if(bumper_left || bumper_right)
{
changeDirection(BWD);
move(100, BWD, DIST_MM(200), true);
rotate(50, RIGHT, 120, true);
}
changeDirection(FWD);
moveAtSpeed(100,100);
}

int main(void)
{
initRobotBase();
setLEDs(0b111111);
mSleep(500);
setLEDs(0b001001);
DDRA |= 1;
BUMPERS_setStateChangedHandler(bumpersStateChanged );
ACS_setStateChangedHandler(acsStateChanged);
powerON();
enableACS();
setACSPwrLow();

changeDirection(FWD);
moveAtSpeed(100,100);
servoposition=0; // 0 ist sleep(8), 1 ist sleep(23)
startStopwatch1(); // Stoppuhr starten

while(1)
{
setLEDs(0b100100);
task_RP6System();
//sleep(50); // das Tasksystem muss man nicht so häufig aufzurufen

if(getStopwatch1() > 2000) // zwei Sekunden sind vorbei
{
setStopwatch1(0); // Stoppuhr zurücksetzen
setLEDs(0b010010);
for(i=0; i<50; i++) // ein Sekunde lang Servo bewegen
{
PORTA |= 1;
if(servoposition) sleep(23); else sleep(8);
PORTA &= ~1;
sleep(100); // Impulspause ist sleep() und Task zusammen!
task_RP6System(); // beim Servoschwenken weiterfahren
}
if(servoposition) servoposition=0; // andere Servoposition wählen
else servoposition=1;
}
}
return(0);
}
Man sollte eben mit dem Tasksystem kein sleep() verwenden ;)

[radbruch]

Irgendwie wird's nicht wirklich besser, ich breche hier mal ab:

#include "RP6RobotBaseLib.h"

uint8_t i, servoposition;

void acsStateChanged(void)
{
setLEDs(statusLEDs.byte | 0b001001);
if(obstacle_right && obstacle_left) // beide Seiten?
rotate(60, RIGHT, 100, true);
else if(obstacle_left) // nur links
{
setLEDs(statusLEDs.byte & ~1);
rotate(80, RIGHT, 50, true);
}
else if(obstacle_right) // bleibt nur noch rechts übrig
{
setLEDs(statusLEDs.byte & ~8);
rotate(80, LEFT, 50, true);
}

setLEDs(statusLEDs.byte & ~0b001001);
changeDirection(FWD); // weiterfahren
moveAtSpeed(100,100);
}

void bumpersStateChanged(void)
{
setLEDs(statusLEDs.byte | 0b001001);
if(bumper_left || bumper_right)
{
changeDirection(BWD);
move(100, BWD, DIST_MM(200), true);
rotate(50, RIGHT, 120, true);
}
setLEDs(statusLEDs.byte & ~0b001001);
changeDirection(FWD);
moveAtSpeed(100,100);
}

int main(void)
{
initRobotBase();
setLEDs(0b111111);
DDRA |= 1;
BUMPERS_setStateChangedHandler(bumpersStateChanged );
ACS_setStateChangedHandler(acsStateChanged);
powerON();
enableACS();
setACSPwrMed();

mSleep(500);
setLEDs(0b000000);

changeDirection(FWD);
moveAtSpeed(100,100);
servoposition=8;
startStopwatch1(); // Stoppuhr starten

while(1)
{
task_RP6System();

if(getStopwatch1() > 2000) // zwei Sekunden sind vorbei
{
setStopwatch1(0); // Stoppuhr zurücksetzen
if(servoposition==8) // Servoseite anzeigen
setLEDs((statusLEDs.byte & ~16) | 2);
else
setLEDs((statusLEDs.byte & ~2) | 16);

for(i=0; i<50; i++) // ein Sekunde lang Servo bewegen
{
PORTA |= 1; // Impuls senden
cli(); // vorsichtshalber atomar
timer=0; // timer wird in ISR alle 100µs inkrementiert
sei();
while(timer < servoposition) task_RP6System();

PORTA &= ~1; // Impulspause
cli(); // vorsichtshalber atomar
timer=0; // timer wird in ISR alle 100µs inkrementiert
sei();
while(timer < (200-servoposition)) task_RP6System();
}

if(servoposition==8) servoposition=23; // andere Servoposition wählen
else servoposition=8;
}

if((getStopwatch1() % 125) == 0) // Betriebsanzeige
{
if(statusLEDs.byte & 4)
setLEDs((statusLEDs.byte & ~4) | 32); // (Spielerei :)
else // krass!
setLEDs((statusLEDs.byte & ~32) | 4);
}
}
return(0);
}