00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include <asuro.h>
00013
00014 unsigned char speed;
00015 int speedLeft,speedRight;
00016 unsigned int lineData[2];
00017 int x, xalt, don, doff, kp, kd, ki, yp, yd, yi, drest, y, y2, isum;
00018
00019 void FollowLine (void)
00020 {
00021 unsigned char leftDir = FWD, rightDir = FWD;
00022 LineData(lineData);
00023 don = (lineData[0] - lineData[1]);
00024 x = don - doff;
00025 isum += x;
00026 if (isum > 16000) isum =16000;
00027 if (isum < -16000) isum =-16000;
00028 yi = isum/625 * ki;
00029 yd = (x - xalt)*kd;
00030 yd += drest;
00031 if (yd > 255) drest = yd - 255;
00032 else if (yd < -255) drest = yd + 255;
00033 else drest = 0;
00034 if (isum > 15000) BackLED(OFF,ON);
00035 else if (isum < -15000) BackLED(ON,OFF);
00036 else BackLED(OFF,OFF);
00037 yp = x*kp;
00038 y = yp + yi + yd;
00039 y2 = y/2;
00040 xalt = x;
00041 speedLeft = speedRight = speed;
00042 MotorDir(FWD,FWD);
00043 if ( y > 0) {
00044 StatusLED(GREEN);
00045 speedLeft = speed + y2;
00046 if (speedLeft > 255) {
00047 speedLeft = 255;
00048 y2 = speedLeft - speed;
00049 }
00050 y = y - y2;
00051 speedRight = speed - y;
00052 if (speedRight < 0) {
00053 speedRight = 0;
00054 }
00055 }
00056 if ( y < 0) {
00057 StatusLED(RED);
00058 speedRight = speed - y2;
00059 if (speedRight > 255) {
00060 speedRight = 255;
00061 y2 = speed - speedRight;
00062 }
00063 y = y - y2;
00064 speedLeft = speed + y;
00065 if (speedLeft < 0) {
00066 speedLeft = 0;
00067 }
00068 }
00069 leftDir = rightDir = FWD;
00070 if (speedLeft < 20) leftDir = BREAK;
00071 if (speedRight < 20) rightDir = BREAK;
00072 MotorDir(leftDir,rightDir);
00073 MotorSpeed(abs(speedLeft),abs(speedRight));
00074 }
00075
00076 int main(void)
00077 {
00078 unsigned char sw;
00079 Init();
00080 MotorDir(FWD,FWD);
00081 StatusLED(GREEN);
00082 speed = 150;
00083 kp = 3; ki = 10; kd = 70;
00084 drest=0;
00085 isum=0;
00086 xalt=0;
00087 sw=0;
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103 FrontLED(OFF);
00104 Msleep(50);
00105 LineData(lineData);
00106 doff = (lineData[0] - lineData[1]);
00107 FrontLED(ON);
00108 Msleep(100);
00109 speedLeft = speed;
00110 speedRight = speed+25;
00111 while(1){
00112 FollowLine();
00113 }
00114 return 0;
00115 }
00116