Linkin1987
19.07.2014, 12:17
Hallo zusammen. Ich bin ganz neu hier und auch ziemlich neu in der ganzen Materie.
Ich habe zusammen mit einem Kollegen ein Programm geschrieben / geändert um ein kleines Projekt zu machen für den Abendunterricht.
Im Prinzip ist es eine geänderte Version des Lichtsuchers.
Dazu haben wir noch das Wifi modul für einen kleinen Datenaustausch mit dem PC und ein Display um aktuelle Zustände anzugeben.
Soweit eigentlich nichts wildes.
Das Programm funktioniert meiner Ansicht nach auch, auf der RP6 base hardware.
Wenn wir es allerdings umschreiben für M256 Wifi Lib, dann funktioniert es nicht.
Es hängt im Schritt "Nach Lichtquelle suchen" :confused::confused::confused::confused:
Dieser wird auch wie es sein soll im Display angezeigt und im Wifi Terminal wiedergegeben.
Weder mein Projektparner noch unser Lehrer haben derzeit eine wirkliche Idee, wieso das ganze nicht funktioniert.
Die funktion make all im Programmers Notepad funktioniert auch ohne Probleme.
Wäre echt super wenn jemand eine hilfreiche Idee oder Tipp hätte!!
Vielen Dank im Vorraus!
/************************************************** *************************
* 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;
}
Ich habe zusammen mit einem Kollegen ein Programm geschrieben / geändert um ein kleines Projekt zu machen für den Abendunterricht.
Im Prinzip ist es eine geänderte Version des Lichtsuchers.
Dazu haben wir noch das Wifi modul für einen kleinen Datenaustausch mit dem PC und ein Display um aktuelle Zustände anzugeben.
Soweit eigentlich nichts wildes.
Das Programm funktioniert meiner Ansicht nach auch, auf der RP6 base hardware.
Wenn wir es allerdings umschreiben für M256 Wifi Lib, dann funktioniert es nicht.
Es hängt im Schritt "Nach Lichtquelle suchen" :confused::confused::confused::confused:
Dieser wird auch wie es sein soll im Display angezeigt und im Wifi Terminal wiedergegeben.
Weder mein Projektparner noch unser Lehrer haben derzeit eine wirkliche Idee, wieso das ganze nicht funktioniert.
Die funktion make all im Programmers Notepad funktioniert auch ohne Probleme.
Wäre echt super wenn jemand eine hilfreiche Idee oder Tipp hätte!!
Vielen Dank im Vorraus!
/************************************************** *************************
* 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;
}