lienefolger: Xbus benutzen

[Fieke]

Hallo,

Ich habe eine lienefolger gebaut für meine RP6 robot. Ich habe dabei unterstehend schema benutzt von lynxx motion:

http://www.active-robots.com/products/sensors/lynxmotion/tra-v5.pdf

Die lienefolger habe ich auf dem Experimentierplatine gebaut (nicht auf dem RP6 CONTROL M32). Es gibt drei sensoren, jeden sensor hat seine eigenen ausgang. Ich dachte das es möglich ware jeden sensor eine eigenen anschluss zu geben, der INT1, INT2 und INT3.

Aber jetzt weiss ich es nicht, wie sol ich meine sensor communicieren lassen mit dem microcontroler? Und wie sol ich die sensorausgang auslesen?

Hoffentlich kunnen sie mir helfen.

Freundliche gruss,
Fieke

Eintschuldigung für meine schlechtes Deutsch, ich bin eine Hollander.

[Dirk]

@Fieke:
Wenn du den Sensor genau so nachgebaut hast, braucht der 3 Digitalports. Die sind an der RP6Base aber nicht INT1, 2, 3, sondern du hast am XBUS die 3 Ports INT1, SCL und SDA frei, wenn du den I2C-Bus nicht brauchst.
Wie du die 3 Ports ansprechen kannst, zeigt dieser Post:
https://www.roboternetz.de/phpBB2/viewtopic.php?t=38575
Da werden 3 LEDs angesteuert.
Du must nur das Programm so ändern, dass die 3 Ports als Eingang geschaltet werden und im Programm abfragen.

Gruß Dirk

[Fieke]

Danke für ihren Hilfe!

[Fieke]

Jetzt will ich die Ausgänge auslesen. Kann ich nach understehend vorbild einfach mit WriteInteger(SCL, DEC); die Werten von SCL, SDA und die INT1 auslesen?

DDRA |= (E_INT1); // PA4 (IT1) als Ausgang definieren
DDRC |= (SCL | SDA); // PC0, PC1 als Ausgänge definieren

Gruss Fieke

[Dirk]

@Fieke:
Ausgänge kann man nicht lesen, sondern nur Eingänge.

Mit ...

// Eingänge definieren:
DDRA &= ~E_INT1; // PA4 (IT1) als Eingang definieren
// PORTA |= E_INT1; // evtl. Pullup für PA4
DDRC &= ~(SCL | SDA); // PC0, PC1 als Eingänge definieren
// PORTC |= (SCL | SDA); // evtl. Pullups für PC0, PC1

... definiert man SCL, SDA und INT1 als Eingänge.

Abfragen kann man sie im Programm mit ...

// Abfrage der Pins:
temp1 = PINA & E_INT1;
temp2 = PINC & SCL;
temp3 = PINC & SDA;

Den Wert von tempX kann man nicht mit WriteInteger o.ä. ausgeben, weil das nur ein Ja/Nein-Wert ist. tempX ist also entweder 0 oder <> 0. Damit erkennt der Roboter dann die Linie, der er folgen soll.
Die Abfrage im Programm erfolgt dann mit ...

if (tempX) {tuwas};
oder ...
if (!tempX) {tuwasanderes};

Gruß Dirk

[Fieke]

Vielen dank!!!

[Fieke]

Hallo,

Mit understehend Programma probier ich die Werten von die SCL, SDA und INT1 aus zu lesen.



#include "RP6RobotBaseLib.h"

int main(void)
{
initRobotBase();
powerON();

DDRA &= ~E_INT1;
DDRC &= ~(SCL | SDA);

uint8_t temp1;
uint8_t temp2;
uint8_t temp3;

temp1 = PINA & E_INT1;
temp2 = PINC & SCL;
temp3 = PINC & SDA;

PORTC &= ~SCL;
PORTC &= ~SDA;

while(true)
{
if(temp1)
{
writeString_P("\nINT1 is hoog\n");
}

if(!temp1)
{
writeString_P("\nINT1 is laag\n");
}

if(temp2)
{
writeString_P("\nSCL is hoog\n");
}

if(!temp2)
{
writeString_P("\nSCL is laag\n");
}

if(temp3)
{
writeString_P("\nSDA is hoog\n");
}

if(!temp3)
{
writeString_P("\nSDA is laag\n");
}
}

return 0;
}


Aber die Werte wird nur einmal ausgelesen. Die volgendes Werten die in dem Terminal erscheint, ist immer dasselbte. Bei ein neues Program start, wird wieder die Werten nur einmal ausgelezen.

Was mach ich falsch? Wie kan ich die Werten kontinuierlich auslesen?

Gruss Fieke

[radbruch]

Hallo Fieke

Das ist ein interessantes Projekt. Allerdings ist Linienfolgen nicht wirklich die Spezialdisziplin des RP6. Schnelle Motoränderungen belasten das Getriebe sehr.

Könnte das die Lösung für dein Problem sein?


#include "RP6RobotBaseLib.h"

int main(void)
{
initRobotBase();
powerON();

uint8_t temp_E_INT1;
uint8_t temp_SCL;
uint8_t temp_SDA;

DDRA &= ~E_INT1; // Datenrichtung Eingang
DDRC &= ~(SCL | SDA);

PORTA &= ~E_INT1; // PullUps deaktivieren
PORTC &= ~(SCL | SDA);

while(true)
{
temp_E_INT1 = PINA & E_INT1; // Pins einlesen
temp_SCL = PINC & SCL;
temp_SDA = PINC & SDA;

if(temp_E_INT1)
{
writeString_P("\nINT1 is hoog\n");
}

if(!temp_E_INT1)
{
writeString_P("\nINT1 is laag\n");
}

if(temp_SCL)
{
writeString_P("\nSCL is hoog\n");
}

if(!temp_SCL)
{
writeString_P("\nSCL is laag\n");
}

if(temp_SDA)
{
writeString_P("\nSDA is hoog\n");
}

if(!temp_SDA)
{
writeString_P("\nSDA is laag\n");
}
mSleep(300);
}

return 0;
}


