Fahren.cpp

00001 /*Roboshark V7
00002 Fahren.cpp
00003 Erstellt: J. Blunschi
00004 geändert: V.Ahlers
00005 V.5.18
00006 */
00007 
00008 #include "Fahren.h"
00009 #include <mbed.h>
00010 #include "IRSensor.h"
00011 
00012 
00013 using namespace std;
00014 
00015 
00016 
00017 //Konstruktor
00018 Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, IRSensor& iRSensor):
00019     controller(controller), counterLeft(counterLeft), counterRight(counterRight), iRSensor (iRSensor){
00020 SpeedR = 80.0f;
00021 SpeedL = 80.0f;
00022 disF = 0.0f;
00023 }
00024 //Dekonstruktor
00025 Fahren::~Fahren() {}
00026     
00027 
00028 
00029 //Implementation der Methode geradeaus fahren geregelt
00030 void Fahren::geradeausG(){
00031     
00032     //einlesen des aktuellen Encoder standes
00033     initialClicksRight = counterRight.read();   
00034     initialClicksLeft = counterLeft.read();
00035     
00036     //Anzahl Clicks die der Encoder zählen soll
00037     wegRechts = 1776;
00038     wegLinks = 1776;
00039 
00040     
00041     //Zustand, dass der Roboter nicht gestoppt hat
00042     stopRight = false;
00043     stopLeft = false;
00044     reglerEinR = 1;
00045     reglerEinL = 1;
00046     
00047     while(1){
00048     float diff = (iRSensor.readR() - iRSensor.readL())*0.9f;            //Regler
00049     if((diff < 1.5f) ||(diff > -1.5f)) diff = 0;
00050     if(iRSensor.readR()>60.0f) diff=0;
00051     if(iRSensor.readL()>60.0f) diff=0;
00052     //printf("diff =%f \n",diff);
00053     controller.setDesiredSpeedRight(SpeedR+diff);
00054     controller.setDesiredSpeedLeft(-(SpeedL-diff));
00055     
00056     wait(0.05);
00057        
00058        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00059        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00060         
00061         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00062         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts || iRSensor.readF() < 50.0f){    
00063             controller.setDesiredSpeedRight(0);
00064             stopRight = true;
00065             reglerEinR = 0;
00066             }
00067             
00068         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00069         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks || iRSensor.readF() < 50.0f){
00070             controller.setDesiredSpeedLeft(0);
00071             stopLeft = true;
00072             reglerEinL = 0;
00073             }
00074      
00075         if(stopRight == true && stopLeft == true){
00076             return;
00077             }  
00078          
00079         }
00080         
00081     }
00082 
00083 //Implementation der Methode 90 Grad Rechtsdrehen
00084 void Fahren::rechts90() {
00085         
00086     //einlesen des aktuellen Encoder standes
00087     initialClicksRight = counterRight.read();   
00088     initialClicksLeft = counterLeft.read();
00089     
00090     //Anzahl Clicks die der Encoder zählen soll
00091     wegRechts = 868;
00092     wegLinks = 868;
00093     
00094     //Zustand, dass der Roboter nicht gestoppt hat
00095     stopRight = false;
00096     stopLeft = false;
00097     
00098     //Drehrichtung der Motoren
00099     controller.setDesiredSpeedRight((SpeedR));
00100     controller.setDesiredSpeedLeft((SpeedL));
00101     //printf("SpeedR in F = %f\n",SpeedR);
00102     //printf("SpeedL in F = %f\n",SpeedL);
00103     while(1){
00104        
00105        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00106        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00107         
00108         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00109         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
00110             controller.setDesiredSpeedRight(0);
00111             stopRight = true;
00112             }
00113             
00114         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00115         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
00116             controller.setDesiredSpeedLeft(0);
00117             stopLeft = true;
00118             }
00119      
00120         if(stopRight == true && stopLeft == true){
00121             return;
00122             }  
00123          
00124         }
00125         
00126     }
00127     
00128     
00129     
00130 //Implementation der Methode 180 Grad Rechtsdrehen
00131 void Fahren::rechts180() {
00132         
00133     //einlesen des aktuellen Encoder standes
00134     initialClicksRight = counterRight.read();   
00135     initialClicksLeft = counterLeft.read();
00136     
00137     //Anzahl Clicks die der Encoder zählen soll
00138     wegRechts = 1752;
00139     wegLinks = 1752;
00140     
00141     //Zustand, dass der Roboter nicht gestoppt hat
00142     stopRight = false;
00143     stopLeft = false;
00144     
00145     //Drehrichtung der Motoren
00146     controller.setDesiredSpeedRight((SpeedR));
00147     controller.setDesiredSpeedLeft((SpeedL));
00148     
00149     while(1){
00150        
00151        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00152        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00153         
00154         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00155         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
00156             controller.setDesiredSpeedRight(0);
00157             stopRight = true;
00158             }
00159             
00160         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00161         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
00162             controller.setDesiredSpeedLeft(0);
00163             stopLeft = true;
00164             }
00165      
00166         if(stopRight == true && stopLeft == true){
00167             return;
00168             }  
00169          
00170         }
00171         
00172     }
00173     
00174 
00175 //Implementation der Methode 90 Grad Linksdrehen
00176 void Fahren::links90() {
00177         
00178     //einlesen des aktuellen Encoder standes
00179     initialClicksRight = counterRight.read();   
00180     initialClicksLeft = counterLeft.read();
00181     
00182     //Anzahl Clicks die der Encoder zählen soll
00183     wegRechts = 868;
00184     wegLinks = 868;
00185     
00186     //Zustand, dass der Roboter nicht gestoppt hat
00187     stopRight = false;
00188     stopLeft = false;
00189     
00190     //Drehrichtung der Motoren
00191     controller.setDesiredSpeedRight((-SpeedR));
00192     controller.setDesiredSpeedLeft((-SpeedL));
00193     
00194     while(1){
00195        
00196        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00197        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00198         
00199         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00200         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
00201             controller.setDesiredSpeedRight(0);
00202             stopRight = true;
00203             }
00204             
00205         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00206         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
00207             controller.setDesiredSpeedLeft(0);
00208             stopLeft = true;
00209             }
00210      
00211         if(stopRight == true && stopLeft == true){
00212             return;
00213             }  
00214          
00215         }
00216         
00217     }
00218     
00219         
00220 //Implementation der Methode Ziel erreicht
00221 void Fahren::ziel() {
00222         
00223     //einlesen des aktuellen Encoder standes
00224     initialClicksRight = counterRight.read();   
00225     initialClicksLeft = counterLeft.read();
00226     
00227     //Anzahl Clicks die der Encoder zählen soll
00228     wegRechts = 7050;
00229     wegLinks = 7050;
00230     
00231     //Zustand, dass der Roboter nicht gestoppt hat
00232     stopRight = false;
00233     stopLeft = false;
00234     
00235     //Drehrichtung der Motoren
00236     controller.setDesiredSpeedRight((SpeedR));
00237     controller.setDesiredSpeedLeft((SpeedR));
00238     
00239     while(1){
00240        
00241        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00242        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00243         
00244         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00245         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
00246             controller.setDesiredSpeedRight(0);
00247             stopRight = true;
00248             }
00249             
00250         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00251         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
00252             controller.setDesiredSpeedLeft(0);
00253             stopLeft = true;
00254             }
00255      
00256         if(stopRight == true && stopLeft == true){
00257             return;  
00258             } 
00259         }
00260         
00261     }
00262 
00263 //Implementation der Methode stopp
00264 void Fahren::stopp(){
00265     
00266     //einlesen des aktuellen Encoder standes
00267     initialClicksRight = counterRight.read();   
00268     initialClicksLeft = counterLeft.read();
00269     
00270     //Anzahl Clicks die der Encoder zählen soll
00271     wegRechts = 0;
00272     wegLinks = 0;
00273         
00274     //Geschwindigkeit
00275     speedRight = 0;
00276     speedLeft = 0;
00277     
00278     //Zustand, dass der Roboter nicht gestoppt hat
00279     stopRight = false;
00280     stopLeft = false;
00281     
00282     //Drehrichtung der Motoren
00283     controller.setDesiredSpeedRight(speedRight);
00284     controller.setDesiredSpeedLeft(-(speedLeft));
00285     
00286     
00287     while(1){
00288        
00289        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00290        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00291         
00292         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00293         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
00294             controller.setDesiredSpeedRight(0);
00295             stopRight = true;
00296             }
00297             
00298         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00299         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
00300             controller.setDesiredSpeedLeft(0);
00301             stopLeft = true;
00302             }
00303      
00304         if(stopRight == true && stopLeft == true){
00305             return;
00306             }  
00307          
00308         }
00309         
00310     }