Liniensensor

[dark_flash01]

Hallo,
ich bastell grade an einem Liniensensor mit 5 LEDs und 6 Phototransistoren.
Bin aber irgendwie nicht sicher ob ich die abstände groß genug gewählt habe.
Was meint ihr? Ausserdem ist die Schaltung so in Ordnung?
Der Roboter soll damit Linien in allen möglichen Winkeln gut erkennen können.
Ist die Anordnung im Halbkreis dazu überhaupt die beste Lösung?
Schonmal im voraus danke!

[ähM_Key]

Du solltest vorher zumindest ungefähr spezifizieren, wie die Linien aussehen können, die du erkennnen willst. Optimale Ergebnisse in Sachen Geschwindigkeit wirst du durch die digitale Auswertung auch nicht erreichen.

MK

[dark_flash01]

Digitale Auswertung? Ich dachte ich klemm die Ausgänge 3 bis 8 der Platine dann an den ADC am Port A meines Mega32.

Die schwarze Linie wird mit Textilband aufgeklebt
und ist 1cm bis 2cm breit. Die schwarze Linie folgt einem
verschlungenen Pfad auf dem Boden. Sie kann Kurven mit
unterschiedlichen Winkelgraden haben. Die Linie hat keine
Kreuzungen.

[ähM_Key]

Hm, aber wenn du analog auslesen kannst/willst dann reicht eine LED und und 2 IR-Transistoren. Denk immer dran, dass du das ganze dannach auch noch auswerten musst!

MK

Guggst du hier (http://web-prokli.homeip.net/robosapiens/videos/spurverfolgung-6-geregelt.MPG) und hier (http://www.kleines-helferlein.com/images/roboturag/03.01.06(6).JPG) (bzw. auch auf unserer HP)

[dark_flash01]

Der ist ja ziemlich schnell. Ich will mit nahezu absoluter Sicherheit keine Linien "überfahren". Deshalb dachte ich an 6 Sensoren.
Könntest du eventuell deinen Programmcode später mal reinstellen?
Konnte auf deiner HP nix finden.
MfG dark_flash01

[ähM_Key]

Hi!

Glaube kaum, dass der dir etwas nützt, aber bitte: :)


program Robo;

{ $BOOTRST $07000} {Reset Jump to $07000}
{ $W+ Warnings} {Warnings off}
{$M+} {end ... on}

Device = mega64, VCC=5;

Import SysTick, SerPort, PWMport1A, PWMport1B;

From System Import Float, Processes;

Define
ProcClock = 8000000; {Hertz}
SysTick = 10; {msec}
StackSize = $0064, iData;
FrameSize = $0064, iData;
Scheduler = iData;
SerPort = 57600, Stop2; {Baud, StopBits|Parity}
RxBuffer = 8, iData;
TxBuffer = 8, iData;
PWMres1 = 8; {bits}
PWMpresc1 = 64;

Implementation

{$IDATA}
{--------------------------------------------------------------}
{ Type Declarations }
type

{--------------------------------------------------------------}
{ Const Declarations }
const
wait : word = 200; //Pause Initialisierung
vmax : float = 255; //Maximal-PWM
vdrive : integer = 128; //Fahr PWM;
vm2 : word = 45; //Maximal-PWM Aufgabe 2
vd2 : word = 10; //PWM Differenz Aufgabe 2 für Lenken
vm3 : word = 64; //Maximal-PWM Aufgabe 3
vd3 : word = 32; //PWM Differenz Aufgabe 3 für Lenken
directMax : word = 200; //Dauer gerade Fahren nach Ball erkannt Aufgabe 3
direct2Max : word = 400; //Dauer gerade Fahren nach Tor erkannt Aufgabe 3

vmess : word = 63; //Mess-PWM für Border-Erkennung

toturn : word = 500; //Maximal Zähler Lenken Aufgabe 2

blockMax : word = 300; //Maximalzeit Dauer Erkennung Blockieren Rad

mi : word = 500; //maximale Mess-Schritte für Border-Erkennung
mu : word = 100; //mind. Abweichung für Border-Erkennung
ma : word = 50; //Offset auf erkannte Border

nRound : byte = 2; //Anzahl Durchschnittswerte Drehzahl - 2^n
nBreak : integer = 30; //Erkennung Abfallende Flanke T-U-Wandler

steps : longint = 1000; //Auflösung Bahnerkennung
isMax : integer = 550; //maximaler is-Werte, Abschneiden, Randerkennung //550
pd : float = 1.5; //Regler Drehzahl //1.5

