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-05-16
Revision:
9:ab19796bf14a
Parent:
8:1c8a747c49c8

File content as of revision 9:ab19796bf14a:

#include <cmath>
#include "Turn.h"
//ausr startx und starty kann aus turn.cpp/turn.h entfernt werden
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, int& modeStart,int& path):
    counterLeft(counterLeft), 
    counterRight(counterRight),
    controller(controller),
    wallRight(wallRight),
    wallFront(wallFront),
    wallLeft(wallLeft),
    dontStop(dontStop),
    modeStart(modeStart),
    path(path)
    
{}

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);
    
    if ((modeStart != 1 && wallLeft == 0) || (modeStart == 1 && path==2)){ //Nach Links Drehen
    while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){
        controller.setDesiredSpeedRight(TURNINGSPEED);
        controller.setDesiredSpeedLeft(TURNINGSPEED);
        countsRight = counterRight.read();
        countsLeft = counterLeft.read();
    }
    
    controller.setDesiredSpeedRight(0.5f);
    controller.setDesiredSpeedLeft(-0.5f);
    
    dontStop = 1;
    
    }else if ((modeStart != 1 && wallFront == 0) || (modeStart == 1 && path==1)){ //Nicht Drehen-> weiter Geradeaus
    
        dontStop = 2;
        
        }else if ((modeStart != 1 && wallRight == 0) || (modeStart == 1 && path==3)) { //Nach Rechts Drehen
            
            while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){
                controller.setDesiredSpeedRight(-TURNINGSPEED);
                controller.setDesiredSpeedLeft(-TURNINGSPEED);
                countsRight = counterRight.read();
                countsLeft = counterLeft.read();
            }
            controller.setDesiredSpeedRight(0.5f);
            controller.setDesiredSpeedLeft(-0.5f);
            dontStop = 3;
            
            
            }else if((modeStart != 1) || (modeStart == 1 && path==4)){ //Alle Wege versperrt-> Wenden
                while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){
                    controller.setDesiredSpeedRight(TURNINGSPEED);
                    controller.setDesiredSpeedLeft(TURNINGSPEED);
                    countsRight = counterRight.read();
                    countsLeft = counterLeft.read();
                }
                controller.setDesiredSpeedRight(0.5f);  //0.0f
                controller.setDesiredSpeedLeft(-0.5f);   //0.0f
                
                dontStop = 4;
            }
}