Du darst die Pinabfrage selbstverständlich auch direkt als Bedingung verwenden:

if(PINA & E_INT1)
{
writeString_P("\nINT1 is hoog\n");
}
else
{
writeString_P("\nINT1 is laag\n");
}


Gruß

mic

[Fieke]

Hallo Mic,

Vielen dank für Ihre Hilfe. Ich hat die Werten nur einmal ausgelesen, das auslesen hatte ich nicht in die While loop geprogrammiert.
Jetzt kan ich die Werten gut auslesen. Auf dieser Moment programmier ich die Robot dass ihr die Liene kan folgen.
Es geht noch nicht gut, aber es gibt ein Begin. Ich denke dass ich es weiter kann machen.

Gruss Fieke

[Fieke]

Hallo,

Jetzt kann die Robot die Liene folgen, mit unterstehendes Program:




#include "RP6RobotBaseLib.h" // moet er altijd bij

uint8_t licht(void)
{
uint8_t vlinks;
uint8_t vrechts;
uint8_t lichtlinks;
uint8_t lichtrechts;

task_ADC;

lichtlinks = adcLSL;
lichtrechts = adcLSR;

writeString_P("\nlichtlinks = ");
writeInteger(lichtlinks, DEC);

writeString_P("\nlichtrechts = ");
writeInteger(lichtrechts, DEC);

if(lichtlinks > 230 || lichtrechts > 230)
{
vlinks=0;
vrechts=0;
writeString_P("\nVeel licht\n");
}

else
{
vlinks=10;
}

return vlinks;
}


uint8_t acsStateChanged(void)
{
uint8_t vlinks;
uint8_t vrechts;
writeString_P("\nObstakel veranderd");

if(obstacle_left || obstacle_right)
{
vlinks=0;
vrechts=0;
writeString_P("\n Obstakel HELP!!!");
}

return vlinks;

}



uint8_t lijnvolgenlinks(uint8_t snelheidlinks, uint8_t snelheidrechts)
{

DDRA &= ~E_INT1;

DDRC &= ~(SCL | SDA);

uint8_t templinks;
uint8_t temprechts;
uint8_t tempmidden;

PORTC &= ~SCL;
PORTC &= ~SDA;

uint8_t m101;
uint8_t l100;
uint8_t ll110;
uint8_t r001;
uint8_t rr011;
uint8_t niks000;
uint8_t alles111;

uint8_t delervooruit = 2;
uint8_t delerneteraf = 3;
uint8_t delerverafoptel = 2;
uint8_t delerverafaftrek = 2;


//writeString_P("\nlijnvolgenlinks\n");

if (snelheidlinks > 20 || snelheidrechts > 20)
{
delervooruit = 5;
}

tempmidden = PINA & E_INT1;
templinks = PINC & SCL;
temprechts = PINC & SDA;
uint8_t casus;

if(templinks && !tempmidden && temprechts)
{
casus=101;
}

if(templinks && !tempmidden && !temprechts)
{
casus=100;
}

if(templinks && tempmidden && !temprechts)
{
casus=110;
}

if(!templinks && !tempmidden && temprechts)
{
casus=001;
}

if(!templinks && tempmidden && temprechts)
{
casus=011;
}

if(!templinks && !tempmidden && !temprechts)
{
casus=000;
}

if(templinks && tempmidden && temprechts)
{
casus=111;
}


switch(casus)
{
case 101: if(l100 > 0)
{
snelheidlinks = snelheidrechts - (snelheidrechts/delerneteraf);
}

if(ll110 > 0)
{
snelheidlinks = snelheidrechts - (snelheidrechts/delerverafaftrek);
}

if(r001 > 0)
{
snelheidrechts = snelheidlinks - (snelheidlinks/delerneteraf);
}

if(rr011 > 0)
{
snelheidrechts = snelheidlinks - (snelheidlinks/delerverafaftrek);
}

else
{

snelheidlinks = snelheidlinks +(snelheidlinks/delervooruit);
//writeString_P("\n 101 snelheidlinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts = snelheidlinks;
//writeString_P("\n 101 snelheidrechts");
//writeInteger(snelheidrechts,DEC);
}


m101++; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 100: //writeString_P("\n 100 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts-(snelheidrechts/delerneteraf);
//writeString_P("\n 100 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100++; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 110: snelheidlinks=snelheidlinks+(snelheidlinks/delerverafoptel);
//writeString_P("\n 110 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts-(snelheidrechts/delerverafaftrek);
//writeString_P("\n 110 srechts");
//writeInteger(snelheidrechts,DEC);


m101=0; l100=0; ll110++; r001=0; rr011=0; niks000=0; alles111=0; break;

case 001: snelheidlinks=snelheidlinks-(snelheidlinks/delerneteraf);
//writeString_P("\n 001 slinks");
//writeInteger(snelheidlinks,DEC);

//writeString_P("\n 001 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100=0; ll110=0; r001++; rr011=0; niks000=0; alles111=0; break;

case 011: snelheidlinks=snelheidlinks-(snelheidlinks/delerverafaftrek);
//writeString_P("\n 011 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts+(snelheidrechts/delerverafoptel);
//writeString_P("\n 011 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100=0; ll110=0; r001=0; rr011++; niks000=0; alles111=0; break;

case 000:

m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000++; alles111=0; break;

case 111:
m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111++; break;

default: writeString("\n Iets anders\n"); break;
}

if(snelheidlinks > 50)
{
snelheidlinks = 50;
}

if(snelheidrechts > 50)
{
snelheidrechts = 50;
}

if(snelheidlinks < 10)
{
snelheidlinks=10;

}

if(snelheidrechts <10)
{
snelheidrechts=10;
}

//writeString("\n main snelheidlinks = ");
//writeInteger(snelheidlinks,DEC);

//writeString("\n main snelheidrechts = ");
//writeInteger(snelheidrechts,DEC);

//writeString("\n m101 = ");
//writeInteger(m101,DEC);

//writeString("\n l100 = ");
//writeInteger(l100,DEC);

//writeString("\n ll110 = ");
//writeInteger(ll110,DEC);

//writeString("\n r001 = ");
//writeInteger(r001,DEC);

//writeString("\n rr011 = ");
//writeInteger(rr011,DEC);

//writeString("\n niks000 = ");
//writeInteger(niks000,DEC);

//writeString("\n alles111 = ");
//writeInteger(alles111,DEC);


return snelheidlinks;


}