//////////PID/////
kp : float = 0.35; //P-Anteil //0.35
kd : float = 10; //D-Anteil =ki/ta //10
//ta ~ 1ms

//////////////////

{--------------------------------------------------------------}
{ Var Declarations }
{$EEPROM}
var
RdarkE : word;
LdarkE : word;
RbrightE : word;
LbrightE : word;
borderE : word;

{$IDATA}
var
R[@PortB, 1] : bit; //Rad rechts
L[@PortB, 0] : bit; //Rad links
T1[@PinC, 0] : bit; //Taster 1-3
T2[@PinC, 1] : bit; // 2-3 getauscht!
T3[@PinC, 2] : bit; //
S[@PortC, 3] : bit; //Schussmotor
B[@PinC, 4] : bit; //Balllichtschranke
E[@PinC, 5] : bit; //Endschalter
Led[@PortC, 7] : bit; //Led Spur
X1[@PinD, 2] : bit; //TSOP 1-3
X2[@PinD, 3] : bit; //
X3[@PinD, 4] : bit; //
D1[@PortA, 2] : bit; //Sensor Indikatoren
D2[@PortA, 4] : bit; //
D3[@PortA, 5] : bit; //


AdcChan :byte;
CurrentAdc :word;
CurrentAdcLo[@CurrentAdc]:byte;
CurrentAdcHi[@CurrentAdc+1]:byte;

ada :array[0..3] of longword;
done :array[0..3] of boolean;
spd :array[0..3] of word;
run :array[0..1, 0..1] of word;

Rdark : word;
Ldark : word;
Rbright : word;
Lbright : word;
border : word;
Ris, Lis : integer;
is : integer;
stat : byte;
vr, vl : integer;

menu : byte; //Menu
pref : boolean; //Untermenu Einstellungen
btm : boolean; //back to menu

z : byte;
z2 : word;
//z4 : word;
z3 : float;
//z5 : float;

turn : word; //Zähler Lenken Aufgabe 2

isold : integer;
yp,yd,y,y2 : integer; //Anteile PID
drest : integer; //Rest von D-Anteil

stat3 : byte; //Status Aufgabe 3
direct : word; //Zählwert gerade Fahren Ball gefunden Aufgabe 3
direct2 : word; //Zählwert gerade Fahren Tor gefunden Aufgabe 3
stat0 : byte; //Status Spur gefunden

{--------------------------------------------------------------}
{ functions }

interrupt ADCRdy;
begin
CurrentAdcLo:=ADCL;
CurrentAdcHi:=ADCH;

if AdcChan<2 then
ada[AdcChan]:=longword(CurrentAdc);
else
if CurrentADC>spd[AdcChan] then
spd[AdcChan]:=CurrentADC;
spd[AdcChan-2]:=0;
endif;

if (spd[AdcChan-2]<blockMax) then inc(spd[AdcChan-2]); endif;

if (CurrentADC>1020) or (spd[AdcChan-2]>=blockMax) then
ada[AdcChan]:=44000;
spd[AdcChan]:=0;
endif;

if (integer(spd[AdcChan])-integer(CurrentADC))>nBreak then
if ada[AdcChan]=44000 then
ada[AdcChan]:=4*longword(spd[AdcChan]);
else
ada[AdcChan]:=ada[AdcChan]-(ada[AdcChan] shr nRound);
ada[AdcChan]:=ada[AdcChan]+longword(spd[AdcChan]);
endif;
spd[AdcChan]:=0;
endif;

endif;
done[AdcChan]:=true;

inc(AdcChan);
if AdcChan>3 then AdcChan:=0; endif;

ADMUX:=(ADCChan and $07)or $40;
ADCSRA:=$CF; //F=128 Prescaler
end interrupt_ADCRdy;

procedure initports;
begin
DDRA:= %11111111; //7-Segmentanzeige
PortA:=%00000000;
DDRB:= %01100011; //Motortreiber; 0:V/R rechts; 1:V/R links; 5:PWM 1A rechts; 6: PWM 1B links;
PortB:=%00000000;
DDRC:= %10001000; //0-2:Taster; 3:Schussmotor; 4:Balllichtschranke; 5:Endschalter; 7:Led (Spur)
PortC:=%00100111;
DDRD:= %00000000; //2-4:Tordedektierung
PortD:=%00011100;
DDRF:= %00000000; //analog; 0:IR-Sensor rechts; 1:IR-Sensor links; 2:Drehzahl links; 3: Drehzahl rechts
PortF:=%00000000;
end initports;

