Kollisions Test C

ASURO Kollisions Test mittels Abfrage der Tastsensoren.

Solange der ASURO auf keine Hindernis trifft:

  • fährt er geradeaus
  • macht die grüne Status LED an und die Bremslichter aus,

Trifft er links auf ein Hindernis (Tasten K4..6):

  • stoppt er,
  • macht das linke Bremslicht an und die Status LED aus,
  • fährt für 1 Sekunde eine Linkskurve zurück.

Trifft er rechts auf ein Hindernis (Tasten K1..3):

  • stoppt er,
  • macht das rechte Bremslicht an und die Status LED aus,
  • fährt für 1 Sekunde eine Rechtskurve zurück.

Programm

Der Programmcode aller Beispiel- und Testprogramme kann im Download Bereich heruntergeladen werden, bzw. befinden sich im Examples Ordner der Asuro Lib unter KollisionTest.

/**************************************************************************
 *
 * File Name:   kollisiontest.c
 * Project  :   ASURO
 *
 * Description: Kollisionstest mit Hilfe der Tastensensoren
 *
 * Ver.     Date         Author               Comments
 * -------  ----------   -----------------    -----------------------------
 * 1.0      10.09.2005   Peter                initial build
 * 1.1      08.01.2006   Peter                2x PollSwitch + Vergleich,
                                              anstelle 8x PollSwitch
 *
 * benoetigt die modifizierte Asuro Bibliothek 'asuro.c'
 * von waste, stochri und andun. Zu finden bei www.roboternetz.de
 */
/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   any later version.                                                    *
 ***************************************************************************/

#include "asuro.h"

/* Um eventuelle Unterschiede zwischen linken und rechtem Motor auszugleichen
 * definieren wir 2 Werte für fullspedd links und rechts
 */
#define FULL_L 250    /* Fullspeed linker Motot */
#define FULL_R 220    /* Fullspeed rechter Motor */

/* Motor vorwärts */
void MotorFwd(void)
{
  MotorDir(FWD,FWD);
  MotorSpeed(FULL_L,FULL_R);
}

/* Motor rückwärts */
void MotorRwd(void)
{
  MotorDir(RWD,RWD);
  MotorSpeed(FULL_L,FULL_R);
}

/* Motor rückwärts Links */
void MotorRwdL(void)
{
  MotorDir(RWD,RWD);
  MotorSpeed(FULL_L,0);
}

/* Motor rückwärts Rechts */
void MotorRwdR(void)
{
  MotorDir(RWD,RWD);
  MotorSpeed(0, FULL_R);
}

/* Motor stop */
void MotorStop(void)
{
  MotorSpeed(0,0);
}

int main(void)
{
  unsigned char t1, t2;

  Init();
  while(1)
  {
    t1 = PollSwitch();
    t2 = PollSwitch();
    if(t1 == 0 && t2 == 0)         /* keine Taste */
    {
      MotorFwd();          /* vorwärts fahren */
      FrontLED(ON);
      BackLED(OFF,OFF);
    }
    else if (t1 && t2 && t1 == t2)
    {
      MotorStop();
      if(t1 & 0x07) /* Tasten links gedrückt? */
      {
        MotorRwdL();       /* Rückwärtskurve links fahren */
        FrontLED(OFF);
        BackLED(ON,OFF);
      }
      if (t1 & 0x38) /* Tasten rechts gedrückt? */
      {
        MotorRwdR();       /* Rückwärtskurve rechts fahren */
        FrontLED(OFF);
        BackLED(OFF,ON);
      }
      Msleep(1000);        /* 1 Sekunde fahren */
    }
  }
  return 0;
}
[$[Get Code]]