/*******************************************************************************
Projektname: ACS.cprj
Benötigte Libs´s: IntFunc_lib.cc
Routinen: lcd.cc, PRO-BOT128C_Lib_V2.cc
Autor: Niels
Datum: 16. 04. 2013
Funktion: Hindernisse erkennen und ausweichen
*******************************************************************************/
// Compiler-Direktiven
#define MIDDLESPEED_LEFT 200
#define MIDDLESPEED_RIGHT 180
#define LOWSPEED 0
void main(void)
{
PRO_BOT128_INIT(); //PRO-BOT128 Setup
ACS_INIT(1); // Geringste Empfindlichkeit = 1; höchste = 20
AbsDelay(100); //Wait 0,1 Sec.
LCD_Init();
LCD_ClearLCD();
Akkuspg(); // Ausgabe der Akkuspannung
DRIVE_ON(); //Motorspannung an
MOTOR_DIR(1,1); //beide Motoren vorwärts - MOTOR DIRECTION
MOTOR_POWER(MIDDLESPEED_LEFT, MIDDLESPEED_RIGHT);
// Endlos-Schleife
do
{
if ((ACS_LEFT() == 0) && (ACS_RIGHT() == 1))
{
// Hindernis links wurde erkannt
// Nach RECHTS ausweichen
// rechtes Rad rückwärts
MOTOR_DIR(0,0); //beide Motoren rückwärts
MOTOR_POWER(LOWSPEED,MIDDLESPEED_RIGHT);
AbsDelay(100); //Wait 0,1 Sec.
DRIVE_ON(); //Motor an!
}
if ((ACS_LEFT() == 1) && (ACS_RIGHT() == 0))
{
// Hindernis rechts wurde erkannt
// Nach LINKS ausweichen
// rechtes Rad rückwärts
MOTOR_DIR(0,0); //beide Motoren rückwärts
MOTOR_POWER(MIDDLESPEED_LEFT, LOWSPEED);
AbsDelay(100); //Wait 0,1 Sec.
DRIVE_ON(); //Motor an!
}
if ((ACS_LEFT() == 1) && (ACS_RIGHT() == 1))
{
// Kein Hindernis
// Geradeausfahrt
MOTOR_DIR(1,1); //beide Motoren vorwärts
MOTOR_POWER(MIDDLESPEED_LEFT, MIDDLESPEED_RIGHT);
AbsDelay(100); //Wait 0,1 Sec.
DRIVE_ON(); //Motor an!
}
if ((ACS_LEFT() == 0) && (ACS_RIGHT() == 0))
{
// Hindernis links und rechts wurde erkannt
// STOPP
DRIVE_OFF();
LCD_ClearLCD();
//LCD auf Zeile 1 Position 1 setzen:
LCD_Locate(1, 1);
LCD_WriteText("Und wohin jetzt?"); //Begruessung
AbsDelay(1000);
LCD_ClearLCD();
}
}
while(1);
}
// Wir lagern Programmcode in eine (Unter-)Funktion aus
int Akkuspg(void)
{
LCD_ClearLCD();
//LCD auf Zeile 1 Position 1 setzen:
LCD_Locate(1, 1);
LCD_WriteText("Akkuspannung:"); //Begruessung
LCD_Locate(2, 1);
float Akkuspannung;
Akkuspannung=AKKU_SPG();
LCD_WriteFloat(Akkuspannung, 6);
AbsDelay(2000); //Wait 2 Sec.
return 0;
}