blockierende funktionen ausschalten

[proevofreak]

hallo, gerade ein einfaches programm zum verstehen der blockierenden funktionen geschrieben. der roboter soll beim fahren mit hilfe dem acs und den bumpern hindernissen ausweichen. nur leider tut er das nicht mehr, nachdem einer der bumper gedrückt wird.
wer kann mir sagen was ich an dem programm verändern muss?
(dürfte ja an den move bzw. rotate funktionen liegen)






#include "RP6RobotBaseLib.h"
void acsStateChanged(void)
{if(obstacle_left)
{writeString_P("Hindernis links\n");
moveAtSpeed(100,60);}

else if(obstacle_right)
{writeString_P("Hindernis rechts\n");
moveAtSpeed(60,100);}


else
{moveAtSpeed(80,80);}
}

void bumpersStateChanged(void)
{if(bumper_left)
{move(50,BWD,DIST_MM(150),true);
rotate(50,RIGHT,90,false);
}

if(bumper_right)
{move(50,BWD,DIST_MM(150),true);
rotate(50,RIGHT,90,false);
}
}


int main(void)
{initRobotBase();
ACS_setStateChangedHandler(acsStateChanged);
powerON();
setACSPwrMed();
BUMPERS_setStateChangedHandler(bumpersStateChanged );

while(true)
{
task_ACS();
task_Bumpers();
task_RP6System();

}
return 0;
}




danke schon mal im voraus

mfg

[RP6conrad]

