Hallo,
Jetzt kann die Robot die Liene folgen, mit unterstehendes Program:
Code:
#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.
Code:
#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.
Code:
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
Lesezeichen