File content as of revision 14:2926932e26df:

 /** 
 *      Micromouse PES2
 *
 *      Suchfahrtalg. + Schnellfahrtalg.  
 *
 */
 
 /* todo:
 
    - Beschleunigung: Wann & Wo?
    - Zentrieren
    
    Optimieren:
    - Beschleunigung
    - Zentrieren
    - über längere Strecke schneller fahren
    - Abbiegen
    
*/
#include <mbed.h>
#include "EncoderCounter.h"
#include "Controller.h"
#include "IRSensor.h"
#include "Motion.h"

//User Button
    InterruptIn button(USER_BUTTON);

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

    IRSensor irSensorL (distance2);
    IRSensor irSensorC (distance4);
    IRSensor irSensorR (distance1);
    IRSensor irSensorB (distance3);

//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 MOVE = 1;
    const int LEFT = 2;
    const int RIGHT = 3;
    const int SPEED = 4;
    
    //Sensor tresholds [mm]
    const float thresholdL = 80;
    const float thresholdR = 80;
    const float thresholdC = 100;
    
//------------------------------------------------------------------------------

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

//Return to last junction
void reverseToJunction(int junc, int &r, int route[]) {
    
        do {
            
            //invert rotation
            if (route[r] == LEFT) {
                
                route[r] = RIGHT;
                
            }else if (route[r] == RIGHT) {
                
                route[r] = LEFT;
            }
            
            motion.runTask(route,r,true);
            printf("Schritt: %d, Befehl: %d\n", r, route[r]);
            route[r] = 0;
            r--;
        } while (junc < r );   
}

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

