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