Code:
/*
RP6 fährt autonom. Erster Versuch. ACS und Bumper werden nicht per Event Handler angesprochen,
sondern direkt in der main loop(). Die Eventhandler haben das eigentliche moven stets unterbrochen.
Dadurch fuhr der RP6 meist im Kreis. Klappt so jetzt besser...
10.01.2009
*/
#include "RP6RobotBaseLib.h" // Always needs to be included!
// prüft den Akku und löst ein Blinklicht aus, wenn die Spannung < ca. 5,5 V ist
// und stopp vorübergehend die Fahrt
void chkAccu (void) {
static uint8_t x=1; // AccuChk-Zähler (static bedeutet, das diese Var. den Wert auch ausserhalb der Funktion behält)
uint16_t ubat = readADC(ADC_BAT);
if (ubat < 550) {
if (x >= 4) {
writeString_P("!!! Akkuspannung gering = ");
writeInteger(ubat, DEC);
writeString_P("\n");
uint8_t i=1;
uint8_t runningLight = 1;
while (i < 50) {
setLEDs(runningLight);
runningLight <<= 1; // bit shift nach links 0b000001, 0b000010, etc.
if(runningLight > 32)
runningLight = 1; // starte wieder bei StatusLED1
i++;
mSleep(100); // delay 100ms = 0.1s
}
setLEDs(0b000000); // alle LEDs aus
x=1;
}
x++;
}
}
// prüft den Motorstrom; der Motorstrom ist hoch, wenn der Antrieb z.b. aufgrund eines Hindernisses
// blockiert wird
void chkMotor(void) {
static uint8_t i=1; // MotorChk-Zähler
if (adcMotorCurrentLeft > 100 || adcMotorCurrentRight > 100) {
writeString_P("\n");
writeString_P("!!! Motorstrom ist zu hoch links: ");
writeInteger(adcMotorCurrentLeft, DEC);
writeString_P(" rechts: ");
writeInteger(adcMotorCurrentRight, DEC);
writeString_P("\n");
if (i >= 3) {
setLEDs(0b110110); // alle roten LEDs an
move(50,BWD,DIST_CM(30),true);
rotate(50,LEFT,180,true);
// sind die Motoren immer noch geblockt ?? wenn ja, versuchen wir es mal 90 Grad links herrum...
if (adcMotorCurrentLeft > 100 || adcMotorCurrentRight > 100) {
rotate(50,LEFT,90,true);
}
setLEDs(0b000000); // alle LEDs aus
i=0;
}
i++;
}
}
// Hindernissen ausweichen
void avoid(void) {
// ACS-Abfrage
if(obstacle_right && obstacle_left) {// left AND right sensor have detected something...
writeString_P("ACS -> middle\n");
setLEDs(0b100100);
move(50,BWD,DIST_CM(30),true);
rotate(30,LEFT,30,true);
setLEDs(0b000000);
} else if (obstacle_left) { // Left "sensor" has detected something
writeString_P("ACS -> left\n");
setLEDs(0b010000);
rotate(30,RIGHT,30,true);
setLEDs(0b000000);
} else if (obstacle_right) {// Right "sensor" has detected something
writeString_P("ACS -> right\n");
setLEDs(0b000100);
rotate(30,LEFT,30,true);
setLEDs(0b000000);
}
// Bumper-Abfrage
if(bumper_left || bumper_right) {
writeString_P("Bumper -> left, right or both\n");
setLEDs(0b010010);
move(50,BWD,DIST_CM(30),true);
rotate(30,LEFT,90,true);
setLEDs(0b000000);
}
}
int main(void)
{
initRobotBase(); // immer als erstes aufrufen, ohne die Funktion geht nix
powerON(); // aktiviert alle wichtigen Systeme
setLEDs(0b111111); // alle LEDs einmal an
mSleep(1000); // 1 sek. warten
setLEDs(0b000000); // alle LEDs wieder aus
// kann man das noch niedriger einstellen? low ist noch zu hoch, der RP6 fährt nicht nah
// genug an Wände heran.
setACSPwrLow(); // ACS Power = low
chkAccu(); // beim ersten Programmstart einmal ausführen, danach wird die Funktion zeitlich getriggert
// Timer starten
startStopwatch1();
startStopwatch2();
// Main loop
while(true)
{
task_RP6System(); // ruft motioncontroll, acs, etc. auf
// alle 30sek. Akku-Spannung prüfen
if(getStopwatch1() > 30000) {
chkAccu();
setStopwatch1(0);
}
// alle 500ms Motor-Spannung prüfen
if(getStopwatch2() > 500) {
chkMotor();
setStopwatch2(0);
}
// fahr mal los... schön gerade aus und mit 80 Speed pro Kette....
changeDirection(FWD);
moveAtSpeed(80,80);
// Hindernissen ausweichen per IR und Bumper...
avoid();
}
return 0;
}
[/code]
Lesezeichen