File content as of revision 1:2b5f79285a3e:

/** 
 *      Micromouse PES2
 *
 *      Suchfahrtalg. + Schnellfahrtalg.  
 *
 */
 
 /* todo:
 
    - Regler gerade Fahrt
        > Mitte Werte L/R
    - Suchfahrt testen
        > Tresholds bestimmen
    - gespeicherten Weg fahren
    - Problem: Links beschleunigt schneller
    - Ziellinie
    - Beschleunigung
    - über längere Strecke schneller fahren
    
*/
#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;
    const int STOP = 5;
    
    //Sensor tresholds [mm]
    const float thresholdL = 70;
    const float thresholdR = 70;
    const float thresholdC = 100;
    
//------------------------------------------------------------------------------

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

//Return to last junction
void reverseToJunction(int& junc, int& r, int route[]) {
    
        while (junc < r ) {
            
            //invert rotation
            if (route[r] == LEFT) {
                
                route[r] = RIGHT;
                
            }else if (route[r] == RIGHT) {
                
                route[r] = LEFT;
            }
            
            motion.runTask(route[r]);
            route[r] = 0;
            r--;
        }    
}
    
    
//------------------------------------------------------------------------------

int main() {
//Init
    
    int route[50] = {0};
    int r = 0;
    
    int junction[5] = {0};
    int j = 0;
    
    short lWall;
    short cWall;
    short rWall;
    
    short Ziel = 0;
    
//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] > 0) {j++;}
            junction[j] = r;
        }
        //No wall left
        if (lWall == 0) {
            
            if (route[r] == LEFT) {
                
                route[r] = MOVE;
                r++;
                
                motion.rotateL();
                motion.scanMove();
                motion.stop();
                
            }else if (route[r] == MOVE) {
                
                route[r] = RIGHT;
                r++;
                route[r] = MOVE;
                r++;
                
                motion.rotateL();
                motion.scanMove();
                motion.stop();
               
            }else if (route[r] == RIGHT) {
                // Kreuzung führt zu Sackgassen -> löschen
                junction[j] = 0;
                if (j > 0) {j--;}
                reverseToJunction(junction[j], r, route);
                 
            }else{
                
                route[r] = LEFT;
                r++;
                route[r] = MOVE;
                r++;
                
                motion.rotateL();
                motion.scanMove();
                motion.stop();
            }
        //No wall center
        }else if (cWall == 0) {
            
            if (route[r] == LEFT) {
                
                route[r] = RIGHT;
                r++;
                route[r] = MOVE;
                r++;
            
                motion.scanMove();
                motion.stop();
                
            }else if (route[r] == MOVE) {
                
                junction[j] = 0;
                if (j > 0) {j--;}
                reverseToJunction(junction[j], r, route);
                
            }else{
                
                route[r] = MOVE;
                r++;
            
                motion.scanMove();
                motion.stop();
            }
        //No wall right
        }else if (rWall == 0) {
            
            route[r] = RIGHT;
            r++;
            route[r] = MOVE;
            r++;
            
            motion.rotateR();
            motion.scanMove();
            motion.stop();
            
        //Dead end routine
        }else if ((lWall + cWall + rWall) == 3) {
            
            motion.rotate180();
            r--;
            //Return to last junction
            while (junction[j] < r ) {
                
                //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]);
                route[r] = 0;
                r--;
            }
        }

        if (motion.finish() == 1) {
            Ziel = 1;  
            r = 0;  
            start = 0;
        }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;  
        }
        motion.runTask(route[r]);
        r++;
        
        if (route[r] == 0) {
            //Weg fertig
            start = 0;    
        }    
    }
    
    
    
    
}
}