Algorithmus

Dependencies:   mbed

main.cpp

Committer:
luethale
Date:
11 months ago
Revision:
35:5a4e1a87b3da
Parent:
33:836ab2089565
Child:
36:99f60052c746

File content as of revision 35:5a4e1a87b3da:

 /*----------------------------------------------------------------------------* 
 *      Micromouse PES2 
 *
 *      PES2 2018, TEAM 3
 *
 *      Code entwickelt von:
 *       
 *
 *-----------------------------------------------------------------------------*/
 
#include <mbed.h>
#include "EncoderCounter.h"
#include "Controller.h"
#include "IRSensor.h"
#include "Motion.h"

//User Button
    InterruptIn button(USER_BUTTON);

//Sensors:
    
    AnalogIn lineSensor(PC_5);
    AnalogIn distance2(PC_3);
    AnalogIn distance4(PB_1);
    AnalogIn distance1(PC_2);

    IRSensor irSensorL (distance2);
    IRSensor irSensorC (distance4);
    IRSensor irSensorR (distance1);
    
    Timer t1;

//Motors:
    DigitalOut myled(LED1);
    
    DigitalOut enableMotorDriver(PB_2);
    
    DigitalIn motorDriverFault(PB_14);
    DigitalIn motorDriverWarning(PB_15);
    
    PwmOut pwmRight(PA_8);
    PwmOut pwmLeft(PA_10);
    
    EncoderCounter counterRight(PB_6, PB_7);
    EncoderCounter counterLeft(PA_0, PA_1);
    
    
    Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
    
    Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, 
                    irSensorR, lineSensor, enableMotorDriver);
    
//------------------------------------------------------------------------------
    
    volatile int start = 0;
    
    const int debuging = 1;
    
    const int MOVE = 1;
    const int LEFT = 2;
    const int RIGHT = 3;
    const int HALF_MOVE = 4;
    const int TURN_LEFT = 5;
    const int TURN_RIGHT = 6;
    const int EMPTY = 7;
    
    //Sensor tresholds [mm]
    const float thresholdL = 70;
    const float thresholdR = 70;
    const float thresholdC = 80;
    
//------------------------------------------------------------------------------

//User button toggle
void press() {
    start = !start;
}

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