uint8_t lijnvolgenrechts(uint8_t snelheidlinks, uint8_t snelheidrechts)
{

uint8_t templinks;
uint8_t temprechts;
uint8_t tempmidden;

uint8_t m101;
uint8_t l100;
uint8_t ll110;
uint8_t r001;
uint8_t rr011;
uint8_t niks000;
uint8_t alles111;

uint8_t delervooruit = 2;
uint8_t delerneteraf = 3;
uint8_t delerverafoptel = 2;
uint8_t delerverafaftrek = 2;


//writeString_P("\nlijnvolgenrechts\n");

if (snelheidlinks > 20 || snelheidrechts > 20)
{
delervooruit = 5;
}

tempmidden = PINA & E_INT1;
templinks = PINC & SCL;
temprechts = PINC & SDA;
uint8_t casus;

if(templinks && !tempmidden && temprechts)
{
casus=101;
}

if(templinks && !tempmidden && !temprechts)
{
casus=100;
}

if(templinks && tempmidden && !temprechts)
{
casus=110;
}

if(!templinks && !tempmidden && temprechts)
{
casus=001;
}

if(!templinks && tempmidden && temprechts)
{
casus=011;
}

if(!templinks && !tempmidden && !temprechts)
{
casus=000;
}

if(templinks && tempmidden && temprechts)
{
casus=111;
}


switch(casus)
{
case 101: if(l100 > 0)
{
snelheidlinks = snelheidrechts - (snelheidrechts/delerneteraf);
}

if(ll110 > 0)
{
snelheidlinks = snelheidrechts - (snelheidrechts/delerverafaftrek);
}

if(r001 > 0)
{
snelheidrechts = snelheidlinks - (snelheidlinks/delerneteraf);
}

if(rr011 > 0)
{
snelheidrechts = snelheidlinks - (snelheidlinks/delerverafaftrek);
}

else
{

snelheidlinks = snelheidlinks +(snelheidlinks/delervooruit);
//writeString_P("\n 101 snelheidlinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts = snelheidlinks;
//writeString_P("\n 101 snelheidrechts");
//writeInteger(snelheidrechts,DEC);
}


m101++; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 100: //writeString_P("\n 100 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts-(snelheidrechts/delerneteraf);
//writeString_P("\n 100 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100++; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 110: snelheidlinks=snelheidlinks+(snelheidlinks/delerverafoptel);
//writeString_P("\n 110 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts-(snelheidrechts/delerverafaftrek);
//writeString_P("\n 110 srechts");
//writeInteger(snelheidrechts,DEC);


m101=0; l100=0; ll110++; r001=0; rr011=0; niks000=0; alles111=0; break;

case 001: snelheidlinks=snelheidlinks-(snelheidlinks/delerneteraf);
//writeString_P("\n 001 slinks");
//writeInteger(snelheidlinks,DEC);

//writeString_P("\n 001 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100=0; ll110=0; r001++; rr011=0; niks000=0; alles111=0; break;

case 011: snelheidlinks=snelheidlinks-(snelheidlinks/delerverafaftrek);
//writeString_P("\n 011 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts+(snelheidrechts/delerverafoptel);
//writeString_P("\n 011 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100=0; ll110=0; r001=0; rr011++; niks000=0; alles111=0; break;

case 000:

m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000++; alles111=0; break;

case 111:
m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111++; break;

default: writeString("\n Iets anders\n"); break;
}

if(snelheidlinks > 50)
{
snelheidlinks = 50;
}

if(snelheidrechts > 50)
{
snelheidrechts = 50;
}

if(snelheidlinks < 10)
{
snelheidlinks=10;

}

if(snelheidrechts <10)
{
snelheidrechts=10;
}

//writeString("\n main snelheidlinks = ");
//writeInteger(snelheidlinks,DEC);

//writeString("\n main snelheidrechts = ");
//writeInteger(snelheidrechts,DEC);

//writeString("\n m101 = ");
//writeInteger(m101,DEC);

//writeString("\n l100 = ");
//writeInteger(l100,DEC);

//writeString("\n ll110 = ");
//writeInteger(ll110,DEC);

//writeString("\n r001 = ");
//writeInteger(r001,DEC);

//writeString("\n rr011 = ");
//writeInteger(rr011,DEC);

//writeString("\n niks000 = ");
//writeInteger(niks000,DEC);

//writeString("\n alles111 = ");
//writeInteger(alles111,DEC);



return snelheidrechts;


}

