#include <asuro.h>gehe zum Quellcode dieser Datei
Funktionen | |
| void | FollowLine (void) |
| int | main (void) |
Variablen | |
| unsigned char | speed |
| int | speedLeft |
| int | speedRight |
| unsigned int | lineData [2] |
| int | x |
| int | xalt |
| int | don |
| int | doff |
| int | kp |
| int | kd |
| int | ki |
| int | yp |
| int | yd |
| int | yi |
| int | drest |
| int | y |
| int | y2 |
| int | isum |
| void FollowLine | ( | void | ) |
Definiert in Zeile 19 der Datei irobot.c.
00020 { 00021 unsigned char leftDir = FWD, rightDir = FWD; 00022 LineData(lineData); // Messung mit LED ON 00023 don = (lineData[0] - lineData[1]); 00024 x = don - doff; // Regelabweichung 00025 isum += x; 00026 if (isum > 16000) isum =16000; //Begrenzung um Überlauf zu vermeiden 00027 if (isum < -16000) isum =-16000; 00028 yi = isum/625 * ki; //I-Anteil berechnen 00029 yd = (x - xalt)*kd; // D-Anteil berechnen und mit 00030 yd += drest; // nicht berücksichtigtem Rest addieren 00031 if (yd > 255) drest = yd - 255; // merke Rest 00032 else if (yd < -255) drest = yd + 255; 00033 else drest = 0; 00034 if (isum > 15000) BackLED(OFF,ON); // nur zur Diagnostik 00035 else if (isum < -15000) BackLED(ON,OFF); 00036 else BackLED(OFF,OFF); 00037 yp = x*kp; // P-Anteil berechnen 00038 y = yp + yi + yd; // Gesamtkorrektur 00039 y2 = y/2; // Aufteilung auf beide Motoren 00040 xalt = x; // x merken 00041 speedLeft = speedRight = speed; 00042 MotorDir(FWD,FWD); 00043 if ( y > 0) { // nach rechts 00044 StatusLED(GREEN); 00045 speedLeft = speed + y2; // links beschleunigen 00046 if (speedLeft > 255) { 00047 speedLeft = 255; // falls Begrenzung 00048 y2 = speedLeft - speed; // dann Rest rechts berücksichtigen 00049 } 00050 y = y - y2; 00051 speedRight = speed - y; // rechts abbremsen 00052 if (speedRight < 0) { 00053 speedRight = 0; 00054 } 00055 } 00056 if ( y < 0) { // nach links 00057 StatusLED(RED); 00058 speedRight = speed - y2; // rechts beschleunigen !!!was speed - y2!! 00059 if (speedRight > 255) { 00060 speedRight = 255; // falls Begrenzung 00061 y2 = speed - speedRight; // dann Rest links berücksichtigen !!was speed - speedRight!! 00062 } 00063 y = y - y2; 00064 speedLeft = speed + y; // links abbremsen !! was speed + y!!! 00065 if (speedLeft < 0) { 00066 speedLeft = 0; 00067 } 00068 } 00069 leftDir = rightDir = FWD; 00070 if (speedLeft < 20) leftDir = BREAK; // richtig bremsen 00071 if (speedRight < 20) rightDir = BREAK; 00072 MotorDir(leftDir,rightDir); 00073 MotorSpeed(abs(speedLeft),abs(speedRight)); 00074 }
| int main | ( | void | ) |
Definiert in Zeile 76 der Datei irobot.c.
00077 { 00078 unsigned char sw; 00079 Init(); 00080 MotorDir(FWD,FWD); 00081 StatusLED(GREEN); 00082 speed = 150; 00083 kp = 3; ki = 10; kd = 70; // Regler Parameter kd enthält bereits Division durch dt 00084 drest=0; 00085 isum=0; 00086 xalt=0; 00087 sw=0; 00088 /* 00089 sw = PollSwitch(); 00090 if (sw & 0x01) 00091 {ki=20;} 00092 if (sw & 0x02) 00093 {speed = 200;} 00094 if (sw & 0x04) 00095 speed = 100; 00096 if (sw & 0x08) 00097 kd = 35; 00098 if (sw & 0x10) 00099 kd = 70; 00100 if (sw & 0x20) 00101 kd = 140; 00102 */ 00103 FrontLED(OFF); 00104 Msleep(50); 00105 LineData(lineData); // Messung mit LED OFF 00106 doff = (lineData[0] - lineData[1]); // zur Kompensation des Umgebungslicht 00107 FrontLED(ON); 00108 Msleep(100); 00109 speedLeft = speed; 00110 speedRight = speed+25; 00111 while(1){ 00112 FollowLine(); 00113 } 00114 return 0; 00115 }
| int speedRight |
1.5.1-p1