Ups, schon nach 20 Uhr. Ende !!
Hier mal mein Programm:
Code:
#include "asuro.h" // bitte die neue Datei benutzen
// asuro.h und asuro.c vom 31.03.05
#define aus MotorDir(BREAK,BREAK)
#define go {MotorSpeed(120,120);MotorDir(FWD,FWD);}
#define backturn { MotorSpeed(150,250);MotorDir(RWD,RWD);Msleep(1000);}
#define vorkurz {MotorSpeed(200,200);MotorDir(FWD,FWD);Msleep(500);aus;Msleep(200);}
#define vorlang {MotorSpeed(200,200);MotorDir(FWD,FWD);Msleep(1500);}
#define rueckganzkurz {MotorSpeed(200,200);MotorDir(RWD,RWD);Msleep(200);aus;Msleep(200);}
#define links90grad {MotorSpeed(150,150);MotorDir(FWD,RWD);Msleep(430);aus;Msleep(200);}
#define rechts90grad {MotorSpeed(150,150);MotorDir(RWD,FWD);Msleep(430);aus;Msleep(200);}
/*************************************************************************************
*************************************************************************************/
void umfahrbecher()
{
rueckganzkurz;
links90grad;
vorkurz;
rechts90grad;
vorkurz;
vorkurz;
rechts90grad;
vorkurz;
rechts90grad;
aus;
}
int speed()
{
int rightspeed,leftspeed;
int leftold,rightold;
Encoder_Set(0,0); // reset encoder
leftold=encoder[LEFT];
rightold=encoder[RIGHT];
Msleep(300);
leftspeed=encoder[LEFT]-leftold;
rightspeed=encoder[RIGHT]-rightold;
//SerWrite(" speed Left,Right ",19);
//PrintInt(leftspeed);
//PrintInt(rightspeed);
return leftspeed+rightspeed;
}
unsigned int uiBuffer[2];
int main(void)
{
int n,v,nullspeed;
int nullwert=21;
Init();
Encoder_Init();
LineData(uiBuffer);
nullwert=uiBuffer[0];
StatusLED(GREEN);
while(uiBuffer[0]<(nullwert+20))
{
LineData(uiBuffer);
};
StatusLED(OFF);
SerWrite("\n\rASURO OK\n\r",8);
go;
Msleep(500);
nullspeed=speed()-6;
SerWrite("\n\r nullspeed ",13);
PrintInt(nullspeed);
while(1)
{
v=speed();
SerWrite("\n\r speed ",9);
PrintInt(v);
if(v<nullspeed)
{
umfahrbecher();
Msleep(3000);
vorlang;
aus;
Msleep(3000);
umfahrbecher();
Msleep(3000);
go;
Msleep(500);
while(uiBuffer[0]<(nullwert+20))LineData(uiBuffer);
}
}
return 0;
}
Leider ohne Linienfolger, den hätte ich schon fast fertig ...
Gruss,
stochri
Lesezeichen