int bcr_scanIrregularLines(unsigned char num, unsigned int spacing)
{
/* maxlength = 100 means that it will drive max. 100 ticks
* to find the first bar! Later in the code it will contain
* the regular space between the bars. */
unsigned int whiteLength = 200;
if (spacing < whiteLength) { spacing = whiteLength; } /* default buffer */
// unsigned int blackLength = 200;
lineCounter = 0;
/* assume we stand on white (or not completely dark) */
currentState = BCR_DIMMING_STATE;
EncoderSet(0,0);
MotorSpeed(READ_SPEED, READ_SPEED);
while(1) {
switch (currentState)
{
case BCR_DIMMING_STATE:
EncoderSet(0,0);
while (bcr_getGradient() >= -1*M_TOL*tolFactor) {
if ( encoder[LEFT] > (whiteLength+spacing) ) {
MotorDir(BREAK, BREAK);
MotorSpeed(0,0);
return lineCounter;
}
}
if (lineCounter >= 1) { whiteLength = encoder[LEFT]; }
currentState = BCR_BRIGHTEN_STATE;
lineCounter += 1;
// util_intToBeep(1);
// MotorSpeed(READ_SPEED, READ_SPEED);
break;
case BCR_BRIGHTEN_STATE:
EncoderSet(0,0);
while (bcr_getGradient() <= M_TOL*tolFactor) {
// if ( encoder[LEFT] > (2*whiteLength+spacing) ) {
// MotorDir(BREAK, BREAK);
// MotorSpeed(0,0);
// return lineCounter;
// }
}
currentState = BCR_DIMMING_STATE;
break;
}
/* enough lines scanned? -> stop */
if (num>0 && lineCounter >= num) {
MotorDir(BREAK, BREAK);
// MotorDir(RWD, RWD);
GoTurn(-5, 0, READ_SPEED);
break;
}
} // end switch
MotorSpeed(0,0);
return lineCounter;
}
/*!
\brief
Faehrt eine bestimmte Strecke mit einer bestimmten Geschwindigkeit.
(Autor: stochri)\n
ODER\n
Dreht um einen bestimmten Winkel mit einer bestimmten Geschwindigkeit.
(Autor: stochri)\n
Benutzt die Odometrie Sensoren im Interrupt Betrieb.\n
Vor dem ersten Aufruf muss deshalb EncoderInit() aufgerufen werden.
\param[in]
distance Distanz in mm (- rueckwaerts, + = vorwaerts)\n
Bei 0 wird degree fuer einen Turn benutzt.
\param[in]
degree Winkel (- rechts, + links)
\param[in]
speed Geschwindigkeit (Wertebereich 0...255)
\return
nichts
\see
MACRO Go() und MACRO Turn() koennen weiterhin aufgerufen werden um bestehenden
Programmcode nicht umschreiben zu muessen.
\see
In der globale Variable encoder, werden die Hell-/Dunkelwechsel der\n
Encoderscheiben im Interruptbetrieb gezaehlt.
\par Hinweis:
Die Berechnung der zu fahrenden Ticks beruht auf der Annahme, dass die\n
Anzahl der schwarzen Teilstuecke und die Raddurchmesser wie bei stochri sind.\n
(Sternthaler) Vermutung, dass der Hersteller unterschiedlich grosse Raeder\n
ausgeliefert hat, da die Berechnung in dieser Form bei Sternthaler nicht\n
funktioniert.
\par Beispiel:
(Nur zur Demonstration der Parameter/Returnwerte)
\code
// Laesst den Asuro ein Quadrat von 200 mm fahren,
// bei einer Geschwindigkeit von 150.
EncoderInit ();
for (i = 0; i < 4; i++)
{
GoTurn (200, 0, 150); // entspricht Go (200, 150)
GoTurn ( 0, 90, 150); // entspricht Turn (90, 150)
}
\endcode
*****************************************************************************/
void GoTurn (
int distance,
int degree,
int speed)
{
unsigned long enc_count;
int tot_count = 0;
int diff = 0;
int l_speed = speed, r_speed = speed;
/* stop the motors until the direction is set */
MotorSpeed (0, 0);
/* if distance is NOT zero, then take this value to go ... */
if (distance != 0)
{
/* calculate tics from mm */
enc_count = abs (distance) * 10000L;
enc_count /= MY_GO_ENC_COUNT_VALUE;
if (distance < 0)
MotorDir (RWD, RWD);
else
MotorDir (FWD, FWD);
}
/* ... else take the value degree for a turn */
else
{
/* calculate tics from degree */
enc_count = abs (degree) * MY_TURN_ENC_COUNT_VALUE;
enc_count /= 360L;
if (degree < 0)
MotorDir (RWD, FWD);
else
MotorDir (FWD, RWD);
}
/* reset encoder */
EncoderSet (0, 0);
/* now start the machine */
MotorSpeed (l_speed, r_speed);
while (tot_count < enc_count)
{
tot_count += encoder [LEFT];
diff = encoder [LEFT] - encoder [RIGHT];
if (diff > 0)
{ /* Left faster than right */
if ((l_speed > speed) || (r_speed > 244))
l_speed -= 10;
else
r_speed += 10;
}
if (diff < 0)
{ /* Right faster than left */
if ((r_speed > speed) || (l_speed > 244))
r_speed -= 10;
else
l_speed += 10;
}
/* reset encoder */
EncoderSet (0, 0);
MotorSpeed (l_speed, r_speed);
Msleep (1);
}
MotorDir (BREAK, BREAK);
Msleep (200);
}
