PES2_R2D2.0 / Mbed 2 deprecated MICROMOUSE_KOMPLETT_MIT_ZIEL_SCHNELLER

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Micromouse_alpha_copy_copy by PES2_R2D2.0

Turn.cpp

Committer:
ruesipat
Date:
2018-04-12
Revision:
4:e3f388933954
Parent:
2:592f01278db4
Child:
5:b8b1a979b0d5

File content as of revision 4:e3f388933954:

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

using namespace std;

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

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);
            }
}