Hallo
1. Spielfeld:
Helle Rückseite einer Lederimetation. Diese gibt es als Meterware von der Rolle zu kaufen.
18 mm breites schwarzes Isolierband als Linie.
2.NIBOburger:
NIBOburger mit Atmega1284 (15Mhz).
Für die Linienverfolgung wird das Signal der farbigen Bodensensoren verwendet.
3.Software:
NiboRoboLib 3.3,
Arduino 1.6.6,
Arduino PID Library 1.1.1,
Im Programm wird für jeden Motor ein eigener PID-Regler (Arduino PID-Library) verwendet.
4.Programmbeschreibung:
Nach dem Einschalten des NIBOburger wird mit Drücken der Taste 1 eine Messung auf dem hellen Untergrund durchgeführt.
Danach wird der Roboter auf die Linie gestellt.
Mit dem Taster 2 wird die kontinuierliche Messung der Bodensensoren gestartet. Die LEDS zeigen die Position über der Linie an:
LED1: links außerhalb der Linie
LED1 + LED2: links außen am Messbereich
LED2: links neben der Mitte
LED2 + LED3: über der Linienmitte
LED3: rechts neben der Mitte
LED3 + LED4: rechts außen am Messbereich
LED4: rechts außerhalb der Linie
Der Roboter wird von Hand hin und her belegt,bis beide blauen LEDs leuchten.
Mit dem Taster 3 werden danach die Motoren gestartet.
5.Sourcecode:
#include <PID_v1.h>
/*
*
***************************************************************
* Arduino PID Library - Version 1.1.1
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
*
* This Library is licensed under a GPLv3 License
***************************************************************
- For an ultra-detailed explanation of why the code is the way it is, please visit:
http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
- For function documentation see: http://playground.arduino.cc/Code/PIDLibrary
*/
#include <NIBOburger.h>
float l_max, c_max, r_max; // Messungen im Feld (100%)
float l, c, r; // Messwerte Links, Center, Rechts
double l_minus_r; // Differenz Messwert Links - Rechts
int16_t pwm_speed = 800; // Maximale Geschwindigkeit
int16_t pwm_links = pwm_speed; // Geschwindigkeit linker Motor
int16_t pwm_rechts = pwm_speed; // Geschwindigkeit rechter Motor
char key = ' ';
double Output_links, Output_rechts, Setpoint = 0.0; // Regler-Output im Bereich von 0 bis. 255
double Kp = 2.0 , Kd = 0.5, Ki = 0;
// PID_rechts wertet l_minus_r im Bereich von -100 bis 0 aus
// Im Bereich von 0 bis 100 ist der Output_rechts == 0
PID PID_rechts(&l_minus_r, &Output_rechts, &Setpoint, Kp, Ki, Kd, REVERSE);
// PID_links wertet l_minus_r im Bereich von 0 .bis 100 aus
// Im Bereich von -100 bis 0 ist der Output_links == 0
PID PID_links(&l_minus_r, &Output_links, &Setpoint, Kp, Ki, Kd, DIRECT);
enum
{
mitte,
mitte_links,
mitte_rechts,
links,
rechts,
links_ausserhalb,
rechts_ausserhalb,
absturz
} lage; // Auswertung des Messen über/entlang der Linie
uint8_t lage_old=13;
// Untergrund im Feld abseits der Linie messen
void Untergrund()
{
lage = absturz;
NIBOburger.setLeds(1,1,1,1);
// Nach dem Drücken der Taste 1 wird der Untegrund gemessen
while (key != 'A')
{
key = NIBOburger.getKeyChar();
}
NIBOburger.setLeds(0,0,0,0);
//Serial.println("Messung im Feld");
//NIBOburger.waitAnalogUpdate();
c_max = 100. / SurfaceSensor.getC(0);
//NIBOburger.waitAnalogUpdate();
//Serial.print("C = ");
//Serial.println(c_max,DEC);
l_max = 100. /SurfaceSensor.getL(0);
//NIBOburger.waitAnalogUpdate();
//Serial.print("L = ");
//Serial.println(l_max,DEC);
r_max = 100. /SurfaceSensor.getR(0);
//NIBOburger.waitAnalogUpdate();
//Serial.print("R = ");
//Serial.println(r_max,DEC);
// Nach dem Drücken der Taste 2 wird die Messung gestartet.
// Der Roboter muss so über der Linie positioniert werden.
// Über der Linienmitte leuten die beiden blauen LEDs.
// Danach werden die Motoeren mit Taste 3 gestartet
while (key != 'B')
{
key = NIBOburger.getKeyChar();
}
key = ' ';
while (key != 'C')
{
Messen();
leds();
key = NIBOburger.getKeyChar();
}
}
// Messen über/entlang der Linie
void Messen()
{
// Die Messwerte der 3 Sensoren werden direkt in %-Werte umgerechnt.
float c = c_max * SurfaceSensor.getC(0);
//NIBOburger.waitAnalogUpdate();
//Serial.print("C%;");
//Serial.print(c,DEC);
float l = l_max * SurfaceSensor.getL(0);
//NIBOburger.waitAnalogUpdate();
//Serial.print(";L%;");
//Serial.print(l,DEC);
float r = r_max * SurfaceSensor.getR(0);
//NIBOburger.waitAnalogUpdate();
//Serial.print(";R%;");
//Serial.print(r,DEC);
// l_minus_r ist der Input für die beiden PID-Regler
// Der Sollwert ist 0.0
l_minus_r = l - r;
//Serial.print(";l-r;");
//Serial.println(l_minus_r,DEC);
//***********************************
// Auswertung der Messwerte
// Im Regelbereich
if (c < 40.)
{
// Roboter ist über der Linienmitte im Regelbereich
if (abs(l_minus_r) < 20.)
{
//NIBOburger.setLeds(0,1,1,0);
lage = mitte;
}
// Roboter ist rechts von der Linienmitte im Regelbereich
else if (abs(l_minus_r) == (l_minus_r))
{
//NIBOburger.setLeds(0,0,1,0);
lage = mitte_rechts;
}
// Roboter ist links von der Linienmitte im Regelbereich
else
{
//NIBOburger.setLeds(0,1,0,0);
lage = mitte_links;
}
}
else // (c >=40)
{
// Rechts außerhalb des Regelbereiches unter dem rechten Sensor
if (r < 80.)
{
//NIBOburger.setLeds(0,0,1,1);
lage = rechts;
}
// Links außerhalb des Regelbereiches unter dem linken Sensor
else if (l < 80.)
{
//NIBOburger.setLeds(1,1,0,0);
lage = links;
}
// Im Feld, Roboter hat die Linie verloren
else if (lage == rechts)
{
//NIBOburger.setLeds(0,0,0,1);
lage = rechts_ausserhalb;
}
else if (lage == links)
{
//NIBOburger.setLeds(1,0,0,0);
lage = links_ausserhalb;
}
}
}
void Fahren()
{
switch (lage) {
// Bei lage == ..._ausserhalb Drehung auf der Stelle zur Linie hin
case links_ausserhalb:
pwm_links = -pwm_speed/2;
pwm_rechts = pwm_speed/2;
break;
// Bei lage == ..._ausserhalb Drehung auf der Stelle zur Linie hin
case rechts_ausserhalb:
pwm_rechts = -pwm_speed/2;
pwm_links = pwm_speed/2;
break;
// Im PID-Regelbereich
default:
// Berechnung
while (!PID_rechts.Compute());
while (!PID_links.Compute());
//Output_links und Output_rechts sind im Bereich von 0 bis 255
pwm_links = pwm_speed - (Output_links * pwm_speed/255.);
pwm_rechts = pwm_speed - (Output_rechts * pwm_speed/255.);
pwm_links = constrain(pwm_links, 0, 1024);
pwm_rechts = constrain(pwm_rechts, 0, 1024);
break;
}
Engine.setPWM(pwm_links,pwm_rechts);
}
// LED-Anzeige
void leds()
{
if (lage_old == lage) return;
switch (lage)
{
case mitte:
NIBOburger.setLeds(0,1,1,0);
break;
case mitte_rechts:
NIBOburger.setLeds(0,0,1,0);
break;
case mitte_links:
NIBOburger.setLeds(0,1,0,0);
break;
case rechts:
NIBOburger.setLeds(0,0,1,1);
break;
case links:
NIBOburger.setLeds(1,1,0,0);
break;
case rechts_ausserhalb:
NIBOburger.setLeds(0,0,0,1);
break;
case links_ausserhalb:
NIBOburger.setLeds(1,0,0,0);
}
lage_old == lage;
}
// Setup
void setup() {
NIBOburger.begin();
//Serial.begin(9600);
Engine.begin();
SurfaceSensor.begin();
PID_rechts.SetMode(AUTOMATIC);
PID_rechts.SetSampleTime(30);
PID_links.SetMode(AUTOMATIC);
PID_links.SetSampleTime(30);
Untergrund();
Engine.setPWM(pwm_links,pwm_rechts);
}
void loop() {
Messen();
Fahren();
leds();
}
lg Georg