00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "asuro.h"
00010
00011 int main(void)
00012 {
00013 int diff,weg;
00014 Init();
00015 EncoderInit();
00016 StatusLED(OFF);
00017 MotorDir(FWD,FWD);
00018 MotorSpeed(175,175);
00019 StartSwitch();
00020 unsigned long endtime=Gettime()+8000;
00021 while (!switched && Gettime()<endtime)
00022 {
00023 diff=encoder[RIGHT]-encoder[LEFT];
00024 MotorSpeed(175+diff,175-diff);
00025 }
00026 weg=encoder[0];
00027 EncoderSet(0,0);
00028 MotorDir(RWD,RWD);
00029 MotorSpeed(200,200);
00030 Msleep(100);
00031 MotorDir(RWD,FWD);
00032 weg-=encoder[0];
00033 EncoderSet(0,0);
00034 while (encoder[1]<90)
00035 {
00036 Msleep(50);
00037 }
00038 MotorDir(FWD,FWD);
00039 EncoderSet(0,0);
00040 while (encoder[0]<weg)
00041 {
00042 diff=encoder[RIGHT]-encoder[LEFT];
00043 MotorSpeed(175+diff,175-diff);
00044 }
00045 MotorSpeed(0,0);
00046 if (weg>600) StatusLED(GREEN);
00047 else if (weg>300) StatusLED(YELLOW);
00048 else StatusLED(RED);
00049 while (1);
00050 return 0;
00051 }