int main(void)
{
initRobotBase(); // altijd eerst dit voor rest programma
//writeString_P("Hoi, hier is robby de robot! Gezellig!\n Alles goed?\n");

//ACS_setStateChangedHandler(acsStateChanged);

DDRA &= ~E_INT1;

DDRC &= ~(SCL | SDA);

PORTC &= ~SCL;
PORTC &= ~SDA;
uint8_t vstart = 10;

powerON();

//setACSPwrMed();

uint8_t slinks = vstart;
uint8_t srechts = vstart;
uint8_t snelheidlinks;
uint16_t xlinks;



while(true)
{
uint16_t tijdelijk;
slinks = lijnvolgenlinks(slinks,srechts);
srechts = lijnvolgenrechts(slinks,srechts);

task_motionControl();
task_ADC();
xlinks = licht();

writeString_P("\nxlinks = ");
writeInteger(xlinks, DEC);

//xlinks == tijdelijk;

//writeString_P("\ntijdelijk = ");
//writeInteger(tijdelijk, DEC);

if(xlinks == 0)
{
slinks = 0;
srechts = 0;
writeString_P("\nin de if als er veel licht is");
}

//task_ACS();
//snelheidlinks = acsStateChanged();
//if (snelheidlinks = 0)
//{
// slinks = 0;
// srechts = 0;
//}
writeString_P("\nslinks =");
writeInteger(slinks,DEC);

writeString_P("\nsrechts =");
writeInteger(srechts,DEC);

moveAtSpeed(slinks,srechts);

//mSleep(1000);
writeString_P("\n\n");

}

return 0;
}


Aber ich will auch dass die Robot stopt wenn der eine Object vor die Robot ist. Ich habe Program womit die Robot Abstand halt.




#include "RP6RobotBaseLib.h" // moet er altijd bij

void acsStateChanged(void)
{
//writeString_P("ACS status is veranderd\n");

if(obstacle_left && obstacle_right)
{
//writeString_P("Iets beide\n");
task_ADC();
task_motionControl();
setMotorDir(FWD, FWD);
moveAtSpeed(0,0);
}

if(obstacle_left && !obstacle_right)
{
//writeString_P("Iets links\n");
task_ADC();
task_motionControl();
setMotorDir(BWD, FWD);
moveAtSpeed(40,40);
}

if(!obstacle_left && obstacle_right)
{
//writeString_P("Iets rechts\n");
task_ADC();
task_motionControl();
setMotorDir(FWD, BWD);
moveAtSpeed(40,40);
}

if(!obstacle_left && !obstacle_right)
{
//writeString_P("Niks\n");
task_ADC();
task_motionControl();
setMotorDir(FWD, FWD);
moveAtSpeed(40,40);
}
}

int main(void)
{
initRobotBase(); // altijd eerst dit voor rest programma
//writeString("Hoi, hier is robby de robot! Gezellig!\n Alles goed?\n");

ACS_setStateChangedHandler(acsStateChanged);
powerON();
setACSPwrMed();


while(true)
{
task_ACS();
task_motionControl();

}

return 0;
}



Aber wie kann ich beide Program kombinieren? Ich habe es geprobiert mit unterstehend Program, aber denn kann ihr die Liene nicht mir volgen. Denn wurde dass Lienefolg Program nur einmal gelesen, also kann die Robot die Liene nicht mehr folgen.



void bumper(void)
{
uint8_t vlinks;
uint8_t vrechts;

writeString_P("\n bumper checken!");


uint8_t bumperl = getBumperLeft();
uint8_t bumperr = getBumperRight();

if (bumperl || bumperr)
{
moveAtSpeed(0,0);

task_motionControl();
task_ADC();

move(40, BWD, DIST_MM(100), true);

rotate(40, RIGHT, 180, true);

writeString_P("\n OEPS, bumper geraakt");
}

}

uint8_t licht(void)
{
uint8_t vlinks;
uint8_t vrechts;
uint8_t lichtlinks;
uint8_t lichtrechts;


task_ADC;

lichtlinks = adcLSL;
lichtrechts = adcLSR;

writeString_P("\nlichtlinks = ");
writeInteger(lichtlinks, DEC);

writeString_P("\nlichtrechts = ");
writeInteger(lichtrechts, DEC);

if(lichtlinks > 300 || lichtrechts > 300)
{
vlinks=0;
vrechts=0;
writeString_P("\nVeel licht\n");
}

else
{
vlinks=10;
}

return vlinks;
}


