Fertiges Labyrinthprogramm!!!!

**G3tzR@zor** · 20.05.2009, 13:41

Ich bin so langsam wirklich am Verzweifeln. Ich habe nun Versucht die Pollswitch abfrage so einzubauen.

Code:

#include "asuro.h"
#include "myasuro.h"

#define TRIGGERLEVEL 830
#define HYSTERESIS 10
#define LOW 0
#define HIGH 1


volatile unsigned int i;

void Fahren(void)                               /* Vorwärts fahren */
{
    unsigned char taste1, taste2;
    unsigned int data[2];
    signed int status[2]={0,0};
    signed int difference=0;
    MotorDir(FWD, FWD);


    while((taste1<1)  && (taste1==taste2)) {
         taste1 = PollSwitch();
         taste2 = PollSwitch();
        // Helligkeitswerte der Lichtschranken auslesen
        OdometrieData(data);
        // Wechsel linker Sensor von niedrig auf hoch?
        if ((status[0]==LOW) && (data[0]>TRIGGERLEVEL+HYSTERESIS)) {
            status[0]=HIGH;
            difference++;
        }
        // Wechsel linker Sensor von hoch auf niedrig?
        if ((status[0]==HIGH) && (data[0]<TRIGGERLEVEL-HYSTERESIS)) {
            status[0]=LOW;
            difference++;
        }
        // Wechsel rechter Sensor von niedrig auf hoch?
        if ((status[1]==LOW) && (data[1]>TRIGGERLEVEL+HYSTERESIS)) {
            status[1]=HIGH;
            difference--;
        }
        // Wechsel rechter Sensor von hoch auf niedrig?
        if ((status[1]==HIGH) && (data[1]<TRIGGERLEVEL-HYSTERESIS)) {
            status[1]=LOW;
            difference--;
        }
        // zur Sicherheit: verhindern, dass der Differenzz?ler
        // den erlaubten Wertebereich verl?st
        if (difference<-255) difference=-255;
        if (difference>255) difference=255;

        // Status-LED noch entsprechend der erkannten Segmente
        // aufleuchten lassen, grn fr links, rot fr rechts
        StatusLED(status[0]+status[1]*2);

        // Z?ldifferenz passend auf die Motoren verteilen
        if (difference>0) MotorSpeed(180-difference,180);
        else MotorSpeed(180,180+difference);

}
}

void Anhalten(void)                          /* Anhalten */
{
  StatusLED(RED);
  BackLED(OFF,OFF);
  MotorDir(BREAK,BREAK);
}

void Zurueck(void)                             /* Zurück fahren */
{
  Encoder_Init();
  Go(-10,200);
}


void KurveR(void)                          /* Kurve Rechts 90° */
{
 Encoder_Init();
 Turn(60,200);
}


void KurveL(void)                           /* Kurve rückwärts Rechts */
{
Encoder_Init();
Turn(-50,200);
}

void Drehung(void)                            /*180° Kurve*/
{
Turn(120,200);
}


void Firstcol(void)                        /* Programm Rechtskurve im Parcour 1. Kollision */
{
  Anhalten();
  Msleep(250);

  Zurueck();
  Msleep(15);

  Anhalten();
  Msleep(10);
    
  KurveR();
  Msleep(400);

  Anhalten();
  Msleep(250);
    
  switched = 0;
  StartSwitch ();
}

void Secondcol(void)                     /* Programm Linkskurve im Parcour, 2. Kollision */
{
  Anhalten();
  Msleep(250);

  Zurueck();
  Msleep(10);
    
  Anhalten();
  Msleep(250);

  Drehung();
  Msleep(355);

  Anhalten();
  Msleep(250);
    
  Anhalten();
  Msleep(250);
         
  switched = 0;
  StartSwitch();
} 
 
void Thirdcol(void)                        /* Programm Sackgasse im Parcour, 3. Kollision */
{
  Anhalten();
  Msleep(250);
                
  Zurueck();
  Msleep(10);
                
  KurveL();
  Msleep(310);
                
  Anhalten();
  Msleep(250);
                
  switched = 0;
  StartSwitch();

}

void Disco(void)                        /* PARTYTIME! -15mal blinken/piepsen-*/
{
 
  for(i = 0; i < 16; i++)
  {
   MotorDir(FWD,FWD);
   MotorSpeed(50,50);
   BackLED(OFF,OFF);
   StatusLED(OFF);
   FrontLED(OFF);
   Msleep(200);
   
   MotorDir(BREAK,BREAK);
   BackLED(ON,ON);
   StatusLED(RED);
   FrontLED(ON);
   Msleep(200);
   }

   
}


