NQC: RCX controlled lawn mower robot

Discussion specific to projects ideas and support.
Post Reply
HaWe
Posts: 2500
Joined: 04 Nov 2014, 19:00

NQC: RCX controlled lawn mower robot

Post by HaWe »

here an old project (probably 7 years old or even older):
a lawn mower robot controlled by an RCX, programmed by NQC.

it has got about a dozen of touch sensors and 6 Sharp GP2D12 IR distance sensors integrated into the bumper for obstacle detection and 2 photo transistors underneath for reflexion detection.
The functionality has been developed and tested by a small Lego model ("Mini Tobi") and then was tranferred to the big bot "Robi Tobi".

The motor commands are transmitted to the heavy duty industry motors by H-bridges and several relais.

for photos see here:
http://www.mindstormsforum.de/viewtopic.php?f=70&t=6892

avoiding obstacles:



avoiding "forbidden" underground:



this is the code (NQC, subsumption architecture):

Code: Select all

//Rasenmaeher
//"Case-sensitive" Version
//sowohl für Lego-Kleinmodell als auch das "große Original"

//-----------Compiler----------//
#define and &&      // da freut sich der Pascal-Programmierer ;-)
#define or ||
#define begin {
#define endif }

#define Maeher (OUT_B)
#define Aus    Float
#define An     OnFwd

//-----------2-Motoren-Antrieb----------//
#define CommNone -1         // Bewegung undefiniert
#define BewegStopp 0        // alle Mot. stoppbzw. float

#define BewegVor 1          // beide Mot. vor
#define BewegRueck 2        // beide Mot. zurück

#define BewegLiDreh  3      // Mot. gegenläufig links
#define BewegReDreh  4      // Mot. gegenläufig rechts

#define BewegLiBogen 5      // Mot. li steht, rechts dreht
#define BewegReBogen 6      // Mot. re steht, links dreht

#define BewegLiRueBogen 7   // Mot. li dreht rü, rechts steht
#define BewegReRueBogen 8   // Mot. re dreht rü, links steht

#define li 3
#define re 4



//----------- Widerstandswerte der Touch-Sensoren----------//
int RawBeide;
int RawLinks;
int RawRechts;
int RawMin;
int deltaRaw = 10;                 // Schwankungsbreite der Touch-Raw-Werte

//----------- Manöverzeiten ----------//
int Wendezeit ;
int PlusFahrzeit ;
int RueckFahrzeit ;
int T;

//-----------Licht-Sensoren----------//
int deltaLicht= 60;           // Schwankungsbreite des Lichtwerts (Rasen-Grün)

//-----------Programmsteuerung----------//
int BumpTimout = 100;         // Zeit des Vergessens...
int MaxThreadTime = 120;
// -----------------------------------------------------------------------

int vCruise;                  // Verhalten ohne Störung => geradeaus !

task cruise ()
{
    vCruise=BewegVor;         // aktiviert sich als unterste Ebene von alleine

    PlaySound (SOUND_CLICK);
    An (Maeher);              // schaltet Relais für Mäh-Motor
    while (true) {
       Wait(20);
    }
}

// -----------------------------------------------------------------------

int vAusweichen;              // Verhalten bei Lichtwechsel (Untergrund)
int LichtMw_1, LichtMw_3;     // Durchschnitts-Lichtwert für Rasen-Grün
int bumpcount;
int LastDir;

