Erstes größeres ASURO-Programm macht nur Probleme

Hallo

Ich habe nun mein erstes "größeres" Asuro-Programm halbwegs fertig gestellt.
Allerdings habe ich einige seltsame Probleme.

Ich stelle das Programm hier einfach mal rein, leider ist es fast nicht kommentiert.

Code:

---

#include "asuro.h"

int DriveCircle( int angel, unsigned char Dir)
        {        Encoder_Init();
       
               
                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);
                }
               
               
               
                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 = 0;
                        }
                       
                        if(encoder[RIGHT] >= segments_right)
                        {
                                MotorSpdRight = 0;
                        }
                       
                        if(encoder[LEFT]&&encoder[RIGHT] >= segments_left)
                        {
                                MotorSpeed(BREAK, BREAK);
                                Msleep(300);
                                MotorSpeed(0,0);
                        }
               
               
                }
                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;
               
                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);
               
                while((encoder[LEFT] < segments_left)||(encoder[RIGHT] < segments_right))
                {
                        MotorDir(MotorDr,MotorDr);
                        MotorSpeed(MotorSpdLeft,MotorSpdRight);
                        schalter = PollSwitch();
                        PrintInt(schalter);
                       
                        if(( schalter > 0)&&(MotorDr == FWD))
                        {
                        MotorSpeed(0,0);
                        MotorDir(BREAK,BREAK);
                        Msleep(300);
                       
                        return encoder[LEFT];
                        }
                       
                        diff = encoder[LEFT]-encoder[RIGHT];
                       
                        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;
                        }
                       
                        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);
        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();
                PrintInt(endschalt);
               
                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)
                {
                        blink = 0;
                        StatusLED(GREEN);
                        Msleep(300);
                }
        }
                       
       
               
    return 0;
}

---

Zuerst wird eine Funktion names "DriveCircle" deklariert. Man kann dort einen Drehwinkel und eine Drehrichtung eingeben. Mann kann den Asuro also z.B. 45° nach rechts oder links drehen lassen.

Die zweite Funktion heißt "DriveLength". Dort kann man eine gewünschte Fahrtstrecke in Zentimeter, die Richtung Vorwärts oder Rückwärts und die Geschwindigkeit von 0 bis 255 eingeben.
Die Funktion "DriveLength" vergleicht die durchgelaufenen Segmente der beiden Odometrie-Sensoren, um eine einigermaßen gerade Fahrt zu gewährleisten.
Außerderm werden die Motoren sofort gestoppt, sobald einer der sechs End-Taster (K1 bis K6) betätigt wird.

Nun wollte ich Folgendes programmieren (Main-Funktion !)
Der Asuro soll eine gesamte Strecke von etwa 10 Metern oder 1000 Zentimetern (way = 1000) zurücklegen.

Stößt er mit den beiden Tastern K1 oder K5 (oder beiden) an ein Hindernis, soll er 10 Zentimeter zurücksetzen und dann 90° nach rechts drehen. Dann soll er seine Fahrt fortsetzen.
Stößt er mit dem Taster K1 an, soll er 45° nach rechts drehen.
Stößt er mit dem Taster K6 an, soll er 45° nach links drehen. Dann soll er seine Fahrt fortsetzen.

Hat er die 10 Meter zurückgelegt, soll die Status-LED grün blinken.



Folgende Probleme treten auf:

- Die Funktion "DriveLength" läßt den Asuro nicht mehr richtig geradeaus fahren. An der Funktion wurde nichts geändert, vorher funktionierte sie eigentlich ganz gut.

- Die Funktion PollSwitch() scheint immer 1 zurückzugeben, statt 0, wenn kein Taster gedrückt ist.
Daher wird in meiner "DriveLength"-Funktion sowie in der Main-Funktion nach der Abfrage mit taste = PollSwitch() bzw. endschalt = PollSwitch() der Wert mit PrintInt() über die Infrarot-Schnittstelle übertragen. Dies scheint seltsamerweise das Problem zu beheben.

- Wenn der Asuro eine Drehung um 45° oder 90° ausführt, dann fährt er danach manchmal nicht mehr weiter. Manchmal geht es, machmal aber nicht. Er bleibt danach einfach stehen. Die Status LED leuchtet grün, blinkt aber nicht. Es liegt also nicht daran daß die 10 Meter Fahrtstrecke erreicht sind.


So....vielleicht tut sich ja wirklich jemand mein Programm an und kann mir sogar einen Tip geben.
Ich bin im Moment jedenfalls ratlos.

Gruß,
Amaris