int main() {
//Init
    
    int route[200] = {0};
    int r = 0;
    
    int junction[20] = {0};
    int j = 0;
    
    short lWall;
    short cWall;
    short rWall;
    
    short Ziel = 0;
    
    //infinite loop
    while(1) {
    
        button.fall(&press); //User button einlesen

/*-----------------------------------------------------------------------------* 
 *    
 *                          Search run
 *
 *-----------------------------------------------------------------------------*/
   
        while(start == 1 && Ziel == 0) { 
         
         
            float distanceL = irSensorL.readL();
            float distanceC = irSensorC.readC();
            float distanceR = irSensorR.readR();
        
            //Wall check
            if (distanceL < thresholdL) lWall = 1;
            else lWall = 0;
            if (distanceC < thresholdC) cWall = 1;
            else cWall = 0;
            if (distanceR < thresholdR) rWall = 1;
            else rWall = 0;
        
            //Junction Check
            if ((lWall + cWall + rWall) < 2) {
     
                if (junction[j] != r) {
                    if (junction[j] > 0) {
                        j += 1;
                        junction[j] = r;   
                    }else{
                        junction[j] = r;   
                    }
                }
            
                if (debuging == 1) printf("Kreuzung: %d Schritt: %d\n", j, r);
            }
            
            //No wall left
            if (lWall == 0) {
            
                if (route[r] == LEFT) {
                
                    route[r] = MOVE;
                    if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
                
                    motion.rotateL();
                    motion.scanMove();
                
                }else if (route[r] == MOVE) {
                
                    route[r] = RIGHT;
                    if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
                    route[r] = MOVE;
                    if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);   
                    r++;
                
                    motion.rotateL();
                    motion.scanMove();
               
                }else if (route[r] == RIGHT) {
                    
                    int lastJunc = route[junction[j]];
                    // Kreuzung führt zu Sackgassen -> löschen
                    junction[j] = 0;
                    printf("Kreuzung %d Schritt %d geloscht\n", j, r);
                    j -= 1;
                    
                    bool looping = false;
                
                    while (junction[j] <= r )  {
                    
                        //invert rotation
                        
                        if (junction[j] == r) {
                        
                            switch (route[r]) {
                                case MOVE: 
                                    if (looping == false) {
                                    motion.runTask(route, r, true, junction[j]);
                                    if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
                                    }
                                    r--;
                                    break;
                                case LEFT:
                                    r--;
                                    break;
                                case RIGHT:
                                    r--;
                                    break;
                            }
                            motion.stop();
                            t1.reset();
                            t1.start();
                            while (t1 < 0.5f) {}
                            t1.stop();
                
                        }else{
                            if (route[r] == LEFT) {
                            
                                route[r] = RIGHT;
                            
                            }else if (route[r] == RIGHT) {
                            
                                route[r] = LEFT;
                                
                            }else if (junction[j] == r-1 && route[r] == MOVE) {
                                
                                looping = true;
                                
                            }else if (junction[j] == r-2 && route[r] == MOVE && route[junction[j]] != MOVE && lastJunc == MOVE) {
                                
                                route[r] = 0;
                                r--;
                            }
                        
                            motion.runTask(route, r, true, junction[j]); 
                            if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
                            route[r] = 0;
                            r--;
                        } 
                    }
                    r++;
                    
                    motion.stop();
                    t1.reset();
                    t1.start();
                    while (t1 < 0.5f) {}
                    t1.stop();
                    if (debuging == 1) printf("Loop stop\n");

                }else{
                    
                    route[r] = LEFT;
                    if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
                    route[r] = MOVE;
                    if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
                
                
                    motion.rotateL();
                    motion.scanMove();
                }
                
            //No wall center
            }else if (cWall == 0) {
            
                if (route[r] == LEFT) {
                
                    route[r] = RIGHT;
                    if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
                    route[r] = MOVE;
                    if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
            
                    motion.scanMove();
                
                }else if (route[r] == MOVE) {
                
                    int lastJunc = route[junction[j]];
                    
                    junction[j] = 0;
                    if (debuging == 1) printf("Kreuzung %d Schritt %d geloscht\n", j, r);
                    j -= 1;
                    bool looping = false;
                
                    while (junction[j] <= r )  {
                    
                        //invert rotation
                        
                        if (junction[j] == r) {
                        
                            switch (route[r]) {
                                case MOVE: 
                                    if (looping == false) {
                                    motion.runTask(route, r, true, junction[j]);
                                    if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
                                    }
                                    r--;
                                    break;
                                case LEFT:
                                    r--;
                                    break;
                                case RIGHT:
                                    r--;
                                    break;
                            }
                            motion.stop();
                            t1.reset();
                            t1.start();
                            while (t1 < 0.5f) {}
                            t1.stop();
                
                        }else{
                            if (route[r] == LEFT) {
                            
                                route[r] = RIGHT;
                            
                            }else if (route[r] == RIGHT) {
                            
                                route[r] = LEFT;
                                
                            }else if (junction[j] == r-1 && route[r] == MOVE) {
                                
                                looping = true;
                                
                            }else if (junction[j] == r-2 && route[r] == MOVE && route[junction[j]] != MOVE && lastJunc == MOVE) {
                                
                                route[r] = 0;
                                r--;
                            }
                        
                            motion.runTask(route, r, true, junction[j]); 
                            if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
                            route[r] = 0;
                            r--;
                        } 
                    }
                    r++;
                    motion.stop();
                    t1.reset();
                    t1.start();
                    while (t1 < 0.5f) {}
                    t1.stop();
                    if (debuging == 1) printf("Loop stop\n");
                
                }else{
                
                    route[r] = MOVE;
                    if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
            
                    motion.scanMove();
                }
                
            //No wall right
            }else if (rWall == 0) {
            
                route[r] = RIGHT;
                if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                r++;
                route[r] = MOVE;
                if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                r++;
            
                motion.rotateR();
                motion.scanMove();
            
            //Dead end routine
            }else if ((lWall + cWall + rWall) == 3) {
            
                motion.rotate180();
                if (debuging == 1) printf("Sackgasse Schritt: %d\n", r);
                r--;
                t1.reset();
                t1.start();
                while (t1 < 0.5f) {}
                t1.stop();
                //Return to last junction
                while (junction[j] <= r ) {
                    
                    if (junction[j] == r) {
                    
                        switch (route[r]) {
                            case MOVE: 
                                motion.runTask(route, r, true, junction[j]);
                                if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
                                r--;
                                break;
                            case LEFT:
                                r--;
                                break;
                            case RIGHT:
                                r--;
                                break;
                        }
                        motion.stop();
                        t1.reset();
                        t1.start();
                        while (t1 < 0.5f) {}
                        t1.stop();
                
                    }else{
                        //invert rotation
                        if (route[r] == LEFT) {
                        
                            route[r] = RIGHT;
                        
                        }else if (route[r] == RIGHT) {
                        
                            route[r] = LEFT;
                        }
                        //Run tasks in declining order
                        motion.runTask(route, r, true, junction[j]);
                        if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
                        route[r] = 0;
                        r--;
                    }
                }
                r++;
            }

            //finish line check
            if (motion.finish() == 1) {
            
                Ziel = 1;  
                r = 1;  
                //Convert rotations to smooth turns
                while (route[r] != 0) {
                    //Convert pattern MOVE - ROTATE - MOVE
                    if (route[r-1] == MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) {
                        
                        route[r-1] = HALF_MOVE;
                        route[r+1] = HALF_MOVE;
                        
                        if (route[r] == LEFT) route[r] = TURN_LEFT;
                        else route[r] = TURN_RIGHT;
                    //Convert consequent ROTATION to a smooth TURN 
                    }else if (route[r-1] == HALF_MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) {
                    
                        route[r-1] = EMPTY;
                        route[r+1] = HALF_MOVE;
                    
                        if (route[r] == LEFT) route[r] = TURN_LEFT;
                        else route[r] = TURN_RIGHT;   
                       
                    }
                    
                    r++;
                }
                r = 0;
                start = 0;
                controller.counterReset();
                myled = 1;
            }else{
                Ziel = 0;
            }


        }
    
    /*-------------------------------------------------------------------------*
     *
     *                                Speed run
     *
     *-------------------------------------------------------------------------*/
     
        while ( start == 1 && Ziel == 1 ) {
        
            motion.runTask(route,r,false,junction[j]);
            r++;
        
            if (route[r] == 0) {
                //Weg fertig
                motion.stop();
                start = 0;  
                myled = 0;  
            }    
        }
    
    
    
    
    }
}