task ausweichen()
{

      while (true)
      {
         if ((abs(SENSOR_1 -LichtMw_1) > deltaLicht) and   // linker und
             (abs(SENSOR_3 -LichtMw_3) > deltaLicht))      // rechter Untergr.
         begin                                             // verändert
            PlaySound (SOUND_DOWN);
            ClearTimer (0);
            T=Timer(0);
            vAusweichen=BewegStopp;     Wait(20);

            if (LastDir==li)                                // in entgegenges.
               vAusweichen= BewegReDreh;                    // Richtung drehen
            else  vAusweichen= BewegLiDreh;
            Wait(Wendezeit*3/2);
            vAusweichen=BewegStopp;  Wait(20);

            do {                                            // bis Untergr. OK !
               vAusweichen=BewegRueck;  Wait(Wendezeit*2);
               if (LastDir==li)  vAusweichen= BewegReRueBogen;
               else    vAusweichen= BewegLiRueBogen;
               Wait(Wendezeit/5);
            }
            while ((abs(SENSOR_1 -LichtMw_1) > deltaLicht) or
                   (abs(SENSOR_3 -LichtMw_3) > deltaLicht)) ;

            vAusweichen=BewegStopp;   Wait(20);
            vAusweichen=BewegRueck;  Wait(Wendezeit*3);
            vAusweichen=BewegStopp;   Wait(20);

            if (LastDir==li)  vAusweichen= BewegLiDreh;
            else              vAusweichen= BewegReDreh;
            Wait(Wendezeit*2);
            vAusweichen=BewegStopp;  Wait(20);
            vAusweichen=CommNone;
         endif

         else
         if  (abs(SENSOR_3 -LichtMw_3) > deltaLicht)  // rechter Untergr.
         begin                                        // verändert
            PlaySound (SOUND_CLICK);                  // => nach links drehen
            LastDir=li;
            vAusweichen=BewegLiBogen; Wait(Wendezeit*2);
            vAusweichen=CommNone;

         endif

         else
         if (abs(SENSOR_1 -LichtMw_1) > deltaLicht)   // linker Untergr.
         begin                                        // verändert
            PlaySound (SOUND_CLICK);
            LastDir=re;                               // => nach rechts drehen
            vAusweichen=BewegReBogen; Wait(Wendezeit*3);
            vAusweichen=CommNone;
         endif
     }
}

// -----------------------------------------------------------------------


int vWenden;

task wenden ()                // Verhalten beim Anstoßen vorn (Stoßstangen)
{                             // und bei IR-Hindernis-Sensor-Signalen

     while (true){
        if ((SENSOR_2 >(RawBeide-deltaRaw)) and (SENSOR_2 <(RawBeide+deltaRaw)))
        begin
           bumpcount += 1;
           PlaySound (SOUND_LOW_BEEP);
           ClearTimer (0);

           vWenden=BewegStopp;    Wait(20);
           vWenden=BewegRueck;    Wait(RueckFahrzeit);
           vWenden=BewegReDreh;   Wait(Wendezeit*3);
           if (bumpcount>4)       Wait(PlusFahrzeit);    //rausdrehen
        endif
        else
        if ((SENSOR_2 >(RawLinks-deltaRaw)) and (SENSOR_2 <(RawLinks+deltaRaw)))
        begin
           PlaySound (SOUND_DOWN);
           bumpcount += 1;
           ClearTimer (0);
           vWenden=BewegStopp;     Wait(20);
           vWenden=BewegRueck;     Wait(RueckFahrzeit);
           vWenden=BewegReDreh;    Wait(Wendezeit*2);
           if (bumpcount>4)        Wait(PlusFahrzeit);  //rausdrehen
        endif
        else
        if((SENSOR_2 >(RawRechts-deltaRaw)) and (SENSOR_2 <(RawRechts+deltaRaw)))
        begin
           PlaySound (SOUND_FAST_UP);
           bumpcount += 1;
           ClearTimer (0);
           vWenden=BewegStopp;     Wait(20);
           vWenden=BewegRueck;     Wait(RueckFahrzeit);
           vWenden=BewegLiDreh;    Wait(Wendezeit);
           if (bumpcount>4)        Wait(PlusFahrzeit);   //rausdrehen
        endif

        vWenden=CommNone;
   }
}

// -----------------------------------------------------------------------

int vRueckAbbr;

task rueckAbbr ()
{                                         // Verhalten beim Rückwärts-Anstoßen
    int wohin;
    while (true){
      if (SENSOR_2 < RawMin)
      begin
         PlaySound (SOUND_DOWN);
         //Aus (Maeher);                  // Mähmotor aus
         stop wenden;
         vWenden=CommNone;

         vRueckAbbr=BewegStopp;           // erst abstoppen und dann
         vRueckAbbr=BewegVor;   Wait(2);  // hintere Stoßstange entlasten
         wohin=Random(2);
         if (wohin==0)  vRueckAbbr=BewegReDreh;
         else   vRueckAbbr=BewegLiDreh;
         Wait(Wendezeit/2);
         vRueckAbbr=BewegStopp;
         Wait(MaxThreadTime);    // warten, bis alle anderen Tasks durch sind

         vRueckAbbr=CommNone;             // zurück zur Tagesordnung!
         An (Maeher);
         ClearTimer (0);
         vCruise=BewegVor;
         start wenden;
      endif

    }
}

// -----------------------------------------------------------------------

int vNotStopp;

