Du kannst auch einfach folgende go-Funtkion verwenden. Als Input parameter wird die Distanz in "mm" angegeben. Funktioniert je nach Bodenbeschaffenheit relativ genau.
Gruss,
stochri
Code:
/***************************************************************************
*
* void Go(int distance, int power = 150)
*
* Go distance in mm. Attention: Encoder_Init() has to be called first.
*
* the driven distance depends a little bit from the floor friction
* limitations: maximum distance +-32m
* possible problems: in full sunlight the encoder sensors may be disturbed
*
* input
* distance: postiv->go forward ; negativ-> go backward
* power: sets motorpower
*
* example:
*
* Encoder_Init();
* Msleep(5000);
* Go(200,150); // move 20cm vorward
* Msleep(2000);
* Go(-200,150); // move 20cm backward
* Msleep(2000);
*
*
* last modification:
* Ver. Date Author Comments
* ------- ---------- -------------- ---------------------------------
* sto1 29.07.2005 stochri motorfunction
* And1 31.07.2005 Andun added speed and Odometrie
* sto1 31.10.2006 stochri distance in mm
* ------- ---------- -------------- ---------------------------------
*
***************************************************************************/
void Go(int distance, int power)
{
uint32_t enc_count;
int tot_count = 0;
int diff = 0;
int l_speed = power, r_speed = power;
// calculation tics/mm
enc_count=abs(distance)*10000L;
enc_count/=19363L;
Encoder_Set(0,0); // reset encoder
MotorSpeed(l_speed,r_speed);
if(distance<0) MotorDir(RWD,RWD);
else MotorDir(FWD,FWD);
while(tot_count<enc_count) {
tot_count += encoder[LEFT];
diff = encoder[LEFT] - encoder[RIGHT];
if (diff > 0) { //Left faster than right
if ((l_speed > power) || (r_speed > 244)) l_speed -= 10;
else r_speed += 10;
}
if (diff < 0) { //Right faster than left
if ((r_speed > power) || (l_speed > 244)) r_speed -= 10;
else l_speed += 10;
}
Encoder_Set(0,0); // reset encoder
MotorSpeed(l_speed,r_speed);
Msleep(1);
}
MotorDir(BREAK,BREAK);
Msleep(200);
}
Lesezeichen