uint8_t lijnvolgenlinks(uint8_t snelheidlinks, uint8_t snelheidrechts)
{

DDRA &= ~E_INT1;

DDRC &= ~(SCL | SDA);

uint8_t templinks;
uint8_t temprechts;
uint8_t tempmidden;

PORTC &= ~SCL;
PORTC &= ~SDA;

uint8_t m101;
uint8_t l100;
uint8_t ll110;
uint8_t r001;
uint8_t rr011;
uint8_t niks000;
uint8_t alles111;

uint8_t delervooruit = 2;
uint8_t delerneteraf = 3;
uint8_t delerverafoptel = 2;
uint8_t delerverafaftrek = 2;


//writeString_P("\nlijnvolgenlinks\n");

if (snelheidlinks > 20 || snelheidrechts > 20)
{
delervooruit = 5;
}

tempmidden = PINA & E_INT1;
templinks = PINC & SCL;
temprechts = PINC & SDA;
uint8_t casus;

if(templinks && !tempmidden && temprechts)
{
casus=101;
}

if(templinks && !tempmidden && !temprechts)
{
casus=100;
}

if(templinks && tempmidden && !temprechts)
{
casus=110;
}

if(!templinks && !tempmidden && temprechts)
{
casus=001;
}

if(!templinks && tempmidden && temprechts)
{
casus=011;
}

if(!templinks && !tempmidden && !temprechts)
{
casus=000;
}

if(templinks && tempmidden && temprechts)
{
casus=111;
}


switch(casus)
{
case 101: if(l100 > 0)
{
snelheidlinks = snelheidrechts - (snelheidrechts/delerneteraf);
}

if(ll110 > 0)
{
snelheidlinks = snelheidrechts - (snelheidrechts/delerverafaftrek);
}

if(r001 > 0)
{
snelheidrechts = snelheidlinks - (snelheidlinks/delerneteraf);
}

if(rr011 > 0)
{
snelheidrechts = snelheidlinks - (snelheidlinks/delerverafaftrek);
}

else
{

snelheidlinks = snelheidlinks +(snelheidlinks/delervooruit);
//writeString_P("\n 101 snelheidlinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts = snelheidlinks;
//writeString_P("\n 101 snelheidrechts");
//writeInteger(snelheidrechts,DEC);
}


m101++; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 100: //writeString_P("\n 100 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts-(snelheidrechts/delerneteraf);
//writeString_P("\n 100 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100++; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 110: snelheidlinks=snelheidlinks+(snelheidlinks/delerverafoptel);
//writeString_P("\n 110 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts-(snelheidrechts/delerverafaftrek);
//writeString_P("\n 110 srechts");
//writeInteger(snelheidrechts,DEC);


m101=0; l100=0; ll110++; r001=0; rr011=0; niks000=0; alles111=0; break;

case 001: snelheidlinks=snelheidlinks-(snelheidlinks/delerneteraf);
//writeString_P("\n 001 slinks");
//writeInteger(snelheidlinks,DEC);

//writeString_P("\n 001 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100=0; ll110=0; r001++; rr011=0; niks000=0; alles111=0; break;

case 011: snelheidlinks=snelheidlinks-(snelheidlinks/delerverafaftrek);
//writeString_P("\n 011 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts+(snelheidrechts/delerverafoptel);
//writeString_P("\n 011 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100=0; ll110=0; r001=0; rr011++; niks000=0; alles111=0; break;

case 000:

m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000++; alles111=0; break;

case 111:
m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111++; break;

default: writeString("\n Iets anders\n"); break;
}

if(snelheidlinks > 50)
{
snelheidlinks = 50;
}

if(snelheidrechts > 50)
{
snelheidrechts = 50;
}

if(snelheidlinks < 10)
{
snelheidlinks=10;

}

if(snelheidrechts <10)
{
snelheidrechts=10;
}

//writeString("\n main snelheidlinks = ");
//writeInteger(snelheidlinks,DEC);

//writeString("\n main snelheidrechts = ");
//writeInteger(snelheidrechts,DEC);

//writeString("\n m101 = ");
//writeInteger(m101,DEC);

//writeString("\n l100 = ");
//writeInteger(l100,DEC);

//writeString("\n ll110 = ");
//writeInteger(ll110,DEC);

//writeString("\n r001 = ");
//writeInteger(r001,DEC);

//writeString("\n rr011 = ");
//writeInteger(rr011,DEC);

//writeString("\n niks000 = ");
//writeInteger(niks000,DEC);

//writeString("\n alles111 = ");
//writeInteger(alles111,DEC);


return snelheidlinks;


}

uint8_t lijnvolgenrechts(uint8_t snelheidlinks, uint8_t snelheidrechts)
{

uint8_t templinks;
uint8_t temprechts;
uint8_t tempmidden;

uint8_t m101;
uint8_t l100;
uint8_t ll110;
uint8_t r001;
uint8_t rr011;
uint8_t niks000;
uint8_t alles111;

uint8_t delervooruit = 2;
uint8_t delerneteraf = 3;
uint8_t delerverafoptel = 2;
uint8_t delerverafaftrek = 2;


//writeString_P("\nlijnvolgenrechts\n");

if (snelheidlinks > 20 || snelheidrechts > 20)
{
delervooruit = 5;
}

tempmidden = PINA & E_INT1;
templinks = PINC & SCL;
temprechts = PINC & SDA;
uint8_t casus;

if(templinks && !tempmidden && temprechts)
{
casus=101;
}

if(templinks && !tempmidden && !temprechts)
{
casus=100;
}

if(templinks && tempmidden && !temprechts)
{
casus=110;
}

if(!templinks && !tempmidden && temprechts)
{
casus=001;
}

if(!templinks && tempmidden && temprechts)
{
casus=011;
}

if(!templinks && !tempmidden && !temprechts)
{
casus=000;
}

if(templinks && tempmidden && temprechts)
{
casus=111;
}


switch(casus)
{
case 101: if(l100 > 0)
{
snelheidlinks = snelheidrechts - (snelheidrechts/delerneteraf);
}

if(ll110 > 0)
{
snelheidlinks = snelheidrechts - (snelheidrechts/delerverafaftrek);
}

if(r001 > 0)
{
snelheidrechts = snelheidlinks - (snelheidlinks/delerneteraf);
}

if(rr011 > 0)
{
snelheidrechts = snelheidlinks - (snelheidlinks/delerverafaftrek);
}

else
{

snelheidlinks = snelheidlinks +(snelheidlinks/delervooruit);
//writeString_P("\n 101 snelheidlinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts = snelheidlinks;
//writeString_P("\n 101 snelheidrechts");
//writeInteger(snelheidrechts,DEC);
}


m101++; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 100: //writeString_P("\n 100 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts-(snelheidrechts/delerneteraf);
//writeString_P("\n 100 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100++; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 110: snelheidlinks=snelheidlinks+(snelheidlinks/delerverafoptel);
//writeString_P("\n 110 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts-(snelheidrechts/delerverafaftrek);
//writeString_P("\n 110 srechts");
//writeInteger(snelheidrechts,DEC);


m101=0; l100=0; ll110++; r001=0; rr011=0; niks000=0; alles111=0; break;

case 001: snelheidlinks=snelheidlinks-(snelheidlinks/delerneteraf);
//writeString_P("\n 001 slinks");
//writeInteger(snelheidlinks,DEC);

//writeString_P("\n 001 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100=0; ll110=0; r001++; rr011=0; niks000=0; alles111=0; break;

case 011: snelheidlinks=snelheidlinks-(snelheidlinks/delerverafaftrek);
//writeString_P("\n 011 slinks");
//writeInteger(snelheidlinks,DEC);

snelheidrechts=snelheidrechts+(snelheidrechts/delerverafoptel);
//writeString_P("\n 011 srechts");
//writeInteger(snelheidrechts,DEC);

m101=0; l100=0; ll110=0; r001=0; rr011++; niks000=0; alles111=0; break;

case 000:

m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000++; alles111=0; break;

case 111:
m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111++; break;

default: writeString("\n Iets anders\n"); break;
}

if(snelheidlinks > 50)
{
snelheidlinks = 50;
}

if(snelheidrechts > 50)
{
snelheidrechts = 50;
}

if(snelheidlinks < 10)
{
snelheidlinks=10;

}

if(snelheidrechts <10)
{
snelheidrechts=10;
}

//writeString("\n main snelheidlinks = ");
//writeInteger(snelheidlinks,DEC);

//writeString("\n main snelheidrechts = ");
//writeInteger(snelheidrechts,DEC);

//writeString("\n m101 = ");
//writeInteger(m101,DEC);

//writeString("\n l100 = ");
//writeInteger(l100,DEC);

//writeString("\n ll110 = ");
//writeInteger(ll110,DEC);

//writeString("\n r001 = ");
//writeInteger(r001,DEC);

//writeString("\n rr011 = ");
//writeInteger(rr011,DEC);

//writeString("\n niks000 = ");
//writeInteger(niks000,DEC);

//writeString("\n alles111 = ");
//writeInteger(alles111,DEC);



return snelheidrechts;


}