int main() {
//Init
    
    int route[200] = {0};
    int r = 0;
    
    volatile int junction[20] = {0};
    volatile 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) { 
    
            /**
            *  Entscheidung und Bewegung
            */
         
         
            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;   
                    }
                }
            
                printf("Kreuzung: %d Schritt: %d\n", j, r);
            }
            //No wall left
            if (lWall == 0) {
            
                if (route[r] == LEFT) {
                
                    route[r] = MOVE;
                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
                
                    motion.rotateL();
                    motion.scanMove();
                 //   motion.stop();
                
                }else if (route[r] == MOVE) {
                
                    route[r] = RIGHT;
                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
                    route[r] = MOVE;
                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
                
                    motion.rotateL();
                    motion.scanMove();
                    //motion.stop();
               
                }else if (route[r] == RIGHT) {
                    // Kreuzung führt zu Sackgassen -> löschen
                    junction[j] = 0;                                            /*Änderung alex*/
                    //if (j > 0) {j--;}
                    printf("Kreuzung %d Schritt %d geloscht\n", j, r);
                    j -= 1;
                    //reverseToJunction(junction[j], r, route);
                
                    /*for (int i = r; i > junction[j]; i--) {
                        r = i;
                        //invert rotation
                        if (route[r] == LEFT) {
                        
                            route[r] = RIGHT;
                        
                        }else if (route[r] == RIGHT) {
                        
                            route[r] = LEFT;
                        }
                    
                        motion.runTask(route[r]);
                        printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                        route[r] = 0;
                        
                        if ( r == junction[j] + 1) {
                            while (controller.getSpeedLeft() > 0.0f) {
                                motion.stop();   
                            }   
                        }         
                        
                    }
                    motion.stop();
                    
                    printf("Loop fertig\n");
                    r--; 
                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    */
                
                    do {
                    
                        //invert rotation
                        if (route[r] == LEFT) {
                        
                            route[r] = RIGHT;
                        
                        }else if (route[r] == RIGHT) {
                        
                            route[r] = LEFT;
                        }
                    
                        motion.runTask(route,r,true);
                        printf("Schritt: %d, Befehl: %d\n", r, route[r]);                   //At work!!!!!
                        route[r] = 0;
                        r--;
                        
                    } while (junction[j] < r ) ;
                    motion.stop();
                    printf("Loop stop\n");

                }else{
                    
                    route[r] = LEFT;
                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
                    route[r] = MOVE;
                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
                
                
                    motion.rotateL();
                    motion.scanMove();
                   // motion.stop();
                }
        //No wall center
            }else if (cWall == 0) {
            
                if (route[r] == LEFT) {
                
                    route[r] = RIGHT;
                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
                    route[r] = MOVE;
                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
            
                    motion.scanMove();
                   // motion.stop();
                
                }else if (route[r] == MOVE) {
                
                    junction[j] = 0;                                            /*Änderung alex*/
                    //if (j > 0) {j--;}
                    printf("Kreuzung %d Schritt %d geloscht\n", j, r);
                    j -= 1;
                    //reverseToJunction(junction[j], r, route);
                
                    /*for (int i = r; i > junction[j]; i--) {
                        r = i;
                        //invert rotation
                        if (route[r] == LEFT) {
                        
                            route[r] = RIGHT;
                        
                        }else if (route[r] == RIGHT) {
                        
                            route[r] = LEFT;
                        }
                    
                        motion.runTask(route[r]);
                        printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                        route[r] = 0; 
                        
                        if ( r == junction[j] + 1) {
                            while (controller.getSpeedLeft() > 0.0f) {
                                motion.stop();   
                            }   
                        }     
                            
                    }
                    motion.stop();
                    printf("Loop fertig\n");
                    r--; 
                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    */
                
                    do {
                    
                        //invert rotation
                        if (route[r] == LEFT) {
                        
                            route[r] = RIGHT;
                        
                        }else if (route[r] == RIGHT) {
                        
                            route[r] = LEFT;
                        }
                    
                        motion.runTask(route,r,true);
                        printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                        route[r] = 0;
                        r--;
                        
                    } while (junction[j] < r );
                    motion.stop();
                    printf("Loop stop\n");
                
                }else{
                
                    route[r] = MOVE;
                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                    r++;
            
                    motion.scanMove();
                }
            //No wall right
            }else if (rWall == 0) {
            
                route[r] = RIGHT;
                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                r++;
                route[r] = MOVE;
                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                r++;
            
                motion.rotateR();
                motion.scanMove();
               // motion.stop();
            
            //Dead end routine
            }else if ((lWall + cWall + rWall) == 3) {
            
                motion.rotate180();
                printf("Sackgasse Schritt: %d\n", r);
                r--;
                //Return to last junction
                while (junction[j] <= r ) {
                //for (int i = r; i >= junction[j]; i--
            
                    if (junction[j] == r) {
                    
                        switch (route[r]) {
                            case MOVE: 
                                motion.runTask(route,r,true);
                                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                                r--;
                                break;
                            case LEFT:
                                r--;
                                break;
                            case RIGHT:
                                r--;
                                break;
                        }
                
                    }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);
                        printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                        route[r] = 0;
                        r--;
                    }
                }
                r++;
            }

            if (motion.finish() == 1) {
            
                /*while(r > 0) {
                    //Zum Start zurückfahren
                    if (route[r] == LEFT) {
                    
                        route[r] = RIGHT;
                    
                    }else if (route[r] == RIGHT) {
                    
                        route[r] = LEFT;
                    }
                
                    motion.runTask(route[r]);
                    r--;    
                }
            
                motion.runTask(route[r])
            */
            
                Ziel = 1;  
                r = 0;  
                start = 0;
                controller.counterReset();
            }else{
                Ziel = 0;
            }


        }
    
    /** 
     *    
     *  Speed run
     *
     */
     
        while ( start == 1 && Ziel == 1 ) {
        
            /*if (route[r] == route[r+1] && route[r] == route[r+2]) {
                //Auf längere Strecke schneller fahren
                route[r+1] = SPEED;  
            }*/
        
            float distanceL = irSensorL.readL();
            float distanceC = irSensorC.readC();
            float distanceR = irSensorR.readR();
        
            motion.runTask(route,r,false);
            r++;
        
            if (route[r] == 0) {
                //Weg fertig
                motion.stop();
                start = 0;    
            }    
        }
    
    
    
    
    }
}