Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
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; } }