Code:
#ifndef NIBO_H
#define NIBO_H
#include <avr/interrupt.h>
#include "nibocc/niboconfig.hpp"
#include "nibocc/i2cmaster.hpp"
#include "nibocc/adc.hpp"
#include "nibocc/bot.hpp"
#include "nibocc/delay.hpp"
#include "nibocc/display.hpp"
#include "nibocc/floor.hpp"
#include "nibocc/graphicdisplay.hpp"
#include "nibocc/irco.hpp"
#include "nibocc/leds.hpp"
#include "nibocc/motco.hpp"
#include "nibocc/pwm.hpp"
#include "nibocc/textdisplay.hpp"
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#endif
#ifndef ROBOTER_H
#define ROBOTER_H
#include "State.h"
const int SPEEDFACTOR = 35;
const int FREE = 0;
const int OBSTACLEFRONT = 1;
const int OBSTACLELEFT = 2;
const int OBSTACLERIGHT = 3;
class Roboter
{
int condition;
State* m_pCurrentState;
public:
Roboter();
~Roboter(){};
int getCondition(){return condition;}
void setCondition(int val){condition=val;}
void MoveAhead();
void MoveBack();
void MoveLeft();
void MoveRight();
void Update();
void ChangeState(State* pNewState);
};
#endif
#ifndef STATE_H
#define STATE_H
class Roboter;
class State // abstrakte Klasse
{
public:
virtual ~State(){}
virtual void Execute(Roboter*)=0;
};
#endif
#ifndef STATE_FREE_H
#define STATE_FREE_H
#include "StateObstacleFront.h"
#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "Roboter.h"
class State_Free : public State
{
public:
State_Free(){}
virtual void Execute(Roboter* bot);
};
#endif
#ifndef STATE_OBSTACLEFRONT_H
#define STATE_OBSTACLEFRONT_H
#include "Roboter.h"
#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "StateFree.h"
class State_ObstacleFront : public State
{
public:
State_ObstacleFront(){}
virtual void Execute(Roboter* bot);
};
#endif
#ifndef STATE_OBSTACLELEFT_H
#define STATE_OBSTACLELEFT_H
#include "Roboter.h"
#include "StateFree.h"
#include "StateObstacleFront.h"
#include "StateObstacleRight.h"
class State_ObstacleLeft : public State
{
public:
State_ObstacleLeft(){}
virtual void Execute(Roboter* bot);
};
#endif
#ifndef STATE_OBSTACLERIGHT_H
#define STATE_OBSTACLERIGHT_H
#include "Roboter.h"
#include "StateFree.h"
#include "StateObstacleFront.h"
#include "StateObstacleLeft.h"
class State_ObstacleRight : public State
{
public:
State_ObstacleRight(){}
virtual void Execute(Roboter* bot);
};
#endif
#ifndef UTIL_H
#define UTIL_H
#include "nibo.h"
using namespace nibocc;
// Hilfsfunktionen
float SupplyVoltage(void)
{
bot_update();
return(0.0166 * bot_supply - 1.19);
}
void float2string(float value, int decimal, char* valuestring)
{
int neg = 0; char tempstr[20];
int i = 0; int j = 0; int c; long int val1, val2;
char* tempstring;
tempstring = valuestring;
if (value < 0){ neg = 1; value = -value; }
for (j=0; j < decimal; j++) {value = value * 10;}
val1 = (long)(value * 2);
val2 = (val1 / 2) + (val1 % 2);
while (val2 !=0){
if ((decimal > 0) && (i == decimal)){
tempstr[i] = (char)(0x2E);
i++;
}
else{
c = (val2 % 10);
tempstr[i] = (char) (c + 0x30);
val2 = val2 / 10;
i++;
}
}
if (neg){
*tempstring = '-';
tempstring++;
}
i--;
for (;i > -1;i--){
*tempstring = tempstr[i];
tempstring++;
}
*tempstring = '\0';
}
void textout(int x, int y, char* str, int ft)
{
Graficdisplay::move(x,y);
Graficdisplay::print_text(str, ft);
}
void leds_set_status_all(uint8_t col0, uint8_t col1, uint8_t col2, uint8_t col3, uint8_t col4, uint8_t col5)
{
Leds::set_status(col0,0);
Leds::set_status(col1,1);
Leds::set_status(col2,2);
Leds::set_status(col3,3);
Leds::set_status(col4,4);
Leds::set_status(col5,5);
}
void Init()
{
sei();
Bot::init();
I2CMaster::init();
Pwm::init();
Leds::init();
Floor::init();
Display::init();
Graficdisplay::init();
}
// Hilfskonstruktionen
// pure virtual wird dennoch ausgeführt:
extern "C"{
void __cxa_pure_virtual() {} }
// Ersatz für new, new[], delete und delete[] der fehlenden C++-Standard-Bibliothek
void* operator new (size_t size) { return malloc(size); }
void* operator new[] (size_t size) { return malloc(size); }
void operator delete (void* ptr) { free(ptr); }
void operator delete[] (void* ptr) { free(ptr); }
#endif
//Roboter.cpp
#include "nibo.h"
#include "Roboter.h"
#include "StateFree.h"
using namespace nibocc;
void static leds_set_status_all(uint8_t col0, uint8_t col1, uint8_t col2, uint8_t col3, uint8_t col4, uint8_t col5)
{
Leds::set_status(col0,0);
Leds::set_status(col1,1);
Leds::set_status(col2,2);
Leds::set_status(col3,3);
Leds::set_status(col4,4);
Leds::set_status(col5,5);
}
Roboter::Roboter()
{
condition = FREE;
m_pCurrentState = new State_Free();
}
void Roboter::Update()
{
m_pCurrentState->Execute(this);
}
void Roboter::ChangeState(State* pNewState)
{
delete m_pCurrentState;
m_pCurrentState = pNewState;
}
void Roboter::MoveAhead()
{
//entry
leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_GREEN, LEDS_GREEN, LEDS_OFF, LEDS_OFF);
//do
MotCo::set_speed( 3*SPEEDFACTOR, 3*SPEEDFACTOR );
MotCo::update();
//exit
}
void Roboter::MoveBack()
{
//entry
leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_RED, LEDS_RED, LEDS_OFF, LEDS_OFF);
//do
MotCo::set_speed(-2*SPEEDFACTOR,-2*SPEEDFACTOR );
MotCo::update();
//exit
}
void Roboter::MoveLeft()
{
//entry
leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_ORANGE, LEDS_ORANGE);
//do
MotCo::set_speed( -SPEEDFACTOR, SPEEDFACTOR );
MotCo::update();
//exit
}
void Roboter::MoveRight()
{
//entry
leds_set_status_all(LEDS_ORANGE, LEDS_ORANGE, LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_OFF);
//do
MotCo::set_speed( SPEEDFACTOR, -SPEEDFACTOR );
MotCo::update();
//exit
}
//StateFree.cpp
#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "StateObstacleFront.h"
#include "StateFree.h"
void State_Free::Execute(Roboter* bot)
{
if ( (bot->getCondition()) == FREE )
{
bot->MoveAhead();
}
if ( (bot->getCondition()) == OBSTACLEFRONT )
{
bot->ChangeState(new State_ObstacleFront());
}
if ( (bot->getCondition()) == OBSTACLELEFT )
{
bot->ChangeState(new State_ObstacleLeft());
}
if ( (bot->getCondition()) == OBSTACLERIGHT )
{
bot->ChangeState(new State_ObstacleRight());
}
}
//StateObstacleFront.cpp
#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "StateObstacleFront.h"
#include "StateFree.h"
void State_ObstacleFront::Execute(Roboter* bot)
{
if ( (bot->getCondition()) == FREE )
{
bot->ChangeState(new State_Free());
}
if ( (bot->getCondition()) == OBSTACLEFRONT )
{
bot->MoveBack();
}
if ( (bot->getCondition()) == OBSTACLELEFT )
{
bot->ChangeState(new State_ObstacleLeft());
}
if ( (bot->getCondition()) == OBSTACLERIGHT )
{
bot->ChangeState(new State_ObstacleRight());
}
}
//StateObstacleLeft.cpp
#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "StateObstacleFront.h"
#include "StateFree.h"
void State_ObstacleLeft::Execute(Roboter* bot)
{
if ( (bot->getCondition()) == FREE )
{
bot->ChangeState(new State_Free());
}
if ( (bot->getCondition()) == OBSTACLEFRONT )
{
bot->ChangeState(new State_ObstacleFront());
}
if ( (bot->getCondition()) == OBSTACLELEFT )
{
bot->MoveRight();
}
if ( (bot->getCondition()) == OBSTACLERIGHT )
{
bot->ChangeState(new State_ObstacleRight());
}
}
//StateObstacleRight.cpp
#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "StateObstacleFront.h"
#include "StateFree.h"
void State_ObstacleRight::Execute(Roboter* bot)
{
if ( (bot->getCondition()) == FREE )
{
bot->ChangeState(new State_Free());
}
if ( (bot->getCondition()) == OBSTACLEFRONT )
{
bot->ChangeState(new State_ObstacleFront());
}
if ( (bot->getCondition()) == OBSTACLELEFT )
{
bot->ChangeState(new State_ObstacleLeft());
}
if ( (bot->getCondition()) == OBSTACLERIGHT )
{
bot->MoveLeft();
}
}
//test.c
/********************************************
* *
* N I B O - A N T I K O L L I S I O N *
* *
********************************************/
// Stand: 09.08.2007
// Erhard Henkes
// Programmiersprache C++
// Geometrischer Sensorschwerpunkt
// Gewichtung der Sensoren
// state pattern design
// TODO: wird noch unter Bürostuhlbein eingeklemmt
// schwingt ab und zu zwischen zwei Zuständen
// Absolute Grenzen durch relative ersetzen
// Header
#include "nibo.h" // Zusammenfassung der typischen Header für Nibo
#include "Roboter.h"
#include "utilities.h"
using namespace nibocc;
//Richtungen
#define LINKS 0
#define VORNE_LINKS 1
#define VORNE 2
#define VORNE_RECHTS 3
#define RECHTS 4
/****************************************************************/
int main()
{
Init();
Roboter Nibo;
// Variablen
uint16_t Vektor[5][2]; // Einheitsvektoren (*10) [0] ist x- und [1] ist y-Wert
uint8_t weightfactor[5]; // Gewichtungsfaktor
uint16_t VektorMalSensor[5][2]; // Sensorwert * Einheitsvektor (*10)
uint16_t VektorMalSensorSumme[2]; // Sensorschwerpunkt (x,y) für Auswertung
// Einheitsvektor (* 10) gerundet
Vektor[0][0] = -10; // LINKS x
Vektor[0][1] = 0; // LINKS y
Vektor[1][0] = -7; // VORNE_LINKS x
Vektor[1][1] = 7; // VORNE_LINKS y
Vektor[2][0] = 0; // VORNE x
Vektor[2][1] = 10; // VORNE y
Vektor[3][0] = 7; // VORNE_RECHTS x
Vektor[3][1] = 7; // VORNE_RECHTS y
Vektor[4][0] = 10; // RECHTS x
Vektor[4][1] = 0; // RECHTS y
// Gewichtung der Sensoren in Abhängigkeit von der Richtung
weightfactor[LINKS] = 1;
weightfactor[VORNE_LINKS] = 2;
weightfactor[VORNE] = 3;
weightfactor[VORNE_RECHTS] = 2;
weightfactor[RECHTS] = 1;
// Vorbereitungen
Leds::set_displaylight(1000);
Leds::set_headlights(256);
Floor::enable_ir();
MotCo::set_pwm(512,512);
MotCo::set_speed(3,3);
// fixe Display-Anzeigen
textout(35,0,"Volt", 0);
textout(0, 8,"distance:", 0);
textout(0,24,"floor:", 0);
textout(0,40,"line:", 0);
// Laufvariablen
int i,j;
while(true)
{
// Akkuspannung anzeigen
float Ubatt = SupplyVoltage();
char text[6];
float2string(Ubatt,2,text);
textout(0,0," ",0); // 5 Zeichen löschen
textout(0,0,text, 0);
// Abstandsmessung Raumgefühl
IrCo::start_measure();
IrCo::update();
// Floor
uint16_t floor_distance[2];
uint16_t line_distance[2];
// Abstandsmessung Floor
Floor::update();
floor_distance[0] = floor_l;
floor_distance[1] = floor_r;
line_distance[0] = line_l;
line_distance[1] = line_r;
//Strings für Display
char irco_string[5][5];
char floor_string[2][5];
char line_string[2][5];
/*
IR-Abstandssensoren
*/
for(i=0; i<5; ++i)
textout(i*21,16," ",0); //löschen
for(i=0; i<5; ++i) // z.Z. noch rechts 0 und links 4 !!!!!!!!!!!!!
{
itoa(irco_distance[i],irco_string[i],10);
textout(i*21,16,irco_string[i],0);
}
/*
IR-Floorsensoren (Abgrunderkennung)
*/
for(i=0; i<2; ++i)
textout(i*28,32," ",0); //löschen
for(i=0; i<2; ++i)
{
itoa(floor_distance[i],floor_string[i],10);
textout(i*28,32,floor_string[i],0);
}
/*
IR-Liniensensoren
*/
for(i=0; i<2; ++i)
textout(i*28,48," ",0); //löschen
for(i=0; i<2; ++i)
{
itoa(line_distance[i],line_string[i],10);
textout(i*28,48,line_string[i],0);
}
/*
MOTCO
Mathematische Methode "x/y-Schwerpunkt der Sensorvektoren bilden":
(Einheitsvektoren * 10) * Sensorwert (0-255) * weightfactor, davon Summe bilden
VektorMalSensorSumme[...] 0 ist x-Wert und 1 ist y-Wert
Blockade: y kann maximal 14790 groß werden (vl, v, vr 255)
Richtung: x kann maximal -6120 (Hindernis links) bzw. +6120 (H. rechts) werden (l, vl 255 bzw. r, vr 255)
*/
// Ermittlung von VektorMalSensorSumme[...] (gewichteter x- und y-Wert)
VektorMalSensorSumme[0] = 0; // x-Wert
VektorMalSensorSumme[1] = 0; // y-Wert
// i entspricht links, vornelinks, vorne, vornerechts, rechts
// j entspricht x und y
for (i=0; i<5; ++i) {
for (j=0; j<2; ++j) {
VektorMalSensor[i][j] = Vektor[i][j] * irco_distance[i] * weightfactor[i]; // 4-i wegen IRCo?????
VektorMalSensorSumme[j] += VektorMalSensor[i][j];
}
}
// Reaktion auf VektorMalSensorSumme[...] (x- und y-Wert)
// GrenzenY
uint16_t GrenzeY1 = 12000; // Zustandsgrenze: BLOCKIERT / EVTL. AUSWEICHEN
uint16_t GrenzeY2 = 5000; // Zustandsgrenze: EVTL. AUSWEICHEN / FREI
// GrenzenX
uint16_t GrenzeXlinks = -2000; // Zustandsgrenze: LINKS / GERADEAUS
uint16_t GrenzeXrechts = 2000; // Zustandsgrenze: RECHTS / GERADEAUS
/* Curr.State Condition State Transition
// ----------------------------------------------------------------------------------------------------------------------
// (alle) (Y>=GrenzeY1) BLOCKIERT
// (alle) ((Y<GrenzeY1) && (Y>= GrenzeY2) && (X<GrenzeXlinks)) HINDERNISLINKS
// (alle) ((Y<GrenzeY1) && (Y>= GrenzeY2) && (X>GrenzeXlinks)) HINDERNISRECHTS
// (alle) ((Y<GrenzeY1) && (Y>= GrenzeY2) && (X>=GrenzeXlinks) && (X<=GrenzeXrechts)) FREI
// (alle) (Y<GrenzeY2) FREI
*/
// Condition ermitteln und entsprechende Klasse State... als m_pCurrentState setzen
// y-Wert
if( VektorMalSensorSumme[1] >=GrenzeY1)
{
Nibo.setCondition(OBSTACLEFRONT);
}
if( (VektorMalSensorSumme[1] < GrenzeY1) && (VektorMalSensorSumme[1] >=GrenzeY2) )
{
// x-Wert
if( VektorMalSensorSumme[0] < GrenzeXlinks )
{
Nibo.setCondition(OBSTACLELEFT);
}
if( VektorMalSensorSumme[0] > GrenzeXrechts )
{
Nibo.setCondition(OBSTACLERIGHT);
}
if( (VektorMalSensorSumme[0] >= GrenzeXlinks) && (VektorMalSensorSumme[0] <= GrenzeXrechts) )
{
Nibo.setCondition(FREE);
}
}
if ( (VektorMalSensorSumme[1] < GrenzeY2) || (irco_distance[2] < 150) )
{
Nibo.setCondition(FREE);
}
// Auf Zustand reagieren
Nibo.Update();
} // end of while
return 0;
}
Zitieren
Lesezeichen