Programm zur Kollisionsabfrage

Da einige sich mit C ein wenig schwer tun poste ich mal mein Programm, bei mir funktioniert es problemlos.. muss allerdings sagen das ich mit PollSwitch keine probleme habe und eigentlich immer richtige werte bekomme, auch mit laufenden motor..
Das programm ist sicherlich nicht optimal aber funktioniert.


Einzigste Nutzungsbedinung ist es nicht als sein eigenes Werk auszugeben..

Code:

---

#include "asuro.h"

#define SPEED_L 110          //falls motoren unterschiedlich schnell
#define SPEED_R 110

int j = 0;  //eine variable zum zählen
int k = 0; //pollswitch

void Msleep(int dauer) {
        int z;
        for (z = 0; z < dauer; z++)
                Sleep(36);
}

//zurück fahren und nach links lenken, wartezeiten dazwischen sind wichtig sonst passiert es das Asuro abschmiert
void drive_back(void) {
        MotorSpeed(0, 0);
        Msleep(50);
        MotorDir(RWD, RWD);
        MotorSpeed(150, 150);
        Msleep(1000);  //sekunde zurueck fahren
        MotorSpeed(0, 0);
        Msleep(500);
        MotorDir(FWD, FWD);
        Msleep(10);
}

//ein stück zurück fahren (brauch man nicht umbedingt)
void drive_back_bit(void) {
        MotorSpeed(0, 0);
        Msleep(50);
        MotorDir(RWD, RWD);
        MotorSpeed(100, 100);
        Msleep(500);
        MotorSpeed(0, 0);
        Msleep(250);
        MotorDir(FWD, FWD);
        Msleep(10);
}

//nach links fahren 1Sekunde
void drive_left(void) {
        Msleep(30);
        MotorSpeed(0, 150);
        Msleep(1000);
        MotorSpeed(0, 0);
        Msleep(10);
}

//rechts fahren 1Sekunde
void drive_right(void) {
        Msleep(30);
        MotorSpeed(150, 0);
        Msleep(1000);
        MotorSpeed(0, 0);
        Msleep(10);
}

void kollisions_abfrage(void){
        for (j = 0; j < 10; j++) {
                                PollSwitch(); //werte stabilisieren
                        }
                        k = PollSwitch();
                        switch (k) {    //die case anweisungen kann man auch ändern, diese sind auf meinen Asuro und Gehäuse angepasst
                        case 1:
                                drive_back_bit();
                                drive_left(); //K6
                                break;
                        case 2:
                                drive_back();
                                drive_left(); //K5
                                break;
                        case 3:
                                drive_back();
                                drive_left(); //K5 & K6
                                break;
                        case 4:
                                drive_back();
                                drive_right(); //K4
                                break;
                        case 6:
                                drive_back();
                                drive_right(); //K4 & K5
                                break;
                        case 8:
                                drive_back();
                                drive_left(); //K3
                                break;
                        case 12:
                                drive_back();
                                drive_left(); //K3 & K4
                                break;
                        case 16:
                                drive_back();
                                drive_right(); //K2
                                break;
                        case 18:
                                drive_back();
                                drive_left(); //K2 & K5
                                break;
                        case 22:
                                drive_back();
                                drive_left(); //K2 & K5 & K4
                                break;
                        case 24:
                                drive_back();
                                drive_left(); //K3 & K2
                                break;
                        case 30:
                                drive_back();
                                drive_left(); //K2 & K3 & K4 & K5
                                break;
                        case 32:
                                drive_back_bit();
                                drive_right(); //K1
                                break;
                        case 48:
                                drive_back();
                                drive_right(); //K1 & K2
                                break;
                        default:
                                MotorSpeed(SPEED_L, SPEED_R); //gerade
                        }
}

int main(void) {
        /*START*/
        Init();
        MotorDir(FWD, FWD);
        StatusLED(YELLOW);
        FrontLED(ON);

        /*WARTEN*/
        while (PollSwitch() != 1) { //blinkt solange bis K6 gedrückt wird
                BackLED(ON, OFF);
                Msleep(300);
                BackLED(OFF, ON);
                Msleep(300);
        }

      BackLED(OFF, OFF);
        Msleep(1000); //1 sekunde warten
        StatusLED(GREEN);
        MotorSpeed(SPEED_L, SPEED_R); //fahre los

while (1) {    //mach das bis er brennt oder akkus leer sind

                //Kollisionsabfrage
                kollisions_abfrage();
                MotorSpeed(SPEED_L, SPEED_R);
        }

       
        StatusLED(RED);
        BackLED(OFF, OFF);
        MotorSpeed(0, 0);

        while (1) {
        }
        return 0;
}

---

Falls jemand Fehler findet (nach 3 Bier kann auch Copy Paste fehlerhaft sein..^^) kann er die ja posten ;)

P.S: code ist nicht optimiert und naja.. Asuro C progge ich wenn ich nicht einschlafen kann


Viel spaß mit dem code und bin für anregungen und vorallem für verbesserungsvorschläge offen..

bei mir selber ist das programm noch komplexer mit tischkantenerkennung, linienverfolgung und odometrieabfrage.. aber die is noch buggy.. poste ich vllt später.

Danke schön!! So was hab ich gesucht.