procedure display(x: byte);
begin
case x of
0: PortA:=%10110111; |
1: PortA:=%00000110; | // =I
2: PortA:=%01110011; |
3: PortA:=%01010111; |
4: PortA:=%11000110; |
5: PortA:=%11010101; | // =S
6: PortA:=%11110101; |
7: PortA:=%00000111; |
8: PortA:=%11110111; |
9: PortA:=%11010111; |
10: PortA:=%00001000; | // =.
11: PortA:=%10110110; | // =U
12: PortA:=%10110001; | // =C
13: PortA:=%11100110; | // =H
14: PortA:=%10110000; | // =L
15: PortA:=%11110001; | // =E
16: PortA:=%00001110; | // =1.
17: PortA:=%01111011; | // =2.
18: PortA:=%01011111; | // =3.
19: PortA:=%11001110; | // =4.
20: PortA:=%11101110; | // =H.
endcase;
end display;

procedure setupadc;
begin
ada[2]:=0; ada[3]:=0;
spd[2]:=0; spd[3]:=0;
ADMUX:=$40;
ADCSRA:=$CF;
ADCChan:=0;
end setupadc;

procedure init;
begin
PWMPort1A:=0;
PWMPort1B:=0;
setupadc;
menu:=1;
display(menu);
pref:=false;
led:=true;
end init;

function getada(chan :byte) :longword;
begin
repeat
until done[chan]=true;
done[chan]:=false;
return(ada[chan]);
end getada;

procedure copyE;
begin
if RdarkE>RbrightE then
Rbright:=RdarkE;
Lbright:=LdarkE;
Rdark:=RbrightE;
Ldark:=LbrightE;
else
Rdark:=RdarkE;
Ldark:=LdarkE;
Rbright:=RbrightE;
Lbright:=LbrightE;
endif;
border:=borderE;
end copyE;

procedure getRL;
var
Rwas, Lwas :integer;
begin
Rwas:=(integer(getada(0))-integer(Rdark));
Lwas:=(integer(getada(1))-integer(Ldark));

if Rwas<0 then Rwas:=0; endif;
if Lwas<0 then Lwas:=0; endif;

Ris:=integer((longint(Rwas)*steps) DIV longint(Rbright-Rdark));
Lis:=integer((longint(Lwas)*steps) DIV longint(Lbright-Ldark));

if Ris>integer(steps) then Ris:=integer(steps); endif;
if Lis>integer(steps) then Lis:=integer(steps); endif;
end getRL;

procedure show;
begin
copyE;
Writeln(SERout, 'Rdark=' + inttostr(Rdark));
Writeln(SERout, 'Ldark=' + inttostr(Ldark));
Writeln(SERout, 'Rbright=' + inttostr(Rbright));
Writeln(SERout, 'Lbright=' + inttostr(Lbright));
Writeln(SERout, 'border=' + inttostr(border));
Writeln(SERout, ' ');
end show;

procedure darkinit;
begin
display(10);
RdarkE:=word(getada(0));
LdarkE:=word(getada(1));
show;
mdelay(wait);
end darkinit;

procedure brightinit;
begin
display(10);
RbrightE:=word(getada(0));
LbrightE:=word(getada(1));
show;
mdelay(wait);
end brightinit;

procedure scanborder;
var
x :word;
begin
display(10);
L:=1;
R:=0;
PWMPort1A:=255;
PWMPort1B:=255;
mdelay(20);
PWMPort1A:=vmess;
PWMPort1B:=vmess;
copyE;
stat:=0;
for x:=1 to mi do
getRL;
//writeln(serout, 'a' + inttostr(Ris) + 'b' + inttostr(Lis) + 'cdz');
mdelay(1);
if Lis+mu<Ris then stat:=1; endif;
if (Lis>Ris) and (stat=1) then x:=mi; endif;
endfor;
PWMPort1A:=0;
PWMPort1B:=0;
borderE:=word(Ris+Lis)div 2 - ma;
Writeln(SERout, 'border=' + inttostr((Ris+Lis)div 2 - ma) +
' Lis=' + inttostr(integer(Lis)) + ' Ris=' + inttostr(integer(Ris)));
end scanborder;

procedure indi;
begin
D1:=X1;
D2:=X2;
D3:=X3;
end indi;

procedure load;
begin
S:=true;
display(14);
repeat
until E=true;
S:=false;
display(5);
while T2=false do endwhile;
if menu=2 then display(10); endif;
repeat
if menu=2 then indi; endif;
until T2=false;
end load;