int main (void)                           /* Hauptprogrammbeginn */
{
  unsigned int data[2];                     /* Speicherplpatz für Helligkeitswert */
  unsigned int Summe;
  Init ();
  switched = 0;                           /* Kollisionserkennung auf 0 setzen */
  StartSwitch ();                        /* Taster-Interrupt aktivieren */
  FrontLED(ON);
  LineData(data);                         /* Speichern von Helligkeitswert in data[0,1] */
  Summe = data[0]+data[1];                  /* Summe der Werte beider Fototransistoren */
 
 
  while (1)
  {
   if (switched == 1)                     /* Wenn 1. Kollision, dann Rechtskurve 90° aus */
    {

     Firstcol();
    
     for(i = 0; i < 25000; i++)            /* Setze Zeitschlaufe für 2. Kollision */
     {
      if (switched == 1)                  /* Wenn 2. Kollision, dann Drehung 180° */
        {
          Secondcol();
         
         for(i = 0; i < 25000; i++)         /* Setze Zeitschlaufe für dritte Kollision */
          {
            if (switched == 1)            /* Wenn 3. Kollision, dann Linksdrehung 90° */
             {
               Thirdcol();
             }

            else                     /* Keine 3. Kollision, Fahre normal */
             {
                  Fahren();
              }
               
               
          }   
         
        }   
       
      else                           /* Keine 2. Kollision, Fahre normal */
       {
         Fahren();
       }
       

     }
                                                                          
      

   }



   else                              /* gar keine Kollision, Fahre normal, Stopp bei Linie */
 {
     LineData(data);
     if(data[0]+data[1]+200 > Summe)         /* Untergrundhelligkeitsdifferenz feststellen */
   {
       Fahren();
     }
     else
     {
       Disco();                        /* Party gut - Alles gut */
     while(1)
    {}                              /* Endlosschleife - Ende des Programmes */
   }
  }
 }
 return 0;
 }

Nun leuchtet die StatusLED Grün und die LED für die Linensensoren.

Setze ich das eine taster1=PollSwitch() vor die while schleife fährt der Ausuro nur noch Rückwärts bei roter StatusLED und leuchtender LiniensensorLED. Er nimmt auch keine Pollswitch anweisung mehr an.

Code:

#include "asuro.h"
#include "myasuro.h"

#define TRIGGERLEVEL 830
#define HYSTERESIS 10
#define LOW 0
#define HIGH 1


volatile unsigned int i;

void Fahren(void)                               /* Vorwärts fahren */
{
    unsigned char taste1, taste2;
    unsigned int data[2];
    signed int status[2]={0,0};
    signed int difference=0;
    MotorDir(FWD, FWD);
    taste1 = PollSwitch();

    while((taste1<1)  && (taste1==taste2)) {

         taste2 = PollSwitch();
        // Helligkeitswerte der Lichtschranken auslesen
        OdometrieData(data);
        // Wechsel linker Sensor von niedrig auf hoch?
        if ((status[0]==LOW) && (data[0]>TRIGGERLEVEL+HYSTERESIS)) {
            status[0]=HIGH;
            difference++;
        }
        // Wechsel linker Sensor von hoch auf niedrig?
        if ((status[0]==HIGH) && (data[0]<TRIGGERLEVEL-HYSTERESIS)) {
            status[0]=LOW;
            difference++;
        }
        // Wechsel rechter Sensor von niedrig auf hoch?
        if ((status[1]==LOW) && (data[1]>TRIGGERLEVEL+HYSTERESIS)) {
            status[1]=HIGH;
            difference--;
        }
        // Wechsel rechter Sensor von hoch auf niedrig?
        if ((status[1]==HIGH) && (data[1]<TRIGGERLEVEL-HYSTERESIS)) {
            status[1]=LOW;
            difference--;
        }
        // zur Sicherheit: verhindern, dass der Differenzz?ler
        // den erlaubten Wertebereich verl?st
        if (difference<-255) difference=-255;
        if (difference>255) difference=255;

        // Status-LED noch entsprechend der erkannten Segmente
        // aufleuchten lassen, grn fr links, rot fr rechts
        StatusLED(status[0]+status[1]*2);

        // Z?ldifferenz passend auf die Motoren verteilen
        if (difference>0) MotorSpeed(180-difference,180);
        else MotorSpeed(180,180+difference);

}
}

void Anhalten(void)                          /* Anhalten */
{
  StatusLED(RED);
  BackLED(OFF,OFF);
  MotorDir(BREAK,BREAK);
}

