hallo,
es ist schon erstaunlich mit was allem man in berührung kommt, wenn man "nur" eine ladestation finden will. IR, induktives laden, oszilloskop, jede menge anderer elektronik und software. Gut so. In diesem zusammenhang taucht jetzt die linienerkennung auf...
Der RP6 ist ja nicht "von natur aus" dafür gemacht, es gibt auch relativ wenig dazu im forum zu lesen - wenn ich da an den asuro denke :-) Da waren viele unterwegs - stochri, waste, sterntaler, radbruch und, und und...
ich hab da ein beispiel gefunden, einfach irre:
ich will es um gottes willen nicht nachbauen, bzw. für den RP6 umschreiben - dazu wäre ich garnicht in der lage. Aber wenn man bedenkt wieviel hirnschmalz dahintersteckt....Code:/*******************************************************************************
*
* Description: Asuro Linienverfolgung mit PID-Regler
* Version 1: Korrektur auf beide Motoren verteilt
* Autor: Waste 26.8.05
*
*****************************************************************************/
#include "asuro.h"
#include <stdlib.h>
unsigned char speed;
int speedLeft,speedRight;
unsigned int lineData[2];
int x, xalt, don, doff, kp, kd, ki, yp, yd, yi, drest, y, y2, isum;
void FollowLine (void)
{
unsigned char leftDir = FWD, rightDir = FWD;
FrontLED(OFF);
LineData(lineData); // Messung mit LED OFF
doff = (lineData[0] - lineData[1]); // zur Kompensation des Umgebungslicht
FrontLED(ON);
LineData(lineData); // Messung mit LED ON
don = (lineData[0] - lineData[1]);
x = don - doff; // Regelabweichung
isum += x;
if (isum > 16000) isum =16000; //Begrenzung um Überlauf zu vermeiden
if (isum < -16000) isum =-16000;
yi = isum/625 * ki; //I-Anteil berechnen
yd = (x - xalt)*kd; // D-Anteil berechnen und mit
yd += drest; // nicht berücksichtigtem Rest addieren
if (yd > 255) drest = yd - 255; // merke Rest
else if (yd < -255) drest = yd + 255;
else drest = 0;
if (isum > 15000) BackLED(OFF,ON); // nur zur Diagnostik
else if (isum < -15000) BackLED(ON,OFF);
else BackLED(OFF,OFF);
yp = x*kp; // P-Anteil berechnen
y = yp + yi + yd; // Gesamtkorrektur
y2 = y/2; // Aufteilung auf beide Motoren
xalt = x; // x merken
speedLeft = speedRight = speed;
MotorDir(FWD,FWD);
if ( y > 0) { // nach rechts
StatusLED(GREEN);
speedLeft = speed + y2; // links beschleunigen
if (speedLeft > 255) {
speedLeft = 255; // falls Begrenzung
y2 = speedLeft - speed; // dann Rest rechts berücksichtigen
}
y = y - y2;
speedRight = speed - y; // rechts abbremsen
if (speedRight < 0) {
speedRight = 0;
}
}
if ( y < 0) { // nach links
StatusLED(RED);
speedRight = speed - y2; // rechts beschleunigen
if (speedRight > 255) {
speedRight = 255; // falls Begrenzung
y2 = speed - speedRight; // dann Rest links berücksichtigen
}
y = y - y2;
speedLeft = speed + y; // links abbremsen
if (speedLeft < 0) {
speedLeft = 0;
}
}
leftDir = rightDir = FWD;
if (speedLeft < 20) leftDir = BREAK; // richtig bremsen
if (speedRight < 20) rightDir = BREAK;
MotorDir(leftDir,rightDir);
MotorSpeed(abs(speedLeft),abs(speedRight));
}
int main(void)
{
unsigned char sw;
Init();
MotorDir(FWD,FWD);
StatusLED(GREEN);
speed = 150;
kp = 3; ki = 10; kd = 70; // Regler Parameter kd enthält bereits Division durch dt
sw = PollSwitch();
if (sw & 0x01)
{ki=20;}
if (sw & 0x02)
{speed = 200;}
if (sw & 0x04)
speed = 100;
if (sw & 0x08)
kd = 35;
if (sw & 0x10)
kd = 70;
if (sw & 0x20)
kd = 140;
FrontLED(ON);
LineData(lineData);
speedLeft = speedRight = speed;
while(1){
FollowLine();
}
return 0;
}
aber zur realität wieder zurück. Ich habe ein viel einfacheres beispiel angefangen, das linienfolgemodul muss ja genutzt werden!
das hier gibt nur die werte der Vishay-sensoren aus. Trotz der enormen speed von 20links/20rechts geht das aber so schnell, dass ich die zeile mit dem move befehl lieber auskommentiert habe und schiebe und drehe von hand um zu erkennen, was da passiert:
das hier geht schon weiter. mit zwei aufeinanderfolgenden if-abfragen der lfs-werte, deren vergleich - letztendlich kommt es ja "nur" drauf an, die differenz möglichst klein zu halten (abgestuft wäre natürlich besser!)....Code:while(true)
{
// moveAtSpeed(20, 20);
setCursorPosLCD(0, 0);
writeStringLCD(" li mi re ");
setCursorPosLCD(1, 0);
lfs_l = getLFS(CH_LFS_L);
writeIntegerLCD(lfs_l, DEC);
setCursorPosLCD(1, 8);
lfs_m = getLFS(CH_LFS_M);
writeIntegerLCD(lfs_m, DEC);
setCursorPosLCD(1, 16);
lfs_r = getLFS(CH_LFS_R);
writeIntegerLCD(lfs_r, DEC);
mSleep(500);
setCursorPosLCD(2, 0);
lfs_l = getLFS(CH_LFS_L);
writeIntegerLCD(lfs_l, DEC);
setCursorPosLCD(2, 8);
lfs_m = getLFS(CH_LFS_M);
writeIntegerLCD(lfs_m, DEC);
setCursorPosLCD(2, 16);
lfs_r = getLFS(CH_LFS_R);
writeIntegerLCD(lfs_r, DEC);
mSleep(500);
setCursorPosLCD(3, 0);
lfs_l = getLFS(CH_LFS_L);
writeIntegerLCD(lfs_l, DEC);
setCursorPosLCD(3, 8);
lfs_m = getLFS(CH_LFS_M);
writeIntegerLCD(lfs_m, DEC);
setCursorPosLCD(3, 16);
lfs_r = getLFS(CH_LFS_R);
writeIntegerLCD(lfs_r, DEC);
mSleep(500);
clearLCD();
setStopwatch1(0);
}
ich bin nicht der typ, der für sich allein herumtüftelt, ich brauche eine "umgebung" und austausch, es gibt ja hier ein paar, die das liniensuchboard der multi IO haben, hätte da jemand lust (und zeit)?Code:while(true)
{
if(getStopwatch3() > 500) // 0.5s
{
moveAtSpeed(20, 20);
lfs_l = getLFS(CH_LFS_L);
lfs_m = getLFS(CH_LFS_M);
lfs_r = getLFS(CH_LFS_R);
clearLCD();
// for (i = 0; i < 10; i++)
// {
if(lfs_r > lfs_l)
{
lfs_l = getLFS(CH_LFS_L);
lfs_m = getLFS(CH_LFS_M);
lfs_r = getLFS(CH_LFS_R);
if(lfs_r > lfs_l)
{
moveAtSpeed(00, 40);
setbumperLEDR(1);
// mSleep(100); // Left bumper LED on
setbumperLEDL(0);
}
}
if(lfs_l > lfs_r)
{
lfs_l = getLFS(CH_LFS_L);
lfs_m = getLFS(CH_LFS_M);
lfs_r = getLFS(CH_LFS_R);
if(lfs_l > lfs_r)
{
moveAtSpeed(40, 00);
setbumperLEDL(1); // right bumper LED on
// mSleep(100);
setbumperLEDR(0);
}
}
// }
setStopwatch3(0);
}
}
in dem zweiten beispiel frage ich z.b. auch den wert des mittleren sensors ab, benutze aber für das abfahren der linie nur die beiden äußeren. Würde sicher sinn machen den mittleren auch einzubinden, hat das z.b. schon jemand gemacht? Wie wäre die überlegung für die realisierung?
