Hi,
ich habe das Programm von Hermann mal als Vorlage genommen und es etwas "verkompliziert". ;-)
Inhaltlich bleibt es bei dem Zweipunktregler. Allderdings versuche ich den Einfluß des Umgebungslichtes zu unterdrücken (ich meine die Idee dazu mal bei waste gefunden zu haben). Bei meinen Messungen (mit einer superhellen LED am Liniensensor) war es allerdings nötig, eine Wartezeit zwischen den Messungen mit aus- bzw. eingeschalteter LED einzuschieben, da die LED zu träge ist. Das mag mit anderen LEDs anders sein.
Weiterhin habe ich die Taster dazu verwendet, um "on the fly" die Parameter für Geschwindigkeit und Hell/Dunkelschwelle zu ändern. Dies geht mit schrittweisem Erhöhen oder erniedrigen oder direktem Eingeben über ein Terminalprogramm oder - im Falle der Hell/Dunkelschwelle - auch über das Messen des Wertes, wenn man den Roboter in der austarierten Stellung hält.
Die Funktionen UartGetc() und ReadInt() schlage ich vor als Erweiterung in die nächste Lib-Version aufzunehmen. Natürlich fehlt dann auch noch ein ReadLong(), dass aber analog zu ReadInt() zu programmieren wäre. Damit hätten die entsprechenden Print-Funktionen dann auch ihr Gegenstück.
Schaut Euch das mal an, experimentiert damit und sagt eure Meinung...
Gruß,
_HP_
EDIT: Der code wurde leicht verändert.
/************************************************** **************************/
/*!
\file test.c
\brief
Programm zum Balancieren von ASURO auf den Hinterrädern.
\par Hinweis:
Die Regelung erfolgt mittels Zweipunktregler. Bitte auch die Infos zur\n
Funktion main() beachten.
\author _HP_
\version beta - 04.04.2007 - _HP_\n
first implementation
\version beta - 05.04.2007 - _HP_\n
bugfixing in ReadInt und kleine Änderungen im Hauptprogramm
************************************************** ***************************/
/************************************************** ***************************
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* any later version. *
* *
************************************************** ***************************/
#include <asuro.h>
#include <stdlib.h>
#define INI_SPEED 240
#define INI_THRESHOLD 53
// Tastenwert bitorientiert, K1 = Bit5, K2 = Bit4, K3 = Bit3, K4 = Bit2,
// K5 = Bit1, K6 = Bit0
#define K1 (1 << 5) // Schalter an der linken Seite neben der IR-Diode
#define K2 (1 << 4)
#define K3 (1 << 3)
#define K4 (1 << 2)
#define K5 (1 << 1)
#define K6 (1 << 0)
/************************************************** **************************/
/* */
/* Funktion: UartGetc() */
/* */
/*!
\brief
Einlesen eines Zeichens von der serielle Schnittstelle. Die Funktion wartet\n
solange, bis ein Zeichen gelesen wurde!
\param
keine
\return
gelesenes Zeichen
\author _HP_
\version beta - 04.04.2007 - _HP_\n
first implementation
\par Beispiel:
(Nur zur Demonstration der Parameter/Returnwerte)
\code
char c;
c = UartGetc();
\endcode
************************************************** ***************************/
char UartGetc(void)
{
unsigned char data = 0x00; // gelesenen Zeichen
UCSRB = 0x10; // Empfaenger einschalten
while (data == 0x00)
{
if (UCSRA & 0x80) data = UDR;
}
return data;
}
/************************************************** **************************/
/* */
/* Funktion: ReadInt() */
/* */
/*!
\brief
Einlesen einer Zahl im Integerbereich (+/- 32767) von der serielle /n
Schnittstelle./n
Die Funktion wartet bis entweder 6 erlaubte Zeichen (Ziffern und an erster\n
Stelle ein Vorzeichen) eingegeben, oder ein Zeichen im Bereich 0x00 - 0x1F\n
gelesen wurden!
\param
keine
\return
Integerwert der eingegebenen Zeichen
\author _HP_
\version beta - 04.04.2007 - _HP_\n
first implementation
\version beta - 05.04.2007 - _HP_\n
numbers[] um ein byte vergrößert, um immer ein NULL-Zeichen\n
am Ende zu haben
\par Beispiel:
(Nur zur Demonstration der Parameter/Returnwerte)
\code
int z;
z = ReadInt();
\endcode
************************************************** ***************************/
int ReadInt(void)
{
unsigned char c; // gelesenes Zeichen
unsigned char i = 0; // Index in numbers
char numbers[7] = {0x00}; // Vorzeichen und 5 Ziffern + abschließendes /0
while (i < 6) // Es können maximal 5 Ziffern und ein Vorzeichen gelesen werden
{
c = UartGetc();
if ((c > 0x2F) && (c < 0x3A))
{
numbers[i++] = c;
}
if ((i == 0) && ((c == 0x2B) || (c == 0x2D))) // Vorzeichen an erste Position zulassen
{
numbers[i++] = c;
}
if (c < 0x20) // Beenden bei Eingabe eines nichtdruckbaren Zeichens
{
i = 10;
}
}
return atoi(numbers);
}
/************************************************** **************************/
/* */
/* Funktion: ReadSensor() */
/* */
/*!
\brief
Das Licht an den Liniensensoren wird gemessen und die Summe von beiden\n
Sensoren wird zurückgegeben.
Die Funktion minimiert den Einfluß des Umgebungslichtes, indem eine Messung\n
zunächst bei ausgeschaltetem Umgebungslicht durchgeführt wird. Das Ergebnis\n
dieser Messung wird von der Messung mit eingeschalteter LED abgezogen.\n
Da die LED eine gewisse Trägheit besitzt, wird nach dem Ein- bzw. Ausschalten\n
eine kurze Zeit gewartet, bis die Messung erfolgt. Diese Zeit muss evtl. auf\n
den eigenen ASURO abgestimmt werden.
\param
keine
\return
Summe beider Fototransistoren, bereinigt um den Wert des Umgebungslichtes.
\author _HP_
\version beta - 04.04.2007 - _HP_\n
first implementation
\par Beispiel:
(Nur zur Demonstration der Parameter/Returnwerte)
\code
int Licht;
Licht = ReadSensor();
\endcode
************************************************** ***************************/
int ReadSensor(void)
{
unsigned int FotoTrans[2]; // gemessene Werte der Fototransistoren (FotoTrans[0] ist links)
int LightEnv; // Summe des gemessen Umgebungslichtes beider Fototransistoren
unsigned char Delay = 10; // Wartezeit in 1/36 ms bis die FrontLED reagiert hat.
FrontLED(OFF);
Sleep(Delay);
LineData(FotoTrans);
LightEnv = FotoTrans[0] + FotoTrans[1];
FrontLED(ON);
Sleep(Delay);
LineData(FotoTrans);
return FotoTrans[0] + FotoTrans[1] - LightEnv;
}
/************************************************** **************************/
/* */
/* Funktion: main() */
/* */
/*!
\brief
Programm zum Balancieren des ASURO auf den Hinterrädern.\n
Die Grundidee ist von HermannSW beschrieben im Thread\n
https://www.roboternetz.de/phpBB2/viewtopic.php?t=29269 .\n
Es ist notwendig, den ASURO hinten so zu beschweren, dass sich die Nase\n
nicht zu weit über dem Boden befindet, wenn er sich im Gleichgewicht\n
befindet.
Mit den Tastern kann man die Parameter für die Helligkeit im \n
Gleichgewichtszustand und die Geschwindigkeit des linken Motors im\n
laufenden Betrieb wie folgt ändern:\n
Taster linke Seite ändert die Helligkeitsschwelle der Sensoren im\n
Gleichgewichtszustand... \n
K1 gedrückt : Threshold++; \n
K2 gedrückt : Threshold--; \n
K3 gedrückt : Threshold über Terminalprogramm vorgeben \n
K1 und K2 gedrückt: Threshold aktuellen Wert der Sensoren übernehmen \n
Taster rechte Seite ändert die Geschwindigkeit... \n
K4 gedrückt : Speed über Terminalprogramm vorgeben \n
K5 gedrückt : Speed--; \n
K6 gedrückt : Speed++; \n
Bei jedem Betätign der Taster werden die neuen Werte für THRESHOLD \n
und SPEED über die Infrarotschnittstelle ausgegeben.
\param
keine
\author _HP_
\version beta - 04.04.2007 - _HP_\n
first implementation
\version beta - 05.04.2007 - _HP_\n
Auswerung der Taster verbessert und Umlaute in den Ausgaben entfernt
************************************************** ***************************/
int main(void)
{
int Light; // Summe des gemessenen Lichtes der Fototransistoren ohne Umgebungslicht
int T1, T2; // Werte der Taster bei zwei aufeinanderfolgenden Messungen
int Threshold = INI_THRESHOLD; // Schwellwert des Lichtes für die Fototransistoren, der eingehalten werden soll.
int Speed = INI_SPEED; // Geschwindikeit der Motoren, wenn der Wert der Fototransistoren vom Sollwert abweicht.
Init();
FrontLED(ON);
MotorSpeed(Speed,0); // Regelung erfolgt nur über das linke Rad
MotorDir(BREAK,BREAK);
for(;;) {
// Ändern der Parameter mit den Tastern:
T1 = PollSwitch();
T2 = PollSwitch();
if (T1 && T2 && T1 == T2)
{
// Motor stoppen wenn Werte geänder werden
StatusLED(RED);
MotorSpeed(0,0);
// Taster linke Seite ändert die Hell/Dunkel-Schwelle...
if (T1 == K1) Threshold++;
if (T1 == K2) Threshold--;
if (T1 == K3)
{
SerPrint("\r\nBitte einen Wert fuer THRESHOLD eingeben: ");
Threshold = ReadInt();
}
if (T1 == (K1 + K2)) //K1 und K2 gedückt
{
SerPrint("\r\nAutokalibration von THRESHOLD!");
Threshold = ReadSensor();
}
if (Threshold < 0) Threshold = 0;
if (Threshold > 1023) Threshold = 1023;
// Taster rechte Seite ändert die Geschwindigkeit...
if (T1 == K4)
{
SerPrint("\r\nBitte einen Wert fuer SPEED eingeben: ");
Speed = ReadInt();
}
if (T1 == K5) Speed--;
if (T1 == K6) Speed++;
if (Speed < 0) Speed = 0;
if (Speed > 255) Speed = 255;
// Ausgabe der aktuellen Werte
SerPrint("\r\nTHESHOLD: ");
PrintInt(Threshold);
SerPrint(" SPEED: ");
PrintInt(Speed);
SerPrint("\r\n");
Msleep(200);
StatusLED(GREEN);
MotorSpeed(Speed,0);
}
// Licht an den Liniensensoren messen
Light = ReadSensor();
// Fahre rückwärts, wenn es zu dunkel und vorwärts wenn es zu hell wird.
if (Light < Threshold) MotorDir(RWD,BREAK);
else MotorDir(FWD,BREAK);
}
return 0;
}