void acsStateChanged(void)
{
//writeString_P("ACS status is veranderd\n");
uint8_t x;
uint8_t y;

if(obstacle_left || obstacle_right)
{

task_ADC();
task_motionControl();
setMotorDir(FWD, FWD);
moveAtSpeed(0,0);
x++;
y=0;

}

else
{

DDRA &= ~E_INT1;

DDRC &= ~(SCL | SDA);

PORTC &= ~SCL;
PORTC &= ~SDA;
uint8_t vstart = 10;
uint8_t slinks = vstart;
uint8_t srechts = vstart;
uint8_t snelheidlinks;
uint16_t xlinks;
uint8_t zlinks;

slinks = lijnvolgenlinks(slinks,srechts);
srechts = lijnvolgenrechts(slinks,srechts);

task_motionControl();
task_ADC();
xlinks = licht();

//writeString_P("\nxlinks = ");
//writeInteger(xlinks, DEC);


if(xlinks == 0)
{
slinks = 0;
srechts = 0;
//writeString_P("\nin de if als er veel licht is");
}


bumper();

//writeString_P("\nslinks =");
//writeInteger(slinks,DEC);

//writeString_P("\nsrechts =");
//writeInteger(srechts,DEC);
setMotorDir(FWD, FWD);
moveAtSpeed(slinks,srechts);


//writeString_P("\n\n");
}

y++;
x=0;
}

int main(void)
{
initRobotBase(); // altijd eerst dit voor rest programma
//writeString("Hoi, hier is robby de robot! Gezellig!\n Alles goed?\n");

ACS_setStateChangedHandler(acsStateChanged);
powerON();
setACSPwrMed();


while(true)
{
task_ACS();
task_motionControl();
acsStateChanged();
}

return 0;
}


Wie kan ich die Robot die Liene folgen und abstand halten lassen?

Gruss Fieke

[Dirk]

Hallo Fieke,

erst mal must du festlegen, was passieren soll, wenn der RP6 auf ein Hindernis stößt. Da er ja wohl auf der Linie bleiben soll (sonst würde ja das Linienfolgen keinen Sinn machen), könnte er bei einem Hindenis nur anhalten und warten, bis es wieder weg ist ODER er könnte auf der Linie wenden und in die andere Richtung fahren.

Dein Hauptprogramm bleibt dazu dein Linienfolgeprogramm. In den Main-Teil (vor der while(true)-Schleife) kopierst du nur:
ACS_setStateChangedHandler(acsStateChanged);
powerON();
setACSPwrMed();

... und schreibst eine neue Routine void acsStateChanged(void):
Für die ersten Tests kannst du z.B. anhalten (moveAtSpeed(0,0) ), wenn ein Hindernis erkannt wird und weiterfahren (moveAtSpeed(slinks,srechts) ), wenn kein Hindernis mehr da ist.

Der Befehl moveAtSpeed(slinks,srechts), der jetzt in der While(true)-Schleife am Ende steht, müßte dann raus. ODER: Du setzt in der Routine void acsStateChanged(void) nur ein Flag (Stoppen oder nicht), das du dann mit einer If-Konstruktion in der Hauptschleife abfragst.

Gruß Dirk

[Fieke]

Vielen dank vor alle Hilfe!
Ich habe noch etwas anderes zu fragen. Wie kann ich Werten von die acsStateChanged(); zurückgeben an die while(true) loop in die int main()?
Denn in die Bibliothek die acsStateChanged function via die ACS_setStateChangedHandler(); angerufen werd.
Gruss Fieke

[Dirk]

Hallo Fieke,

acsStateChanged() wird immer aufgerufen, wenn sich am Zustand der beiden Variablen obstacle_left und obstacle_right etwas ändert.

Dann kann man da entscheiden, ob man etwas machen oder wie man darauf reagieren möchte.

Man braucht aber nicht unbedingt in der acsStateChanged() etwas zu tun.

Es ginge auch, die beiden obstacle_left und obstacle_right irgendwo im Programm abzufragen. Wenn beide = 0 sind, dann ist kein Hindernis erkennbar.
Nachteil: Man testet die beiden eventuell umsonst, weil sie sich gar nicht geändert haben. Dazu dient acsStateChanged().

So etwa klar?

Gruß Dirk

[Fieke]

Hallo Dirk,

