Turn.cpp

00001 #include <cmath>
00002 #include "Turn.h"
00003 //ausr startx und starty kann aus turn.cpp/turn.h entfernt werden
00004 using namespace std;
00005 
00006 const float Turn::TURNINGSPEED = 70.0f;//Drehgeschwindgkeit Drehzahl in [rpm]
00007 const int Turn::TURNINGCOUNTS = 950;  //Entspricht Drehung um 90Grad //DONT TOUCH //940//1070
00008 
00009 Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop, int& modeStart,int& path):
00010     counterLeft(counterLeft), 
00011     counterRight(counterRight),
00012     controller(controller),
00013     wallRight(wallRight),
00014     wallFront(wallFront),
00015     wallLeft(wallLeft),
00016     dontStop(dontStop),
00017     modeStart(modeStart),
00018     path(path)
00019     
00020 {}
00021 
00022 Turn::~Turn() {}
00023 
00024 
00025 void Turn::turning()
00026 {
00027     int countsRight = counterRight.read();  //EncoderCounts auslesen  
00028     int countsRight0 = countsRight;         //ReferenzCounts setzten
00029     int countsLeft = counterLeft.read();
00030     int countsLeft0 = countsLeft;
00031     
00032     //Vor dem abbiegen halten
00033     
00034     controller.setDesiredSpeedRight(0.5f);
00035     controller.setDesiredSpeedLeft(-0.5f);
00036     
00037     if ((modeStart != 1 && wallLeft == 0) || (modeStart == 1 && path==2)){ //Nach Links Drehen
00038     while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){
00039         controller.setDesiredSpeedRight(TURNINGSPEED);
00040         controller.setDesiredSpeedLeft(TURNINGSPEED);
00041         countsRight = counterRight.read();
00042         countsLeft = counterLeft.read();
00043     }
00044     
00045     controller.setDesiredSpeedRight(0.5f);
00046     controller.setDesiredSpeedLeft(-0.5f);
00047     
00048     dontStop = 1;
00049     
00050     }else if ((modeStart != 1 && wallFront == 0) || (modeStart == 1 && path==1)){ //Nicht Drehen-> weiter Geradeaus
00051     
00052         dontStop = 2;
00053         
00054         }else if ((modeStart != 1 && wallRight == 0) || (modeStart == 1 && path==3)) { //Nach Rechts Drehen
00055             
00056             while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){
00057                 controller.setDesiredSpeedRight(-TURNINGSPEED);
00058                 controller.setDesiredSpeedLeft(-TURNINGSPEED);
00059                 countsRight = counterRight.read();
00060                 countsLeft = counterLeft.read();
00061             }
00062             controller.setDesiredSpeedRight(0.5f);
00063             controller.setDesiredSpeedLeft(-0.5f);
00064             dontStop = 3;
00065             
00066             
00067             }else if((modeStart != 1) || (modeStart == 1 && path==4)){ //Alle Wege versperrt-> Wenden
00068                 while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){
00069                     controller.setDesiredSpeedRight(TURNINGSPEED);
00070                     controller.setDesiredSpeedLeft(TURNINGSPEED);
00071                     countsRight = counterRight.read();
00072                     countsLeft = counterLeft.read();
00073                 }
00074                 controller.setDesiredSpeedRight(0.5f);  //0.0f
00075                 controller.setDesiredSpeedLeft(-0.5f);   //0.0f
00076                 
00077                 dontStop = 4;
00078             }
00079 }