00001 #include "asuro.h" 00002 00003 #define SPEED 0xA0 00004 00005 #define INIT_COUNT 0x40 00006 #define INIT_DIVISOR 6 00007 00008 int speedLeft,speedRight; 00009 unsigned int lineData[INIT_COUNT][2]; 00010 unsigned int lineSData[2]; 00011 unsigned int lineMDData[2]; 00012 unsigned int lineMLData[2]; 00013 int ADOffset; 00014 00015 int main(void) 00016 { 00017 unsigned char j; 00018 int i; 00019 00020 Init(); 00021 00022 SerPrint("\r\nLineTest\r\n"); 00023 00024 lineSData[0] = 0; 00025 lineSData[1] = 0; 00026 FrontLED(OFF); 00027 for (j = 0; j < INIT_COUNT; j++) 00028 { 00029 LineData(lineData[j]); 00030 lineSData[0] += lineData[j][0]; 00031 lineSData[1] += lineData[j][1]; 00032 } 00033 lineMDData[0] = (lineSData[0] >> INIT_DIVISOR); 00034 lineMDData[1] = (lineSData[1] >> INIT_DIVISOR); 00035 00036 SerPrint("LED OFF\r\n"); 00037 PrintInt(lineSData[0]); 00038 SerPrint(" "); 00039 PrintInt(lineSData[1]); 00040 SerPrint("\r\n"); 00041 00042 PrintInt(lineMDData[0]); 00043 SerPrint(" "); 00044 PrintInt(lineMDData[1]); 00045 SerPrint("\r\n\r\n"); 00046 for (j = 0; j < INIT_COUNT; j++) 00047 { 00048 PrintInt(lineData[j][0]); 00049 SerPrint(" "); 00050 PrintInt(lineData[j][1]); 00051 SerPrint("\r\n"); 00052 } 00053 00054 lineSData[0] = 0; 00055 lineSData[1] = 0; 00056 FrontLED(ON); 00057 for (j = 0; j < INIT_COUNT; j++) 00058 { 00059 LineData(lineData[j]); 00060 lineSData[0] += lineData[j][0]; 00061 lineSData[1] += lineData[j][1]; 00062 } 00063 lineMLData[0] = (lineSData[0] >> INIT_DIVISOR); 00064 lineMLData[1] = (lineSData[1] >> INIT_DIVISOR); 00065 00066 SerPrint("LED ON\r\n"); 00067 PrintInt(lineSData[0]); 00068 SerPrint(" "); 00069 PrintInt(lineSData[1]); 00070 SerPrint("\r\n"); 00071 00072 PrintInt(lineMLData[0]); 00073 SerPrint(" "); 00074 PrintInt(lineMLData[1]); 00075 SerPrint("\r\n\r\n"); 00076 for (j = 0; j < INIT_COUNT; j++) 00077 { 00078 PrintInt(lineData[j][0]); 00079 SerPrint(" "); 00080 PrintInt(lineData[j][1]); 00081 SerPrint("\r\n"); 00082 } 00083 /* 00084 ADOffset = lineData[0] - lineData[1]; 00085 speedLeft = speedRight = SPEED; 00086 00087 for(;;) { 00088 LineData(lineData); 00089 i = (lineData[0] - lineData[1]) - ADOffset; 00090 if ( i > 4) { //Line is left 00091 StatusLED(GREEN); 00092 //speedLeft = SPEED + i; 00093 //if (speedLeft > 0xFE) speedLeft = 0xFF; 00094 speedRight = SPEED - 2 * i; 00095 if (speedRight < 0x01) speedRight = 0x00; 00096 } 00097 else if ( i < -4) { //Line is right 00098 StatusLED(RED); 00099 //speedRight = SPEED - i; 00100 //if (speedRight > 0xFE) speedRight = 0xFF; 00101 speedLeft = SPEED + 2 * i; 00102 if (speedLeft < 0x01) speedLeft = 0x00; 00103 } 00104 else { 00105 StatusLED(OFF); 00106 //acc = 0; 00107 speedLeft = speedRight = SPEED; 00108 } 00109 MotorSpeed(speedLeft,speedRight); 00110 } 00111 */ 00112 return 0; 00113 } 00114