Code:
/***************************************************************************
* Projekt: LICHTSUCHER ROBOTER *
* *
* von Michael Eisele und David Hüls *
* *
***************************************************************************/
/* ############################################################################
* #+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+
*
* VORSICHT: Der Roboter bewegt sich. Lassen sie genung Platz um ihn herrum.
* 2m x 2m sollten genügen Raum sein für ihn um sich zu bewegen
*
* >>> Vergessen sich nicht das Flachbandkabel zu entfernen bevor sie den
* Roboter starten!!!!
*
* #+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+
* ############################################################################
* ****************************************************************************
*/
// Eingebundene Bibiotheken (Liberys, Includes):
#include "RP6M256Lib.h" //Libery für WIFI Module
#include "RP6I2CmasterTWI.h" //Libery I2C Master (WIFI Modul)
/*****************************************************************************/
/*****************************************************************************/
// Bindet die "RP6 Control M256 I2C Master Bibiothek" ein.
// Dieses ermöglicht den Roboter zu über das Erweiterungsmodul zu steuern fast
// als würde er direkt aus der RP6RobotBaseLib.h gesteuert werden
// direkt vom RP6 aus.
#include "RP6M256_I2CMasterLib.h"
/*****************************************************************************/
/*****************************************************************************/
// Behaviour command type: (Funktions Kommando Typen)
#define IDLE 0
// The behaviour command data type: (Prototypenaufruf aus Lib)
typedef struct {
uint8_t speed_left; // Geschwindigkeit linke Kette, wird genutzt zum rotieren
// und für die verschiedenen Fahrbefehle und Berchnungen
// Wenn dieser Befehl aktiv ist wird die Rechte kette blockiert
uint8_t speed_right; // Geschwindigkeit rechte Kette
unsigned dir:2; // Richtung Vorwärts (FWD), Rückwärts (BWD), Links (Left), Rechts (Right
unsigned move:1; // Datyntyp fetlegen für move
unsigned rotate:1; // Datemtyp festlegen für Rotate
uint16_t move_value; // Variable zur Berechnung der Distanz und Winkelwerte (Fahren und drehen)
uint8_t state; // Aktueller Zustand
} behaviour_command_t; // Zusammengesetzte Funktion in der die davor genannten Werte eingegeben werden können!
behaviour_command_t STOP = {0, 0, FWD, false, false, 0, IDLE};// Keiner der fünf Fälle tritt ein
// Suchen, Ziel gefunden, Hinderniss gerammt, Hinderniss erkannt oder Fahren
//Sollte normal NIE passieren!!!!
/*****************************************************************************/
// Cruise Behaviour:
#define CRUISE_SPEED_FWD 80 // Voreinstellung Geschwindigkeit
#define CRUISE_ROTATION 360 // Voreinstellung Rotationswinkel
#define MOVE_FORWARDS 1
behaviour_command_t cruise = {CRUISE_SPEED_FWD, CRUISE_SPEED_FWD, FWD,
false, CRUISE_ROTATION,0, MOVE_FORWARDS};
/**
* Cruise Behaviour (Suchmuster)
*/
void behaviour_cruise(void)
{
}
/*****************************************************************************/
// Escape Behaviour: Bumper Fluchtfunktion
// Diese wird nur aktiviert wenn einer der Micoschalter an den Bumpern ausgelöst wird.
//Define´s
#define ESCAPE_SPEED_BWD 80
#define ESCAPE_SPEED_ROTATE 60
#define ESCAPE_FRONT 1
#define ESCAPE_FRONT_WAIT 2
#define ESCAPE_LEFT 3
#define ESCAPE_LEFT_WAIT 4
#define ESCAPE_RIGHT 5
#define ESCAPE_RIGHT_WAIT 6
#define ESCAPE_WAIT_END 7
behaviour_command_t escape = {0, 0, FWD, false, false, 0, IDLE};
/**
* This is the Escape behaviour for the Bumpers. (Standartfunktion für Bumber)
* Übernommen aus den Beispielprogrammen
*/
void behaviour_escape(void)
{
static uint8_t bump_count = 0;
switch(escape.state)
{
case IDLE:
break;
case ESCAPE_FRONT:
escape.speed_left = ESCAPE_SPEED_BWD;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 200;
else
escape.move_value = 140;
escape.state = ESCAPE_FRONT_WAIT;
bump_count+=2;
break;
case ESCAPE_FRONT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
if(bump_count > 3)
{
escape.move_value = 110;
escape.dir = RIGHT;
bump_count = 0;
}
else
{
escape.dir = LEFT;
escape.move_value = 75;
}
escape.rotate = true;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_LEFT:
escape.speed_left = ESCAPE_SPEED_BWD;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 160;
else
escape.move_value = 100;
escape.state = ESCAPE_LEFT_WAIT;
bump_count++;
break;
case ESCAPE_LEFT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
escape.dir = RIGHT;
escape.rotate = true;
if(bump_count > 3)
{
escape.move_value = 100;
bump_count = 0;
}
else
escape.move_value = 65;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_RIGHT:
escape.speed_left = ESCAPE_SPEED_BWD ;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 160;
else
escape.move_value = 100;
escape.state = ESCAPE_RIGHT_WAIT;
bump_count++;
break;
case ESCAPE_RIGHT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
escape.dir = LEFT;
escape.rotate = true;
if(bump_count > 3)
{
escape.move_value = 100;
bump_count = 0;
}
else
escape.move_value = 65;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_WAIT_END:
if(!(escape.move || escape.rotate))
escape.state = IDLE;
break;
}
}
/**
* Bumpers Event handler (Bumberaufruf)
* Übernommen aus Beispielprogrammen
*/
void bumpersStateChanged(void)
{
if(bumper_left && bumper_right)
{
escape.state = ESCAPE_FRONT;
}
else if(bumper_left)
{
if(escape.state != ESCAPE_FRONT_WAIT)
escape.state = ESCAPE_LEFT;
}
else if(bumper_right)
{
if(escape.state != ESCAPE_FRONT_WAIT)
escape.state = ESCAPE_RIGHT;
}
}
/*****************************************************************************/
// Avoid Behaviour: Antikollisionssystem (ACS)
#define AVOID_SPEED_L_ARC_LEFT 20
#define AVOID_SPEED_L_ARC_RIGHT 80
#define AVOID_SPEED_R_ARC_LEFT 80
#define AVOID_SPEED_R_ARC_RIGHT 20
#define AVOID_SPEED_ROTATE 60
#define AVOID_OBSTACLE_RIGHT 1
#define AVOID_OBSTACLE_LEFT 2
#define AVOID_OBSTACLE_MIDDLE 3
#define AVOID_OBSTACLE_MIDDLE_WAIT 4
#define AVOID_END 5
behaviour_command_t avoid = {0, 0, FWD, false, false, 0, IDLE};
/**
* Avoid behaviour with ACS IR Sensors.
*/
void behaviour_avoid(void)
{
static uint8_t last_obstacle = LEFT;
static uint8_t obstacle_counter = 0;
switch(avoid.state)
{
case IDLE:
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
else if(obstacle_left)
avoid.state = AVOID_OBSTACLE_LEFT;
else if(obstacle_right)
avoid.state = AVOID_OBSTACLE_RIGHT;
break;
case AVOID_OBSTACLE_MIDDLE:
avoid.dir = last_obstacle;
avoid.speed_left = AVOID_SPEED_ROTATE;
avoid.speed_right = AVOID_SPEED_ROTATE;
if(!(obstacle_left || obstacle_right))
{
if(obstacle_counter > 3)
{
obstacle_counter = 0;
setStopwatch4(0);
}
else
setStopwatch4(400);
startStopwatch4();
avoid.state = AVOID_END;
}
break;
case AVOID_OBSTACLE_RIGHT:
avoid.dir = FWD;
avoid.speed_left = AVOID_SPEED_L_ARC_LEFT;
avoid.speed_right = AVOID_SPEED_L_ARC_RIGHT;
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
if(!obstacle_right)
{
setStopwatch4(500);
startStopwatch4();
avoid.state = AVOID_END;
}
last_obstacle = RIGHT;
obstacle_counter++;
break;
case AVOID_OBSTACLE_LEFT:
avoid.dir = FWD;
avoid.speed_left = AVOID_SPEED_R_ARC_LEFT;
avoid.speed_right = AVOID_SPEED_R_ARC_RIGHT;
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
if(!obstacle_left)
{
setStopwatch4(500);
startStopwatch4();
avoid.state = AVOID_END;
}
last_obstacle = LEFT;
obstacle_counter++;
break;
case AVOID_END:
if(getStopwatch4() > 1000)
{
stopStopwatch4();
setStopwatch4(0);
avoid.state = IDLE;
}
break;
}
}
/**
* ACS Event Handler
*/
void acsStateChanged(void)
{
if(avoid.state != IDLE)
{
if(obstacle_left && obstacle_right)
statusLEDs.byte = 0b100100;
else
statusLEDs.byte = 0b000000;
statusLEDs.LED5 = obstacle_left;
statusLEDs.LED4 = (!obstacle_left);
statusLEDs.LED2 = obstacle_right;
statusLEDs.LED1 = (!obstacle_right);
updateStatusLEDs();
}
}
/*****************************************************************************/
// Follow Behaviour:
#define FOLLOW 3
#define LIGHT_MIN 1000 // Lichtstärkenminimum einstellen => Wechsel in "Ziel gefunden"
behaviour_command_t follow = {0, 0, FWD, false, false, 0, IDLE};
/**
* The new Light following behaviour.
*/
void behaviour_follow(void)
{
switch(follow.state)
{
case IDLE:
if(adcLSL >= LIGHT_MIN || adcLSR >= LIGHT_MIN) // Ist das Licht hell genung? Linker oder Rechter LDR
{
setStopwatch6(0);
startStopwatch6();
follow.state = FOLLOW;
}
break;
case FOLLOW:
if(getStopwatch6() > 100)
{
if(adcLSL >= LIGHT_MIN || adcLSR >= LIGHT_MIN) // ISt das Licht hell genung?
{
// Hier wird die Geschwindigkeit der Motoren berechet - Der Roboter bewegt sich
// immer in die Richtung des Sensors der heller angestrahlt wird.
// Hier kann man die konstanten Werte dafür einstellen um andere Geschwindigkeiten
// zu fahren (40 und 60 hier)
// Vergleich Lichtstärke rechts links
int16_t dif = ((int16_t)(adcLSL - adcLSR))>>1;
if(dif > 40) dif = 40;
if(dif < -40) dif = -40;
follow.speed_left = 60 - dif;
follow.speed_right = 60 + dif;
// Schaltet die LEDs - Wie eine kleine Balckengrafik zeigen diese dann an wo das Hinderniss
// geortet wurde.
// Dazu wird die Differenz der Lichtstärke damit dargestellt => Welcher Richtung muss der
// Roboter fahren (Anzeige Lichtsstärke differenz)
if(dif > 30)
setLEDs(0b111000);
else if(dif > 25)
setLEDs(0b011000);
else if(dif > 5)
setLEDs(0b001000);
else if(dif > -5)
setLEDs(0b001001);
else if(dif > -25)
setLEDs(0b000001);
else if(dif > -30)
setLEDs(0b000011);
else
setLEDs(0b000111);
}
else // Die Lichtstärke ist zu gering => Es ist zu dunkel
{
stopStopwatch6();
follow.state = IDLE;
}
if((avoid.state || escape.state)) // Gibt aktuelle Informationen über ein Hinderniss
{ // an die LEDs aus um dieses anzuzeigen
if(obstacle_left && obstacle_right)
statusLEDs.byte = 0b100100;
else
statusLEDs.byte = 0b000000;
statusLEDs.LED5 = obstacle_left; // Hinderniss links, LED rot
statusLEDs.LED4 = (!obstacle_left); // kein Hinderniss Links, LED grün
statusLEDs.LED2 = obstacle_right; // Hinderniss rechts, LED rot
statusLEDs.LED1 = (!obstacle_right);
updateStatusLEDs();
}
setStopwatch6(0);
}
break;
}
}
/*****************************************************************************/
// End Behaviour:
#define LIGHT_MAX 1200 //Maximale Lichtstärke einstellen => Ziel gefunden!
behaviour_command_t end = {0, 0, FWD, false, false, 0, IDLE};
/**
* End Behaviour
*/
void behaviour_end(void)
{
if(adcLSL >= LIGHT_MAX || adcLSR >= LIGHT_MAX)
{
moveAtSpeed(0,0); // Ende der Bewegung
setLEDs(0b010000); // LED setzen
startStopwatch1(); // Stopwatch starten
}
}
/*****************************************************************************/
// Behaviour control: Suchmuster (Verhaltenssteuerung)
/**
* Diese Funktion ist für das Suchmuster verantwortlich.
* Je nach dem wie die aktuellen Werte von behaviour_command_t grade sind wird
* der Fahrweg festgelegt. Ist keine Lichtquelle gefunden sucht der Roboter
* nach dem Muster fahr 50cm, dreh dich 360°, fahr wieder 50cm... usw.
* Das ganze geschieht mit festgelegter Geschwindigkeit und Rotation
*/
void moveCommand(behaviour_command_t * cmd)
{
if(cmd->move_value > 0) // Fahren ca. 50cm
{
if(cmd->rotate) // Rotieren, einmal im kreis drehen und dabei suchen
// Rotate Wert auslesen aus cmd
rotate(cmd->speed_left, cmd->dir, cmd->move_value, false);
// Wenn cmd.rote = true wird dieser Befehl ausgeführt
else if(cmd->move) //fahre wieder ca. nach dem drehen50 cm
// Wenn in cmd.move= true wird dieser Befehlt ausgeführt
move(cmd->speed_left, cmd->dir, DIST_MM(cmd->move_value), false);
cmd->move_value = 0; //fahren
}
else if(!(cmd->move || cmd->rotate)) // fahren und rotieren => Richtung ändern
{
changeDirection(cmd->dir);
moveAtSpeed(cmd->speed_left,cmd->speed_right);
}
else if(isMovementComplete())
{
cmd->rotate = false;
cmd->move = false;
}
}
/**
* Hier werden die Prioritäten und Rheinfolgen der eigenen Modis fest.
* Es wird nur der Modus mit der hören Priorität bearbeitet,
* falls gleichzeitig die Bedingungen für einen zweiten gegeben sei sollten.
*/
void behaviourController(void)
{
// Aufruf der einzelen Verhaltsnsweisen (Fälle)
behaviour_cruise(); //Suchen!
behaviour_follow(); //Verfolgen!
behaviour_avoid(); //Ausweichen!
behaviour_escape(); //Flüchten!
behaviour_end(); //Gefunden
// Ausführen der Befehle je nach Priorität:
if(end.state != IDLE) // Höchste Priorität - 5
{
moveCommand(&end);
clearLCD();
showScreenLCD("Lichtquelle","erreicht");
writeString_P("Lichtquelle erreicht\n");
writeString_P_WIFI("Lichtquelle erreicht\n");
}
else if(escape.state != IDLE) //Priorität - 4
{
moveCommand(&escape);
clearLCD();
showScreenLCD("Hindernis gerammt","zuruecksetzen");
writeString_P("Hindernis gerammt , zuruecksetzen\n");
writeString_P_WIFI("Hinderniss gerammt, zurücksetzen\n");
}
else if(avoid.state != IDLE) // Priorität- 3
{
moveCommand(&avoid);
clearLCD();
showScreenLCD("Hindernis erkannt","...umfahren");
writeString_P("Hindernis erkannt, Hinderniss umfahren\n");
writeString_P_WIFI("Hinderniss erkannt, Hinderniss umfahren\n");
}
else if(follow.state != IDLE) // Priorität - 2
{
moveCommand(&follow);
clearLCD();
showScreenLCD("Licht Quelle","gefunden");
writeString_P("Lichtquelle gefunden, Licht folgen");
writeString_P_WIFI("Lichtquelle gefunden, Licht folgen\n");
}
else
if(cruise.state != IDLE) // Priorität - 1
{
moveCommand(&cruise);
clearLCD();
showScreenLCD("Suche nach","Lichtquelle");
writeString_P("Suche nach Lichtquelle\n");
writeString_P_WIFI("Suche nach Lichtquelle\n");
}
else // Niedrigste Priorität - 0
moveCommand(&STOP); // Ist kein Fall aktiv, stehen bleiben, nichts tun
// Sollte in der aktuellen Konfiguration nie passieren
}
/*****************************************************************************/
// Main: Hauptprogramm
int main(void)
{
initRP6M256(); //Prototypenaufruf
initLCD(); //Prototypenaufruf
setLEDs(0b111111); //Alle LEDs ansteuern für 2,5 sek.
mSleep(2500); //Wartezeit
setLEDs(0b001001); //Grüne LED´s ansteuern
// Setup ACS power:
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_ACS_POWER, ACS_PWR_MED);
I2CTWI_initMaster(100);
clearLCD(); // Clear the whole LCD Screen
// Set Bumpers state changed event handler:
BUMPERS_setStateChangedHandler(bumpersStateChanged);
// Set ACS state changed event handler:
ACS_setStateChangedHandler(acsStateChanged);
// Main loop (Endlosschleife)
while(true)
{
behaviourController();
task_checkINT();
task_I2CTWI();
}
return 0;
}
RP6- WIFI Lichtsucher mit Programm Problemen
Zitieren
Lesezeichen