In ihre main program steht eigentlich nicht was der RP6 machen soll wan da keine Hindernis von ACS/Bumper ist !! Beim Anfang functioniert das einigermasse durch die {moveAtSpeed(80,80);} in der ACSstatechangedhandler. Diese Auftragen laufen weiter wen du das einmal anruft. Einmal der ACS etwas sieht, wird der moveAtSpeed einmal aufgerufen.
Bei den Bumpertask brauchts du ein {move(50,BWD,DIST_MM(150),true); nach die 150 mm komt dan noch die Rotate mit 90° und dan ist schluss !! Eine neue Move-Auftrag komt nur als der ACS etwas sieht ; dan wird wieder diese ACSstatehandler Function aufgerufen !!
Besser ist um diese Abfrage in das "Main" teil zu machen, und nicht in diese Handlerfunctionen !!

[proevofreak]

das muss aber auch anders gehen. wenn du dir zum beispiel das beispielprogramm move5 anschaust, kannst du sehen, dass keine dieser abfragen in den main-teil gemacht wurde.
es muss also auch anders gehen. die frage ist nur wie?

[SlyD]

Du darfst keine blockierenden Funktionen (nichts länger als ein paar Millisekunden) in den Bumper Event Handler einfügen.
Das hier: move(50,BWD,DIST_MM(150),true);
blockiert.
Bewegungsabläufe (Rückwärtsfahren + Drehen) nach Kollision machst Du am einfachsten komplett OHNE den Event Handler komplett blockierend, oder aber (besser) mit einer Statemachine und Abfrage ob die Bewegung beendet wurde.

Ohne Event handler - einfach in der Main loop die Bumper Variablen abfragen.
Aber vorsicht: wenn dann in einem anderen Event Handler auch bewegungsfunktionen aufgerufen werden, müsste das mit einer zusätzlichen Variable gesperrt werden (also "markieren" das da schon jemand die Motoren verwendet).
Aber das reicht nur für einfache Dinge ... komplexere Sachen macht man dann besser mit Statemachines / Subsumption.

MfG,
SlyD

[proevofreak]

Hallo, ich weiß ich habe damals schon einmal wegen den blockierenden funktionen eine frage gestellt.

aber jetzt 2 monate später stehe ich wieder vor einem ähnlichen problem.
mittlerweile habe ich an meinen rp6 einen roboarm mit greifer gebaut.

vorn am greifer habe ich einen reflexkoppler gebaut, mit welchem der greifer gesteuert wird.

es funktioniert eigentlich fast alles.

nur ist es so, dass der rp6 nicht anhält, wenn der reflexkoppler ein teil erkennt.

ich bin mit ziemlich sicher, dass es an meiner move_At_Speed() funktion liegt.
nur habe ich keine idee, wie ich dieses problem umgehen kann.

das fast fertige programm:




#include "RP6RobotBaseLib.h"

uint8_t i;
uint8_t a;
uint8_t n;
uint8_t p;
uint8_t z;


void task_TeilErkannt(void)
{
if(PINA & (1<<4))
{z=1;
writeString_P("Teil erkannt\n");}
else {z=0;
writeString_P("Kein Teil erkannt\n");}

}



void task_servo(void)
{

if(PINA & (1<<4)) // Greifer schließen
{
while(i<52)
{
writeString_P("Greifer schließen\n");
PORTC |=(1<<0);
mSleep(1);
PORTC &=~ (1<<0);
mSleep(19);
p=0;
i++;}
}


if(i>50) // Schwenkarm nach oben
{
while(a<52)
{
writeString_P("Schwenkarm nach oben\n");
PORTC |=(1<<1);
mSleep(2);
PORTC &=~ (1<<1);
mSleep(18);
i=0;
a++;}
}


if(a>50) // Greifer öffnen
{
while(n<40)
{
writeString_P("Greifer öffnen\n");
PORTC |=(1<<0);
mSleep(2);
PORTC &=~ (1<<0);
mSleep(18);
setStopwatch1(0);
a=0;
n++;}

}

if(n>38) //Schwenkarm wieder nach unten
{
while(p<20)
{
writeString_P("Schwenkarm wieder nach unten\n");
PORTC |=(1<<1);
mSleep(1);
PORTC &=~ (1<<1);
mSleep(19);
n=0;
p++;}
}

}

void task_fahren(void)
{if(z==0)
{
moveAtSpeed(50,50);}


else if(z==1)
{moveAtSpeed(0,0);}

}


int main(void)
{
initRobotBase();

DDRC |=(1<<1); //PORT 1 von Register C als Ausgang für Servo
DDRC |= (1<<0); //PORT 0 von Register C als Ausgang für Servo
DDRA &=~ (1<<4); //PORT 4 von Register A als Eingang für Reflexkoppler

powerON();
changeDirection(BWD);





while(true)
{task_servo();
task_TeilErkannt();
task_fahren();
task_RP6System();
}
return 0;
}


hat jemand von euch eine idee?

danke schon mal im voraus

[RP6conrad]

Ich glaube das die While Schleifen die Ursache sein. Sobald der optokoppler etwas sieht, fangen die While Schleifen an von GreiferSteuerung. Da wird nicht anderes mehr gemacht. Nah das alle while Schleifen aggelaufen sind, kommt er in das unterprogram "Task_fahren" und dan wird diese MoveAtSpeed(0,0) activiert, when der Optokopler noch immer etwas sieht.

[proevofreak]

jetzt hab ich mal das task_TeilErkannt(); aus der while- schleife herausgeholt und ganz normal in die main funktion geschrieben.

aber der rp6 hält jetzt auch nicht an. es hat sich nichts geändert.

ich bin mir ja immer noch relativ sicher dass es an dem moveAtSpeed(50,50) liegt. ich glaub das blockiert dann da moveAtSpeed(0,0).

gruß

[RP6conrad]

Setzt du mal auch eine writeString_P(speed50) und writeString_P(speed0) in diese task-fahren. Dann siehst du schon welche MoveAtSpeed ausgefuhrt wird.
Aber solange den Greifertask lauft, kommt er uberhaupt nicht in das task_fahren.

[proevofreak]

Jetzt hab ich das Programm einmal nochmal umgeschrieben. Aber ich habe immer noch das gleiche Problem.

Der RP6 fährt nach wie vor die ganze zeit und bleibt nicht während dem greifen und aufnehmen stehen.

hier das abgeänderte Programm:




#include "RP6RobotBaseLib.h"

uint8_t i;
uint8_t a;
uint8_t n;
uint8_t p;
uint8_t z;


void task_TeilErkannt(void)
{
if(PINA & (1<<4))
{z=1;
writeString_P("Teil erkannt\n");
mSleep(5000);
z=0;}

}



void task_servo(void)
{

if(PINA & (1<<4)) // Greifer schließen
{
while(i<52)
{
writeString_P("Greifer schließen\n");
PORTC |=(1<<0);
mSleep(1);
PORTC &=~ (1<<0);
mSleep(19);
p=0;
i++;}
}


if(i>50) // Schwenkarm nach oben
{
while(a<52)
{
writeString_P("Schwenkarm nach oben\n");
PORTC |=(1<<1);
mSleep(2);
PORTC &=~ (1<<1);
mSleep(18);
i=0;
a++;}
}


if(a>50) // Greifer öffnen
{
while(n<40)
{
writeString_P("Greifer öffnen\n");
PORTC |=(1<<0);
mSleep(2);
PORTC &=~ (1<<0);
mSleep(18);
setStopwatch1(0);
a=0;
n++;}

}

if(n>38) //Schwenkarm wieder nach unten
{
while(p<20)
{
writeString_P("Schwenkarm wieder nach unten\n");
PORTC |=(1<<1);
mSleep(1);
PORTC &=~ (1<<1);
mSleep(19);
n=0;
p++;}
}

}

void task_fahren(void)
{if(z==0)
{
moveAtSpeed(50,50);
writeString_P("Fahren\n");}
else
{moveAtSpeed(0,0);
writeString_P("Nicht fahren\n");}

}


int main(void)
{
initRobotBase();

DDRC |=(1<<1); //PORT 1 von Register C als Ausgang für Servo
DDRC |= (1<<0); //PORT 0 von Register C als Ausgang für Servo
DDRA &=~ (1<<4); //PORT 4 von Register A als Eingang für Reflexkoppler

powerON();
changeDirection(BWD);
task_TeilErkannt();




while(true)
{task_servo();

task_fahren();
task_RP6System();
}
return 0;
}



Wer kann mir sagen an was das liegt?

gruß

[proevofreak]

kann mir denn niemand sagen an was das liegt?

[RP6conrad]

Versuch erst mal die Fahr-Auftrage ohne Greifer Function Functionieren zu lassen. Einfach in haupt Schleife taskServo weglassen. Bleibt dan den RP6 stehen wen die Eingang von Sensor hoch ist ?
So ja, dan hangt das ab von diese taskServo. Wahrscheinlich bleibt er dan in eine While Schleife ewig hangen.
Functioniert diesen taskServo eigentlich zwei mal nach ein ander ? Sensor ist hoch, Greifer lauft, Sensor ist aus, Greifer steht. Sensor wieder hoch, Greifer lauft wieder.
Bei SW Problemen muss men immer Schritt pro Schritt weiter gehen. Nicht zu fiel zusammen aendern. Moglich so programmieren das Functionen unabhangig voneinander laufen. Diese While Schleifen sind dan nicht der richtige Weg.

[radbruch]

Hallo

Am Ende von task_TeilErkannt() wird z=0 gesetzt, das bedeutet für task_fahren() dann natürlich fahren:

void task_fahren(void)
{if(z==0)
{
moveAtSpeed(50,50);
writeString_P("Fahren\n");}
.....

Das z=0 sollte wohl besser im else-Zweig sein und die 5 Schlafsekunden würde ich ersatzlos streichen weil er dann 5 Sekunden weiterdüst obwohl das hinderniss erkannt wurde? Fehlt hier nicht noch ein MoveAtSpeed():

powerON();
changeDirection(BWD);
task_TeilErkannt();

Vermutlich wäre es so richtiger:

powerON();
changeDirection(BWD);

while(true)
{task_servo();
task_TeilErkannt();
task_fahren();
task_RP6System();
}

Gruß

mic

[proevofreak]

ich habe jetzt das ganze programm nochmal gründlich überarbeitet.
das task_TeilErkannt(); habe ich jetzt im main teil in die while(true) schleife verschoben.



#include "RP6RobotBaseLib.h"

uint8_t i;
uint8_t a;
uint8_t n;
uint8_t p;
uint8_t z;

void task_teil_erkannt(void)
{if(PINA & (1<<4))
{z=1;
writeString_P("Teil erkannt\n");}

if(z==1)
{startStopwatch1();}

if(getStopwatch1()>3000)
{z=0;
setStopwatch1(0);}
}




void task_servo(void)
{

if(PINA & (1<<4)) // Greifer schließen
{
while(i<52)
{
writeString_P("Greifer schließen\n");
PORTC |=(1<<0);
mSleep(1);
PORTC &=~ (1<<0);
mSleep(19);
p=0;
i++;}
}


if(i>50) // Schwenkarm nach oben
{
while(a<52)
{
writeString_P("Schwenkarm nach oben\n");
PORTC |=(1<<1);
mSleep(2);
PORTC &=~ (1<<1);
mSleep(18);
i=0;
a++;}
}


if(a>50) // Greifer öffnen
{
while(n<40)
{
writeString_P("Greifer öffnen\n");
PORTC |=(1<<0);
mSleep(2);
PORTC &=~ (1<<0);
mSleep(18);
setStopwatch1(0);
a=0;
n++;}

}

if(n>38) //Schwenkarm wieder nach unten
{
while(p<20)
{
writeString_P("Schwenkarm wieder nach unten\n");
PORTC |=(1<<1);
mSleep(1);
PORTC &=~ (1<<1);
mSleep(19);
n=0;
p++;}
}

}

void task_fahren(void)
{if(z==1)
{
moveAtSpeed(0,0);
writeString_P("Nicht Fahren\n");}
else if(z==0)
{moveAtSpeed(50,50);
writeString_P("Fahren\n");}

}


int main(void)
{
initRobotBase();

DDRC |=(1<<1); //PORT 1 von Register C als Ausgang für Servo
DDRC |= (1<<0); //PORT 0 von Register C als Ausgang für Servo
DDRA &=~ (1<<4); //PORT 4 von Register A als Eingang für Reflexkoppler

powerON();
changeDirection(BWD);





while(true)
{task_servo();
task_teil_erkannt();
task_fahren();
task_RP6System();
}
return 0;
}















aber das alte problem besteht immer noch.
der roboter ignoriert den task_teil_erkannt() teil komplett.

die textmeldung "teil erkannt wird nie" ausgegeben.

nur die servos bewegen sich korrekt.

irgendwie wird das alles durch die servo schleifen blockiert.

ich kann mir das nicht erklären.....

[Dirk]

Hallo proevofreak,

irgendwie wird das alles durch die servo schleifen blockiert.
Ja, genau. Und auch noch auf 2 Arten:
1. Die eigentliche Impulserzeugung und Impulswiederholung alle 20ms ist mit den Sleep-Befehlen voll blockierend.
2. Du erzeugst die Bewegung des Greiferarms ebenfalls voll blockierend mit While-Schleifen.

Lösung zu 1:
In diesem Thread stehen Links zu vielen Möglichkeiten, die Impulserzeugung für Servos nicht vollständig blockierend zu machen:
https://www.roboternetz.de/phpBB2/viewtopic.php?t=45790

Lösung zu 2:
Da die Bewegungen des Arms auch Zeit brauchen, würde ich sie nicht in Schleifen abarbeiten, sondern auch in einer Task laufen lassen. Das bedeutet, dass du die Bewegungen mit einer Stopwatch so steuerst, dass z.B. alle 40ms den Servos eine neue Position gegeben wird. Danach wird die Hauptschleife in Main jeweils wieder ausgeführt.

Am Ende sieht deine Main dann so aus:
while(true)
{task_SERVO();
task_teil_erkennen();
task_teil_greifen();
task_fahren();
task_RP6System();
}
Wichtig ist, dass ALLE Tasks (auch die task_teil_greifen) nur so kurz wie möglich sind und die Kontrolle immer rasch wieder an die Hauptschleife abgeben. Das Verhalten der Tasks kannst du mit globalen Flags steuern: Wenn er gerade ein Teil greift, wird z.B. in der task_teil_greifen ein Flag auf true gesetzt, das in der task_fahren benutzt wird, um den RP6 zu stoppen.

Viel Erfolg!

Gruß Dirk