Code:
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.
Zitieren
Lesezeichen