procedure shot;
begin
load;
for z:=9 downto 0 do
display(z);
if z=1 then S:=true; endif;
mdelay(725);
endfor;
S:=false;
end shot;

procedure calcIs;
begin
is:=integer(Ris-Lis);

if ((Lis>integer(border)) and (is<0)) or ((Ris>integer(border)) and (is>0)) then stat:=0; endif;

if (Ris<integer(border)) and (is>0) and (stat=0) then stat:=1; endif;
if (Lis<integer(border)) and (is<0) and (stat=0) then stat:=2; endif;

if stat=1 then is:=isMax; endif;
if stat=2 then is:=-isMax; endif;
end calcIs;

procedure pid;
begin
yd:=integer(float(is-isold)*kd); // D-Anteil berechnen und mit
yd:=yd+drest; // nicht berücksichtigtem Rest addieren
if yd>vmax then
drest:=yd-vmax; // merke Rest
else
if yd<-vmax then
drest:=yd+vmax;
else
drest:=0;
endif;
endif;

yp:=integer(float(is)*kp); // P-Anteil berechnen
y:=yp+yd; // Gesamtkorrektur
y2:=y div 2; // Aufteilung auf beide Motoren
isold:=is; // is merken

vl:=vdrive;
vr:=vdrive;

if y>0 then // nach rechts
vl:=vdrive+y2; // links beschleunigen
if vl>vmax then
vl:=vmax; // falls Begrenzung
y2:=vl-vdrive; // dann Rest rechts berücksichtigen
endif;
y:=y-y2;
vr:=vdrive-y; // rechts abbremsen
if vr<0 then vr:=0; endif;
endif;

if y<0 then // nach links
vr:=vdrive-y2; // rechts beschleunigen
if vr>vmax then
vr:=vmax; // falls Begrenzung
y2:=vdrive-vr; // dann Rest links berücksichtigen
endif;
y:=y-y2;
vl:=vdrive+y; // links abbremsen
if vl<0 then vl:=0; endif;
endif;

end pid;

procedure swing;
begin
if (X2=true) {or (S=true)} then
turn:=0;
run[0,0]:=vm2;
run[0,1]:=vm2;
else
inc(turn);

if (turn<=toturn) or (turn>3*toturn) then
run[0,0]:=vm2-vd2;
run[0,1]:=vm2;
if turn>4*toturn then turn:=0; endif;
endif;
if (turn>toturn) and (turn<=3*toturn) then
run[0,0]:=vm2;
run[0,1]:=vm2-vd2;
endif;

endif;
end swing;

procedure menu2;
begin
if B=true then
swing;
getRL;
if (word(Ris)>border) or (word(Lis)>border) then S:=true; endif;
else
run[0,0]:=0;
run[0,1]:=0;
btm:=true;
endif;
end menu2;

procedure iniline;
begin
L:=true;
R:=true;
stat0:=1;
stat3:=0;
end iniline;

{--------------------------------------------------------------}
{ Main Program }
{$IDATA}

begin
initports;
Start_Processes;
init;
repeat
{Menu}
repeat
if T1=false then
inc(menu);
if pref=false then
if menu>5 then menu:=1; endif;
if menu<>5 then display(menu); else display(15); endif; // 15=E
else
if menu>5 then menu:=1; endif;
if menu<>5 then display(15+menu); else display(20); endif; // 20=H
endif;
mdelay(200);
while T1=false do endwhile;
endif;

if T2=false then
if pref=false then
if menu=5 then
pref:=true;
menu:=1;
endif;
else
case menu of
1: darkinit; |
2: brightinit; |
3: scanborder; |
4: shot; |
5: pref:=false;
menu:=1; |
endcase;
endif;
while T2=false do endwhile;
endif;

if pref=false then
if menu<>5 then display(menu); else display(15); endif; // 15=E
else
if menu<>5 then display(15+menu); else display(20); endif; // 20=H.
endif;

until (T2=false) and (pref=false) and (menu<=4);
{Program}
show;
while T2=false do endwhile;

copyE;
L:=true;
R:=true;
btm:=false;
turn:=0;

if (menu=1) or (menu=3) then stat0:=0; endif;

if (menu=2) or (menu=3) then
load;
while T2=false do endwhile;
endif;

if menu=3 then
stat3:=0;
direct:=0;
direct2:=0;
endif;

if menu=4 then
run[0,0]:=vm2;
run[0,1]:=vm2;
endif;

if menu<>1 then display(10); endif;

{Main Routine}
repeat
if (menu=1) or (menu=3) then
getRL;
if stat0=1 then
if (menu=3) then
if (stat3=0) and (B=true) then inc(direct); endif;

