Hallo

Ich kann das Programm zwar nicht kompilieren, war aber so frei mal ein paar Stellen zu kommentieren. Ist noch nicht vollständig, aber ich muss zur Arbeit:

Code:
#include "asuro.h"

int DriveCircle( int angel, unsigned char Dir)
   {   Encoder_Init(0,0);


      int segments_left;
      int segments_right;
      unsigned char MotorDr_Left = FWD;
      unsigned char MotorDr_Right = RWD;
      int MotorSpdLeft = 120;
      int MotorSpdRight = 120;



      if (angel == 45)
      {
      segments_left = 13;
      segments_right = 13;
      }
      else if (angel == 90)
      {
      segments_left = 27;
      segments_right = 27;
      }
      else if (angel == 180)
      {
      segments_left = 53;
      segments_right = 53;
      }
      else if (angel == 360)
      {
      segments_left = 107;
      segments_right = 107;
      }
      else
      {
      StatusLED(RED);   // Statusleuchtdiode Rot schalten
      }



      Encoder_Set(0,0);



      if(Dir == RIGHT)
      {
         MotorDr_Left = FWD;
         MotorDr_Right = RWD;
      }



      if(Dir == LEFT)
      {
         MotorDr_Left = RWD;
         MotorDr_Right = FWD;
      }

      while((encoder[LEFT] < segments_left)||(encoder[RIGHT] < segments_right))
      {
         MotorDir(MotorDr_Left,MotorDr_Right);
         MotorSpeed(MotorSpdLeft,MotorSpdRight);

         if(encoder[LEFT] >= segments_left)
         {
            MotorSpdLeft = 160;
         }

         if(encoder[RIGHT] >= segments_right)
         {
            MotorSpdRight = 160;
         }

         if(encoder[LEFT]&&encoder[RIGHT] >= segments_left)
         {
            MotorSpeed(BREAK, BREAK);
            Msleep(300);
            MotorSpeed(0,0);   // Kollision! Sofort anhalten
         }


      }
      return 0;

   }


int DriveLength( int length, unsigned char MotorDr, int MotorSpd)
   {
      Encoder_Init();
      int diff;
      float cycles;
      int segments_left;
      int segments_right;
      int MotorSpdLeft = MotorSpd;
      int MotorSpdRight = MotorSpd;
      unsigned char schalter;

// In der folgenden Rechnung waehre es guenstiger erst zu multiplizieren
// und dann die Division durchzufuehren um den Rundungsfehler der Division
// nicht noch zu multiplizieren.
// Wenn die Werte um den Faktor 10 vergroeßert werden, kann man auch
// auf float verzichten:
// segments_left = segments_right = lenght*40/12.2 bzw. lenght*400/122

      cycles = length / 12,2;      // Umdrehungen des linken Reifens = zu fahrende Strecke links / Reifen-Umfangs (Umfang = 12,2 cm !!!)
      segments_left = cycles * 40;   // Anzahl der zu fahrenden Segmente = Umdrehungen * Anzahl der Segmente pro Umdrehung (40)
      segments_right = cycles * 40;
      Encoder_Set(0,0);

// Besser auf die Summe der Werte beider Seiten pruefen. Wenn eine Seite trotz
// Regelung hinterherhinkt, faehrt der asuro sonst weiter bis die langsame Seite
// auch den Wert erreicht hat:
// while ((encoder[LEFT]+encoder[RIGHT]) < ((segments_left+segments_right))

      while((encoder[LEFT]+ < segments_left)||(encoder[RIGHT] < segments_right))
      {
         MotorDir(MotorDr,MotorDr);
         MotorSpeed(MotorSpdLeft,MotorSpdRight);
         schalter = PollSwitch(); // PollSwitch() ohne Mehrfachabfragen !!!
         PrintInt(schalter); // funktioniert in den wenigsten Faellen !!!

         if(( schalter > 0)&&(MotorDr == FWD))
         {
         MotorSpeed(0,0);
         MotorDir(BREAK,BREAK);
         Msleep(300);

         return encoder[LEFT];
         }

         diff = encoder[LEFT]-encoder[RIGHT];

// Auch hier koennte man die Rechnug umstellen um die Rundungsfehler durch die
// Integerrechnung zu minimieren:
// (MotorSpdRight * 25 / 1000) bzw. (MotorSpdLeft * 25 / 1000)
// Ein "unsigned int" waere guenstiger, weil die Speeds nie negativ werden
// Und auch wieder bei der Gesamtstrecke auf Summe beider Seiten pruefen

        if ((diff > 0)&&(encoder[LEFT] < segments_left)&&(encoder[RIGHT] < segments_right))
         {
            MotorSpdRight = MotorSpdRight + (MotorSpdRight / 100 * 2.5);
            MotorSpdLeft = MotorSpd;
         }
         else if ((diff < 0)&&(encoder[LEFT] < segments_left)&&(encoder[RIGHT] < segments_right))
         {
            MotorSpdLeft = MotorSpdLeft + (MotorSpdLeft / 100 * 2.5);
            MotorSpdRight = MotorSpd;
         }
         else if ((diff == 0)&&(encoder[LEFT] < segments_left)&&(encoder[RIGHT] < segments_right))
         {
            MotorSpdLeft = MotorSpd;
            MotorSpdRight = MotorSpd;
         }

// Wenn die schnellere Seite stoppt, dreht der asuro beim Anhalten, wenn die andere Seite alleine weiterfaehrt
         if(encoder[LEFT] >= segments_left)
         {
            MotorSpdLeft = 0;
         }

         if(encoder[RIGHT] >= segments_right)
         {
            MotorSpdRight = 0;
         }


      }
      MotorSpeed(0,0);




   return encoder[LEFT];
   }



int main(void)
{
      Init();

   StatusLED(GREEN); // die ist immer gruen nach dem Init();
   int blink = 0;
   float cycles;
   int way = 0;
   int segments;
   unsigned char endschalt;


   while(way < 1000)
   {

      segments = DriveLength(100, FWD, 120);
      cycles = segments / 40;
      way = way + (cycles * 12.2);


      endschalt = PollSwitch(); // PollSwitch() ohne Mehrfachabfragen !!!
      PrintInt(endschalt); // funktioniert in den wenigsten Faellen !!!

      if (( endschalt == 2)||( endschalt == 16)||( endschalt == 18 ))
         {
         DriveLength(10, RWD, 120);
         DriveCircle(90, RIGHT);
         }
         else if ( endschalt == 32)
         {
         DriveCircle(45, RIGHT);
         }
         else if ( endschalt == 1)
         {
         DriveCircle(45, LEFT);
         }



   }




   while(1)
   {
      if (blink == 0)
      {
         blink = 1;
         StatusLED(OFF);
         Msleep(300);
      }
      else if(blink == 1) // wenn nicht 0, dann muss es 1 sein
      {
         blink = 0;
         StatusLED(GREEN);
         Msleep(300);
      }
   }



    return 0;
}
Vielleicht hilft es euch weiter.

Viel Glück heute am 13.

Gruß

mic