task NotStopp ()
{                                    // Verhalten bei Notstopp
     int x;

     while (true){
        x= BatteryLevel();
        if ((x<7500) or
           ((SENSOR_1< RawMin) and (SENSOR_2 <RawMin) and (SENSOR_3 <RawMin)))
        begin                        // NOTSTOPP-BEDINGUNG: zuwenig Saft
                                     // u/o alle Sensoren gleichz. gedrückt!
             InitCommNone();         // alle Verhaltensvariablen auf CommNone
             
             vNotStopp=BewegStopp;   // MotorCommand erhält BewegStopp
                                     // über arbitrate ()
             Aus Maeher;
             StopAllTasks ();
             SetUserDisplay (x,0);
             PlaySound (SOUND_DOWN);
             Wait(20);
             PlaySound (SOUND_LOW_BEEP);      // Das war's ...!
        endif
     }
}


// -----------------------------------------------------------------------



task deepThought ()            // vergessen oder nicht ... ?
{
     while (true){
        T=Timer(0);            // Timer-Wert (Stoppuhr) abfragen
        if (T > BumpTimout)    // nach TimeOut: Bump-Gedächtnis löschen
        //begin
           bumpcount=0;
        //endif
     }
}

//  ===================================================================

int MotorCommand;
task arbitrate ()
{         // Hier werden die Verhaltens-Prioritäten festgelegt!
    while(true) {
       if (vCruise != CommNone)      MotorCommand = vCruise;   // niedrigste Pr.
       if (vAusweichen != CommNone)  MotorCommand = vAusweichen;
       if (vWenden != CommNone)      MotorCommand = vWenden;
       if (vRueckAbbr != CommNone)   MotorCommand = vRueckAbbr;
       if (vNotStopp != CommNone)    MotorCommand = vNotStopp; // höchste Pr.


       MotorControl ();     // MotorCommand erhält seinen Wert Event-gesteuert
                            // vom entsprechenden aktiven Task
   }                        // MotorControl übernimmt dann die
                            // einfachsten Motorbefehle
}

sub MotorControl () {
   if      (MotorCommand == BewegVor)         OnFwd (OUT_A + OUT_C);
   else if (MotorCommand == BewegRueck)       OnRev (OUT_A + OUT_C);
   else if (MotorCommand == BewegLiDreh)     {OnRev (OUT_A); OnFwd (OUT_C);}
   else if (MotorCommand == BewegReDreh)     {OnRev (OUT_C); OnFwd (OUT_A);}
   else if (MotorCommand == BewegLiBogen)    {Aus   (OUT_A); OnFwd (OUT_C);}
   else if (MotorCommand == BewegReBogen)    {Aus   (OUT_C); OnFwd (OUT_A);}
   else if (MotorCommand == BewegStopp)       Aus   (OUT_A + OUT_C);
   else if (MotorCommand == BewegLiRueBogen) {Aus   (OUT_C); OnRev (OUT_A);}
   else if (MotorCommand == BewegReRueBogen) {Aus   (OUT_A); OnRev (OUT_C);}
}

//  ===================================================================

task main ()
{
   init();
   initLego();
   InitCommNone();
   vNotStopp=CommNone;      // Notstopp gesondert auf "undefiniert"

   Calibrate();

   start cruise;
   start wenden;
   start ausweichen;
   start rueckAbbr;
   start NotStopp;
   start deepThought;

   start arbitrate;
}

//  ===================================================================

sub init ()
{
   SetTxPower (TX_POWER_HI);
   ClearMessage();

   SetSensor(SENSOR_2, SENSOR_TOUCH);        // Stoßstangen
   SetSensorMode(SENSOR_2, SENSOR_MODE_RAW);

   SetSensor(SENSOR_1, SENSOR_TOUCH);        // Licht links
   SetSensorMode(SENSOR_1, SENSOR_MODE_RAW);

   SetSensor(SENSOR_3, SENSOR_TOUCH);        // Licht rechts
   SetSensorMode(SENSOR_3, SENSOR_MODE_RAW);

   SelectDisplay(DISPLAY_SENSOR_3);          // Digitalanzeige des Sensorwerts

   bumpcount=0;
   ClearTimer(0);
                           // (Standard) Großmodell: Init für Mikroschalter
   RawLinks=  275;         // 3,6 kOhm Mikroschalter: 275 -  Lego: 295
   RawRechts= 134;         // 1,5 kOhm Mikroschalter: 134 -  Lego: 178
   RawBeide=  100;         // beide    Mikroschalter: 130 -  Lego: 130
   RawMin=     70;         // 470 Ohm = Innenwid. Legoschalter

   Wendezeit= 100 ;        // Standard: Init Fahrzeiten für Groß-Modell
   PlusFahrzeit= 200 ;
   RueckFahrzeit= 400 ;    // Fertig: Init für Großmodell

   LichtMw_1=0;
   LichtMw_3=0;
}