Danke vor ihre Hilfe!

Es wirkt beinahe. Wann ich die obstacle_left und obstacle_right benutzen mit eine anderes programm (bumper(); zum Beispiel, sehe unterstehend Program), liese die Robot die obstacle_left und obstacle_right nicht mehr aus.




#include "RP6RobotBaseLib.h"

void bumper(void)
{
uint8_t bumperl = getBumperLeft();
uint8_t bumperr = getBumperRight();


if (bumperl || bumperr)
{
moveAtSpeed(0,0);

move(40, BWD, DIST_MM(100), true);

rotate(40, RIGHT, 180, true);

writeString_P("\n OEPS, bumper geraakt");
}

else
{
writeString_P("\n geen bumper");
setMotorDir(FWD, FWD);
moveAtSpeed(30,30);
}
}

void acsStateChanged(void)
{
writeString_P("\nACS status is veranderd\n");

}


int main(void)
{
initRobotBase();
ACS_setStateChangedHandler(acsStateChanged);
powerON();
setACSPwrMed();
uint8_t y=0;

while(true)
{
writeString_P("\ny = ");
writeInteger(y, DEC);

writeString_P("\n bumper checken!");

task_ACS();
writeString_P("\n na task_ACS ");

task_motionControl();
writeString_P("\n na motionControl");

task_ADC();
writeString_P("\n Na ADC checken");

task_RP6System();

bumper();

writeString_P("\n obstacle left = ");
writeInteger(obstacle_left, DEC);

writeString_P("\n obstacle right = ");
writeInteger(obstacle_right, DEC);

writeString_P("\n\n");
mSleep(1000);
y++;
}


return 0;
}


Aber wenn die Bumper eingedrükt ist und die move und rotate function ausgefürht werden, liese die Robot die Werten von obstacle_left und obstacle_right wohl aus.

Warum liese es die Werten nicht konstant aus?

Gruss Fieke

[Dirk]

Hallo Fieke,

es gibt in deiner Hauptschleife zu viele Dinge, die bewirken, dass die Tasks zu langsam aufgerufen werden. Auf jeden Fall darf in der Hauptschleife keine Pause sein (mSleep(1000) ) und die Textausgaben würde ich auch in ein Unterprogramm verlagern und z.B. über eine Stopwatch nur 1x / Sekunde ausgeben oder nur, wenn z.B. obstacle_... TRUE ist.
In der bumper-Funktion sind move und rotate auch blockierend. Auch das verzögert den Ablauf der Hauptschleife.

Die Kunst ist letztlich, alle deine Aufgaben auch als Tasks zu schreiben und in die Main-Schleife einzubinden. Jede Task sollte möglichst schnell wieder verlassen werden und die Kontrolle an die Hauptschleife abgeben. In der Main-Schleife selbst sollte es keine Verzögerungen oder Textausgaben geben.

Main-Loop:

while(true)
{
task_MyTask_1();
task_MyTask_2();
task_RP6System();
}
return 0;
}

Gruß Dirk

[Fieke]

Hallo Dirk,

Vielen dank vor alle Hilfe! Die Program wirkt!
Es ist vor ein Projekt auf meine Schule. Dank deine Hilfe kann ich das Projekt gut beenden.

Angenehmen Festtagen!

Gruss Fieke

[Fieke]

Hallo,

Das Projekt ist fertig. Die Robot kann die Liene folgen und stoppen bei ein Obstacle. Er braucht die Obstacle sensoren und die gemachte Lienefolg sensor. Ich habe die Lienefolgsensor in zwei teilen erbaut.

Unterstehend das Programm vor das Projekt.


#include "RP6RobotBaseLib.h" // moet er altijd bij

uint8_t afstandlezen(uint8_t slinks, uint8_t srechts)
{
task_ACS();
task_RP6System();

if(obstacle_left || obstacle_right)
{
slinks = 0;
}

return slinks;

}

uint8_t licht(uint8_t slinks, uint8_t srechts)
{
task_ACS();
uint8_t lichtlinks;
uint8_t lichtrechts;
uint8_t lichtgrens = 990;

task_ADC;

lichtlinks = adcLSL;
lichtrechts = adcLSR;

if(lichtlinks > lichtgrens || lichtrechts > lichtgrens)
{
slinks=0;
srechts=0;
}

return slinks;
}