if direct>directMax then
if (X3=true) and (X1=false) then stat3:=2; else stat3:=3; endif;
direct:=0;
endif;

if (stat3=2) and (X1=true) and (X3=false) then
stat3:=3;
else
if (stat3=3) and (X3=true) and (X1=false) then stat3:=2; endif;
endif;

if ((stat3=2) or (stat3=3)) and (X2=true) then stat3:=4; endif;

if stat3=4 then
if S=false then
stat3:=5;
else
run[0,0]:=0;
run[0,1]:=0;
endif;
endif;

if stat3=5 then
swing;
inc(direct2);
if direct2>direct2Max then
S:=true;
direct2:=0;
endif;
if (B=false) and (direct2=0) then stat0:=2; endif;
endif;
if (stat3=2) or (stat3=3) then
if stat3=2 then
run[0,0]:=vm3-vd3;
run[0,1]:=vm3;
endif;
if stat3=3 then
run[0,0]:=vm3;
run[0,1]:=vm3-vd3;
endif;
endif;

endif; //menu=3
endif; //stat0=1 (found)

if (stat3<>5) and (E=true) then S:=false; endif;

if stat0=0 then
if (word(Ris)>=border) and (word(Lis)<border) then iniline; endif;
if (word(Lis)>=border) and (word(Ris)<border) then stat0:=3; endif;
endif;

if (stat0=3) and (word(Ris)>=border) and (word(Lis)<border) then stat0:=4; endif;
if (stat0=4) and (word(Lis)>=border) and (word(Ris)<border) then iniline; endif;

if stat0=2 then
if (word(Ris)>=border) then iniline; endif;
endif;

if (stat0=1) and ((menu=1) or ((menu=3) and (stat3=0))) then
calcIs;
pid;
run[0,0]:=word(vl);
run[0,1]:=word(vr);
endif;

if stat0=0 then
run[0,0]:=vmax;
run[0,1]:=vmax;
endif;
if stat0=2 then
//drive back
run[0,0]:=(5*vmax) div 4;
run[0,1]:=vmax div 2;
L:=false;
R:=false;
endif;

if (stat0=3) or (stat0=4) then
run[0,0]:=vmax div 3;
run[0,1]:=vmax div 2;
L:=false;
endif;

endif; //menu=1 or menu=3

if menu=2 then menu2; endif;

if menu<>1 then indi; endif;

//display(stat0);

//run[0,0]:=vm2;
//run[0,1]:=vm2;

for z:=0 to 1 do

z2:=11000 div word( getada(z+2) shr nRound ) ;
if (z2>270) or (z2<15) then z2:=0; endif;

//if z=0 then z4:=z2; endif;

z3:=float(run[0,z])+ pd*(float(run[0, z])-float(z2));
if z3<0 then z3:=0; endif;
if z3>255 then z3:=255; endif;
//if z=0 then z5:=z3; endif;

run[1,z]:=word(z3); //z3
endfor;

//writeln(serout, inttostr(is));
//mdelay(50);

//writeln(serout, 'a' + inttostr(z2) + 'b' + inttostr(integer(z3)) + 'c'
// + inttostr(z4) + 'd' + inttostr(integer(z5)) + 'z');
//mdelay(30);

PWMport1A:=run[1,1];
PWMport1B:=run[1,0];

until (T1=false) or (T2=false) or (T3=false) or (btm=true);


if btm=true then
repeat
until E=true;
S:=false;
endif;

until btm=false;

PortA:=%00000000;
PWMPort1A:=0;
PWMPort1B:=0;
S:=false;

end Robo.



Das interesanteste für dich ist sicherliche die procedure PID, die ich von waste übernommen habe. Aber das Auswerten und Anpassen durch Initialisieren der Sensordaten war auch nicht trivial.

Und wenn du einen 10cm breiten Balken an Sensoren verwenden würdest, wärst du nicht schneller, ohne die Linie zu verlieren. Es kommt sehr auf die Geometrie des Robos drauf an. Ich weiß nicht, ob du dir die DOKU (http://www.kleines-helferlein.com/files/robodokumentation.pdf) [5,7 MB] schon durchgelesen hast, aber der Abstand zw. einem Rad und den Sensoren ist der minimale Radius, den der Roboter fahren kann. (differentiell angetriebener)

MK

[dark_flash01]

danke.
Nochmal zur Anordnung:
sind die vllt zu nahe nebeneinander?
Hat jemand verbessungsvorschläge?
bin für alles dankbar.
MfG flash