void Zurueck(void)                             /* Zurück fahren */
{
  Encoder_Init();
  Go(-10,200);
}


void KurveR(void)                          /* Kurve Rechts 90° */
{
 Encoder_Init();
 Turn(60,200);
}


void KurveL(void)                           /* Kurve rückwärts Rechts */
{
Encoder_Init();
Turn(-50,200);
}

void Drehung(void)                            /*180° Kurve*/
{
Turn(120,200);
}


void Firstcol(void)                        /* Programm Rechtskurve im Parcour 1. Kollision */
{
  Anhalten();
  Msleep(250);

  Zurueck();
  Msleep(15);

  Anhalten();
  Msleep(10);
    
  KurveR();
  Msleep(400);

  Anhalten();
  Msleep(250);
    
  switched = 0;
  StartSwitch ();
}

void Secondcol(void)                     /* Programm Linkskurve im Parcour, 2. Kollision */
{
  Anhalten();
  Msleep(250);

  Zurueck();
  Msleep(10);
    
  Anhalten();
  Msleep(250);

  Drehung();
  Msleep(355);

  Anhalten();
  Msleep(250);
    
  Anhalten();
  Msleep(250);
         
  switched = 0;
  StartSwitch();
} 
 
void Thirdcol(void)                        /* Programm Sackgasse im Parcour, 3. Kollision */
{
  Anhalten();
  Msleep(250);
                
  Zurueck();
  Msleep(10);
                
  KurveL();
  Msleep(310);
                
  Anhalten();
  Msleep(250);
                
  switched = 0;
  StartSwitch();

}

void Disco(void)                        /* PARTYTIME! -15mal blinken/piepsen-*/
{
 
  for(i = 0; i < 16; i++)
  {
   MotorDir(FWD,FWD);
   MotorSpeed(50,50);
   BackLED(OFF,OFF);
   StatusLED(OFF);
   FrontLED(OFF);
   Msleep(200);
   
   MotorDir(BREAK,BREAK);
   BackLED(ON,ON);
   StatusLED(RED);
   FrontLED(ON);
   Msleep(200);
   }

   
}


int main (void)                           /* Hauptprogrammbeginn */
{
  unsigned int data[2];                     /* Speicherplpatz für Helligkeitswert */
  unsigned int Summe;
  Init ();
  switched = 0;                           /* Kollisionserkennung auf 0 setzen */
  StartSwitch ();                        /* Taster-Interrupt aktivieren */
  FrontLED(ON);
  LineData(data);                         /* Speichern von Helligkeitswert in data[0,1] */
  Summe = data[0]+data[1];                  /* Summe der Werte beider Fototransistoren */
 
 
  while (1)
  {
   if (switched == 1)                     /* Wenn 1. Kollision, dann Rechtskurve 90° aus */
    {

     Firstcol();
    
     for(i = 0; i < 25000; i++)            /* Setze Zeitschlaufe für 2. Kollision */
     {
      if (switched == 1)                  /* Wenn 2. Kollision, dann Drehung 180° */
        {
          Secondcol();
         
         for(i = 0; i < 25000; i++)         /* Setze Zeitschlaufe für dritte Kollision */
          {
            if (switched == 1)            /* Wenn 3. Kollision, dann Linksdrehung 90° */
             {
               Thirdcol();
             }

            else                     /* Keine 3. Kollision, Fahre normal */
             {
                  Fahren();
              }
               
               
          }   
         
        }   
       
      else                           /* Keine 2. Kollision, Fahre normal */
       {
         Fahren();
       }
       

     }
                                                                          
      

   }



   else                              /* gar keine Kollision, Fahre normal, Stopp bei Linie */
 {
     LineData(data);
     if(data[0]+data[1]+200 > Summe)         /* Untergrundhelligkeitsdifferenz feststellen */
   {
       Fahren();
     }
     else
     {
       Disco();                        /* Party gut - Alles gut */
     while(1)
    {}                              /* Endlosschleife - Ende des Programmes */
   }
  }
 }
 return 0;
 }

Warum fährt er Rückwärts? Dies ist doch im ganzen Programm nicht vorgesehen, bis auf dieses kurz zurück bei einer Kollision.

Hier mal die beiden Hex files zu den Codes. Vll könnt ihr sie bei eurem Asuro mal testen.

Thema: Fertiges Labyrinthprogramm!!!!

Themen-Optionen

Thema durchsuchen

Anzeige

Hybrid-Darstellung

Lesezeichen

Lesezeichen

Berechtigungen