uint8_t lijnvolgenlinks(uint8_t snelheidlinks, uint8_t snelheidrechts)
{
task_ACS();
DDRA &= ~E_INT1;

DDRC &= ~(SCL | SDA);

uint8_t templinks;
uint8_t temprechts;
uint8_t tempmidden;

PORTC &= ~SCL;
PORTC &= ~SDA;

uint8_t m101;
uint8_t l100;
uint8_t ll110;
uint8_t r001;
uint8_t rr011;
uint8_t niks000;
uint8_t alles111;

uint8_t delervooruit = 2;
uint8_t delerneteraf = 2;
uint8_t delerverafoptel = 2;
uint8_t delerverafaftrek = 2;


if (snelheidlinks > 20 || snelheidrechts > 20)
{
delervooruit = 5;
}

tempmidden = PINA & E_INT1;
templinks = PINC & SCL;
temprechts = PINC & SDA;
uint8_t casus;

if(templinks && !tempmidden && temprechts)
{
casus=101;
}

if(templinks && !tempmidden && !temprechts)
{
casus=100;
}

if(templinks && tempmidden && !temprechts)
{
casus=110;
}

if(!templinks && !tempmidden && temprechts)
{
casus=001;
}

if(!templinks && tempmidden && temprechts)
{
casus=011;
}

if(!templinks && !tempmidden && !temprechts)
{
casus=000;
}

if(templinks && tempmidden && temprechts)
{
casus=111;
}


switch(casus)
{
case 101: if(l100 > 0)
{
snelheidlinks = snelheidrechts - (snelheidrechts/delerneteraf);
}

if(ll110 > 0)
{
snelheidlinks = snelheidrechts - (snelheidrechts/delerverafaftrek);
}

else
{

snelheidlinks = snelheidlinks +(snelheidlinks/delervooruit);
}
setMotorDir(FWD,FWD);
m101++; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 100: snelheidlinks = snelheidlinks - (snelheidlinks / 4);
if (snelheidlinks < 30)
{
snelheidlinks = 30;
}

setMotorDir(FWD,FWD);
m101=0; l100++; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 110: snelheidlinks=snelheidlinks+(snelheidlinks/delerverafoptel);

setMotorDir(FWD,BWD);
m101=0; l100=0; ll110++; r001=0; rr011=0; niks000=0; alles111=0; break;

case 001: snelheidlinks=snelheidlinks-(snelheidlinks/delerneteraf);

setMotorDir(FWD,FWD);
m101=0; l100=0; ll110=0; r001++; rr011=0; niks000=0; alles111=0; break;

case 011: snelheidlinks=snelheidlinks-(snelheidlinks/delerverafaftrek);

setMotorDir(BWD,FWD);
m101=0; l100=0; ll110=0; r001=0; rr011++; niks000=0; alles111=0; break;

case 000:

m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000++; alles111=0; break;

case 111:
m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111++; break;

}
task_ACS();
if(snelheidlinks > 50)
{
snelheidlinks = 50;
}
task_RP6System();


if(snelheidlinks < 10)
{
snelheidlinks=10;
}

task_ACS();
return snelheidlinks;
}

uint8_t lijnvolgenrechts(uint8_t snelheidlinks, uint8_t snelheidrechts)
{
task_ACS();

uint8_t templinks;
uint8_t temprechts;
uint8_t tempmidden;

uint8_t m101;
uint8_t l100;
uint8_t ll110;
uint8_t r001;
uint8_t rr011;
uint8_t niks000;
uint8_t alles111;

uint8_t delervooruit = 2;
uint8_t delerneteraf = 2;
uint8_t delerverafoptel = 2;
uint8_t delerverafaftrek = 2;
task_RP6System();

if (snelheidlinks > 20 || snelheidrechts > 20)
{
delervooruit = 5;
}

tempmidden = PINA & E_INT1;
templinks = PINC & SCL;
temprechts = PINC & SDA;
uint8_t casus;
task_RP6System();
if(templinks && !tempmidden && temprechts)
{
casus=101;
}

if(templinks && !tempmidden && !temprechts)
{
casus=100;
}
task_RP6System();
if(templinks && tempmidden && !temprechts)
{
casus=110;
}

if(!templinks && !tempmidden && temprechts)
{
casus=001;
}

if(!templinks && tempmidden && temprechts)
{
casus=011;
}

if(!templinks && !tempmidden && !temprechts)
{
casus=000;
}

if(templinks && tempmidden && temprechts)
{
casus=111;
}

task_RP6System();
switch(casus)
{
case 101:
if(r001 > 0)
{
snelheidrechts = snelheidlinks - (snelheidlinks/delerneteraf);
}

if(rr011 > 0)
{
snelheidrechts = snelheidlinks - (snelheidlinks/delerverafaftrek);
}

else
{

snelheidrechts = snelheidlinks;
}
setMotorDir(FWD,FWD);

m101++; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 100: snelheidrechts=snelheidrechts-(snelheidrechts/delerneteraf);

setMotorDir(FWD,FWD);
m101=0; l100++; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;

case 110:
snelheidrechts=snelheidrechts-(snelheidrechts/delerverafaftrek);
setMotorDir(FWD,BWD);
m101=0; l100=0; ll110++; r001=0; rr011=0; niks000=0; alles111=0; break;

case 001:
snelheidrechts=snelheidrechts-(snelheidrechts/4);
if(snelheidrechts < 30)
{
snelheidrechts = 30;
}
setMotorDir(FWD,FWD);
m101=0; l100=0; ll110=0; r001++; rr011=0; niks000=0; alles111=0; break;

case 011:
snelheidrechts=snelheidrechts+(snelheidrechts/delerverafoptel);
setMotorDir(BWD,FWD);
m101=0; l100=0; ll110=0; r001=0; rr011++; niks000=0; alles111=0; break;

case 000: m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000++; alles111=0; break;

case 111: m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111++; break;
}
task_ACS();


if(snelheidrechts > 50)
{
snelheidrechts = 50;
}


if(snelheidrechts <10)
{
snelheidrechts=10;
}
task_ACS();
return snelheidrechts;
}

int main(void)
{
initRobotBase();

uint8_t vstart = 10;

powerON();

DDRA &= ~E_INT1;
DDRC &= ~(SCL | SDA);
PORTC &= ~SCL;
PORTC &= ~SDA;

setACSPwrMed();

uint8_t slinks = vstart;
uint8_t srechts = vstart;
uint8_t snelheidlinks;
uint16_t xlinks;
uint8_t zlinks;
uint8_t afstand;
uint8_t x = 0;


while(true)
{
task_RP6System();
slinks = lijnvolgenlinks(slinks,srechts);
srechts = lijnvolgenrechts(slinks,srechts);

task_RP6System();
//slinks = licht(slinks, srechts);
task_RP6System();

slinks = afstandlezen(slinks, srechts);
task_RP6System();

if(slinks == 0)
{
srechts = 0;
}

task_RP6System();

moveAtSpeed(slinks,srechts);

task_RP6System();
}

return 0;
}



Ich habe auch ein Film on youtube gemacht: http://www.youtube.com/watch?v=H-tXpLv0l_M

Vielen dank vor alle Hilfe!

Gruss Fieke

[Dirk]

Hallo Fieke,

das Projekt ist super geworden! \:D/


Gruß Dirk