Code:
#include "RP6RobotBaseLib.h"
int main(void)
{
initRobotBase();
powerON();
setLEDs(0b001001);
int a = 0;
int b = 0;
int c = 0;
int d = 0;
int e = 0;
int f = 0;
setACSPwrMed();
moveAtSpeed(160,160);
while(true)
{
if(getBumperLeft())
{
move(85, BWD, DIST_MM(70), true);
rotate(70, RIGHT, 50, true);
changeDirection(FWD);
moveAtSpeed(160,160);
b++;
}
if(getBumperRight())
{
move(85, BWD, DIST_MM(70), true);
rotate(70, LEFT, 50, true);
changeDirection(FWD);
moveAtSpeed(160,160);
c++;
}
if(obstacle_left)
{
setLEDs(0b000001);
move(85, BWD, DIST_MM(70), true);
rotate(70, RIGHT, 60, true);
changeDirection(FWD);
setLEDs(0b001001);
moveAtSpeed(160,160);
d++;
}
if(obstacle_right)
{
setLEDs(0b001000);
move(85, BWD, DIST_MM(70), true);
rotate(70, LEFT, 60, true);
changeDirection(FWD);
setLEDs(0b001001);
moveAtSpeed(160,160);
e++;
}
if(obstacle_right && obstacle_left)
{
setLEDs(0b001001);
moveAtSpeed(0,0);
setMotorDir(BWD,BWD);
setStopwatch1(0);
startStopwatch1();
moveAtSpeed(70,70);
if(getStopwatch1() > 50000)
{
moveAtSpeed(0,0);
setMotorDir(FWD,FWD);
setStopwatch1(0);
}
moveAtSpeed(160,160);
setLEDs(0b001001);
f++;
}
if(adcBat<600)
{
for(a = 0;a>1000;a++)
{
setLEDs(0b010101);
mSleep(150);
setLEDs(0b101010);
}
}
if(getBumperLeft() && getBumperRight())
{
writeString_P("a");
writeInteger(a, DEC);
writeString_P("b");
writeInteger(b, DEC);
writeString_P("c");
writeInteger(c, DEC);
writeString_P("d");
writeInteger(d, DEC);
writeString_P("e");
writeInteger(e, DEC);
writeString_P("f");
writeInteger(f, DEC);
a = 0;
b = 0;
c = 0;
d = 0;
e = 0;
f = 0;
}
task_RP6System();
task_motionControl();
}
return 0;
}
Lesezeichen