//  ===================================================================

sub initLego ()                      // (Lego:) Init für Lego-Sensoren,
{                                    // wenn Sensor-Stoßstange beim Start
   PlaySound (SOUND_DOUBLE_BEEP);    // nach Doppel-Beep gedrückt wird
   Wait(100);
   if (SENSOR_2 < 800)
   begin                             // (Lego:) andere Schalter-RAW-Werte
        do {
            PlaySound (SOUND_CLICK); // warten, bis Stoßstange
            Wait(10);                // wieder losgelassen wird
        }   while (SENSOR_2 < 800);

        RawLinks=  295;         // 3,6 kOhm Mikroschalter: 275 - Lego: 295
        RawRechts= 178;         // 1,5 kOhm Mikroschalter: 134 - Lego: 178
        RawBeide=  130;         // beide    Mikroschalter: 100 - Lego: 130

        Wendezeit= 30 ;         // Lego:  kleinere Manöverzeiten
        PlusFahrzeit= 60 ;
        RueckFahrzeit= 120;

        PlaySound (SOUND_DOWN); // Fertig: Init für Lego-Kleinmodell
   endif
}

//  ===================================================================

sub InitCommNone ()             // alle Verhaltensweisen auf "undefiniert"
{
   vCruise=CommNone;
   vWenden=CommNone;
   vAusweichen=CommNone;
   vRueckAbbr=CommNone;
}


//  ===================================================================

sub Calibrate()
{
   LichtMw_1= SENSOR_1;
   LichtMw_3= SENSOR_3;

   ClearTimer(0);

   OnFwd (OUT_A + OUT_B + OUT_C);
   do {
      T=Timer(0);
      LichtMw_1=(LichtMw_1+SENSOR_1)/2;
      LichtMw_3=(LichtMw_3+SENSOR_3)/2;
      Wait(20);
      }
   while (T<40);  // 4 sek.

   Aus (OUT_A + OUT_C);   Wait(50);

   ClearTimer(0);
   OnRev (OUT_A + OUT_C);
   do {
      T=Timer(0);
      LichtMw_1=(LichtMw_1+SENSOR_1)/2;
      LichtMw_3=(LichtMw_3+SENSOR_3)/2;
      Wait(20);
      }
   while (T<40);  // 4 sek.

   Aus (OUT_A + OUT_C);
   ClearTimer(0);
}
Last edited by HaWe on 09 Jun 2013, 15:40, edited 1 time in total.
Top
h-g-t
Posts: 552
Joined: 07 Jan 2011, 08:59
Location: Albania

Re: NQC: RCX controlled lawn mower robot

Post by h-g-t »

Pretty impressive, especially using the RCX!
A sophistical rhetorician, inebriated with the exuberance of his own verbosity, and gifted with an egotistical imagination that can at all times command an interminable and inconsistent series of arguments to malign an opponent and to glorify himself.
Top
mightor
Site Admin
Posts: 1079
Joined: 25 Sep 2010, 15:02
Location: Rotterdam, Netherlands
Contact:
Contact mightor

Re: NQC: RCX controlled lawn mower robot

Post by mightor »

That's pretty cool :) That robot didn't end up killing anyone, did it? I am not sure you'd tell us if it had, haha. I am sure many a teenager will want one of these, so they can get out of mowing their parents' lawn.

- Xander
| My Blog: I'd Rather Be Building Robots (http://botbench.com)
| RobotC 3rd Party Driver Suite: (http://rdpartyrobotcdr.sourceforge.net)
| Some people, when confronted with a problem, think, "I know, I'll use threads,"
| and then two they hav erpoblesms. (@nedbat)
Top
HaWe
Posts: 2500
Joined: 04 Nov 2014, 19:00

Re: NQC: RCX controlled lawn mower robot

Post by HaWe »

That robot didn't end up killing anyone, did it?
well, actually not "anyone"...^^
Top
Post Reply

Return to “Mindstorms Projects”

Who is online

Users browsing this forum: No registered users and 5 guests