File content as of revision 7:5ef09519a6e9:

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

using namespace std;

const float Turn::TURNINGSPEED = 70.0f;//Drehgeschwindgkeit Drehzahl in [rpm]
const int Turn::TURNINGCOUNTS = 950;  //Entspricht Drehung um 90Grad //DONT TOUCH //940//1070

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

Turn::~Turn() {}


void Turn::turning()
{
    int countsRight = counterRight.read();  //EncoderCounts auslesen  
    int countsRight0 = countsRight;         //ReferenzCounts setzten
    int countsLeft = counterLeft.read();
    int countsLeft0 = countsLeft;
    
    //vor dem abbiegen halten
    controller.setDesiredSpeedRight(0.5f);
    controller.setDesiredSpeedLeft(-0.5f);
    //wait(0.1f);
    
    
    
    //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.5f);
    controller.setDesiredSpeedLeft(-0.5f);
    
    dontStop = 1;
    
    }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
    
        //printf("Vorne ist frei\n");
        
        dontStop = 2;
        
        }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.5f);
            controller.setDesiredSpeedLeft(-0.5f);
            
            dontStop = 3;
            
            
            }else{ //Alle Wege versperrt-> Wenden
            
                //printf("Alles versperrt...zurueck\n");
            
                while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){
                    controller.setDesiredSpeedRight(TURNINGSPEED);
                    controller.setDesiredSpeedLeft(TURNINGSPEED);
                    countsRight = counterRight.read();
                    countsLeft = counterLeft.read();
                    //printf("%d\n", countsRight);
                    //printf("%d\n", countsLeft);
                }
                controller.setDesiredSpeedRight(0.5f);  //0.0f
                controller.setDesiredSpeedLeft(-0.5f);   //0.0f
                
                dontStop = 4;
            }
}