/*!
\brief
LCD Daten schreiben
\param data auszugebende Date
*/
void SetDataLCD(unsigned char data)
{
unsigned char dataPins; // Pin Compatibility
// Set First Nibble Data to DataPins on PCF8574
dataPins &= 0x00;
dataPins |= ((data & 0x80) >> 7) << LD7;
dataPins |= ((data & 0x40) >> 6) << LD6;
dataPins |= ((data & 0x20) >> 5) << LD5;
dataPins |= ((data & 0x10) >> 4) << LD4;
SetIOLCD(OFF, LCD_D4 | LCD_D5 | LCD_D6 | LCD_D7); // Clear old LCD Data (Bit[7..4])
SetIOLCD(ON, dataPins); // Strobe High Nibble Command
SetIOLCD(ON, LCD_EN); // Enable ON
Msleep(1);
SetIOLCD(OFF, LCD_EN); // Enable OFF
// Set Second Nibble Data to DataPins on PCF8574
dataPins &= 0x00;
dataPins |= ((data & 0x08) >> 3) << LD7;
dataPins |= ((data & 0x04) >> 2) << LD6;
dataPins |= ((data & 0x02) >> 1) << LD5;
dataPins |= ((data & 0x01) >> 0) << LD4;
SetIOLCD(OFF, LCD_D4 | LCD_D5 | LCD_D6 | LCD_D7); // Clear old LCD Data (Bit[7..4])
SetIOLCD(ON, dataPins); // Strobe Low Nibble Command
SetIOLCD(ON, LCD_EN); // Enable ON
Msleep(1);
SetIOLCD(OFF, LCD_EN); // Enable OFF
Msleep(1); // Wait LCD Busy
}
enum follow_result follow_wall(enum dir dir) {
if (switched && has_hit_wall()) {
switched = false;
// We hit a wall. First, reverse.
SetMotorPower(-BASE_SPEED * 2, -BASE_SPEED * 2);
Msleep(100);
// Continue in direction `dir`.
if (dir == DIR_Left)
SetMotorPower(-BASE_SPEED * 3 / 2, BASE_SPEED * 3 / 2);
else if (dir == DIR_Right)
SetMotorPower(BASE_SPEED * 3 / 2, -BASE_SPEED * 3 / 2);
Msleep(180);
return HIT_WALL;
}
if (dir == DIR_Left)
SetMotorPower(BASE_SPEED * 4 / 3, BASE_SPEED);
else if (dir == DIR_Right)
SetMotorPower(BASE_SPEED, BASE_SPEED * 3 / 2);
return NO_WALL;
}
int main (void) {
unsigned int data[2];
Init();
while(1) {
LineData(data);
PrintInt(data[LEFT]);
SerPrint("\n");
PrintInt(data[RIGHT]);
SerPrint("\n\n\n");
Msleep(500);
}
}
/*!
\brief
LCD Initialisierung
*/
void InitLCD(void)
{
unsigned char init[] = LCD_INIT;
unsigned char i = 0;
SetIOLCD(OFF, LCD_EN); // Start LCD Control, EN=0
Msleep(1); // Wait LCD Ready
// Initialize LCD
CommandLCD( LCD_8BIT | (LCD_8BIT >> 4) );
CommandLCD( LCD_8BIT | (LCD_4BIT >> 4) );
while (init[i] != 0x00)
{
CommandLCD(init[i]);
i++;
}
CommandLCD( LCD_DISPLAYON ); // Display on/off Control (Entry Display,Cursor off,Cursor not Blink)
CommandLCD( LCD_INCREASE ); // Entry Mode Set (I/D=1 Increment,S=0 Cursor Shift)
CommandLCD( LCD_CLEAR ); // Clear Display
CommandLCD( LCD_HOME ); // Home Cursor
Msleep(1); // Wait Initial Complete
}
void bcr_initBarcodeReader(void) {
EncoderInit();
MotorDir(FWD,FWD);
FrontLED(ON);
lineCounter = 0;
rawCounter = 0;
latestLineData = 0;
unsigned int tmp[2];
util_getStableLineData(tmp);
tolFactor = getToleranceValue(tmp[LEFT] + tmp[RIGHT]);
int i;
util_pauseInterrupts();
for(i = 0; i < (M_RAW_DATAPOINTS * M_WINDOW_SIZE); i++) {
measureDataPoint();
Msleep(5);
}
ts_init();
ts_addFunction(&measureDataPoint, M_SAMPLING_FREQ);
sei();
}
bool has_hit_wall() {
uint8_t t1, t2;
t1 = PollSwitch(); Msleep(10); t2 = PollSwitch();
// Ignore K6 while driving.
return t1 && t1 == t2 && t1 & ~K6 && t1 < 64;
}
int main()
{
unsigned char ctl, engineLeft, engineRight;
// do ASURO initialization
Init();
// initialize control value
ctl = 100;
// loop forever
while (1)
{
// generate new speed levels for left and right engine
engineLeft = (unsigned char)randomValue(ctl);
engineRight = (unsigned char)randomValue(engineLeft);
// go straight forward when ctl is between 0 and 31
if (ctl <32)
{
MotorDir(FWD,FWD);
MotorSpeed(engineLeft,engineLeft);
}
// go a curve in forward direction in case that ctl is between 32 and 63
else if (ctl <64)
{
MotorDir(FWD,FWD);
MotorSpeed(engineLeft,engineRight);
}
// cycle right when ctl between 64 and 127
else if (ctl <128)
{
MotorDir(FWD, RWD);
MotorSpeed(engineLeft,engineRight);
}
// cycle left when ctl is between 128 and 191
else if (ctl <192)
{
MotorDir(RWD, FWD);
MotorSpeed(engineLeft,engineRight);
}
// go straight back when ctl is between 192 and 223
else if (ctl <224)
{
MotorDir(RWD,RWD);
MotorSpeed(engineRight,engineRight);
}
// go straight back in case that ctl is between 224 and 255
else
{
MotorDir(RWD,RWD);
MotorSpeed(engineRight,engineRight);
}
// drive for the number of milliseconds specified by five-fold ctl
Msleep(5*ctl);
// generate a new control word
ctl = (unsigned char)randomValue(engineRight);
}
return -1;
}
/*!
\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);
}
