PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_FOUR

Pathfinding nach rechts funktioniert noch nicht...der rest schon

Dependencies:   mbed

Fork of MicroMouse_MASTER_THREE by PES2_R2D2.0

Turn.cpp

Committer:
ruesipat
Date:
2018-04-04
Revision:
2:592f01278db4
Parent:
1:d9e840c48b1e
Child:
4:e3f388933954

File content as of revision 2:592f01278db4:

#include <cmath>
#include "Turn.h"

using namespace std;

const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit
const int Turn::TURNINGCOUNTS = 941;  //Entspricht Drehung um 90Grad

Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft):
    counterLeft(counterLeft), 
    counterRight(counterRight),
    controller(controller),
    wallRight(wallRight),
    wallFront(wallFront),
    wallLeft(wallLeft)
{}

Turn::~Turn() {}


void Turn::turning()
{
    int countsRight = counterRight.read();  //EncoderCounts auslesen  
    int countsRight0 = countsRight;         //ReferenzCounts setzten
    int countsLeft = counterLeft.read();
    int countsLeft0 = countsLeft;
    
    //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind
    
    if (wallLeft == 0){ //Nach Links Drehen
    
    printf("Links ist frei\n");
    
    while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){
        controller.setDesiredSpeedRight(TURNINGSPEED);
        controller.setDesiredSpeedLeft(TURNINGSPEED);
        countsRight = counterRight.read();
        countsLeft = counterLeft.read();
        printf("%d\n\r", countsRight);
        printf("%d\n\r", countsLeft);
    }
    controller.setDesiredSpeedRight(0.0f);
    controller.setDesiredSpeedLeft(0.0f);
    
    }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
    
        printf("Vorne ist frei\n");
        
        }else if (wallRight == 0) { //Nach Rechts Drehen
            
            printf("Rechts ist frei\n");
            
            while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){
                controller.setDesiredSpeedRight(-TURNINGSPEED);
                controller.setDesiredSpeedLeft(-TURNINGSPEED);
                countsRight = counterRight.read();
                countsLeft = counterLeft.read();
                //printf("%d\n", countsRight);
                //printf("%d\n", countsLeft);
            }
            controller.setDesiredSpeedRight(0.0f);
            controller.setDesiredSpeedLeft(0.0f);
            
            
            }else{ //Alle Wege versperrt-> Wenden
            
                printf("Alles versperrt...zurueck\n");
            
                while((countsRight <= countsRight0 + 2*TURNINGCOUNTS) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS)){
                    controller.setDesiredSpeedRight(TURNINGSPEED);
                    controller.setDesiredSpeedLeft(TURNINGSPEED);
                    countsRight = counterRight.read();
                    countsLeft = counterLeft.read();
                    //printf("%d\n", countsRight);
                    //printf("%d\n", countsLeft);
                }
                controller.setDesiredSpeedRight(0.0f);
                controller.setDesiredSpeedLeft(0.0f);
            }
}