Fahren.cpp

00001 /*Roboshark V6
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 "Regler.h"
00011 
00012 
00013 using namespace std;
00014 
00015 const float Fahren :: PERIOD = 0.8f;
00016 
00017 //Konstruktor
00018 Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, Regler& regler, int reglerEin, IRSensor& iRSensor):
00019     controller(controller), counterLeft(counterLeft), counterRight(counterRight), regler(regler), reglerEin(reglerEin), iRSensor (iRSensor) {         //IRSENSOR IETAH
00020     
00021     SpeedR = 50;
00022     SpeedL = 50;
00023     disF = 0;
00024     ticker.attach(callback(this, &Fahren::getSpeed), PERIOD);
00025 }
00026 
00027 //Dekonstruktor
00028 Fahren::~Fahren() {
00029     ticker.detach();
00030     }
00031     
00032 
00033 //Korrigierte Geschwindigkeit aus der Klasse Regler aufrufen
00034 void Fahren::getSpeed(){
00035     if(reglerEin == 1){
00036     SpeedR = regler.getSpeedR();
00037     SpeedL = regler.getSpeedL();
00038     }else {SpeedR = regler.getSpeedR();
00039             SpeedL = regler.getSpeedL();
00040     }
00041     disF = iRSensor.readF();
00042     //printf("SpeedR in F = %f\n",SpeedR);
00043     //printf("SpeedL in F = %f\n",SpeedL);
00044 }
00045 
00046 
00047 //Implementation der Methode geradeaus fahren ungeregelt
00048 void Fahren::geradeausU(){
00049     
00050     //einlesen des aktuellen Encoder standes
00051     initialClicksRight = counterRight.read();   
00052     initialClicksLeft = counterLeft.read();
00053     
00054     //Anzahl Clicks die der Encoder zählen soll
00055     wegRechts = 1773;
00056     wegLinks = 1773;
00057         
00058     //Geschwindigkeit
00059     //speedRight = 50;
00060     //speedLeft = 50;
00061     
00062     //Zustand, dass der Roboter nicht gestoppt hat
00063     stopRight = false;
00064     stopLeft = false;
00065     
00066     //Drehrichtung der Motoren
00067     controller.setDesiredSpeedRight(SpeedR);
00068     controller.setDesiredSpeedLeft(-(SpeedL));
00069     
00070     
00071     while(1){
00072        
00073        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00074        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00075         
00076         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00077         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
00078             controller.setDesiredSpeedRight(0);
00079             stopRight = true;
00080             }
00081             
00082         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00083         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
00084             controller.setDesiredSpeedLeft(0);
00085             stopLeft = true;
00086             }
00087      
00088         if(stopRight == true && stopLeft == true){
00089             return;
00090             }  
00091          
00092         }
00093         
00094     }
00095     
00096 //Implementation der Methode geradeaus fahren geregelt
00097 void Fahren::geradeausG(){
00098     
00099     //einlesen des aktuellen Encoder standes
00100     initialClicksRight = counterRight.read();   
00101     initialClicksLeft = counterLeft.read();
00102     
00103     //Anzahl Clicks die der Encoder zählen soll
00104     wegRechts = 1788;           //vo 1773
00105     wegLinks = 1788;            //vo 1773
00106         
00107     //Geschwindigkeit
00108     //speedRight = 50;//SpeedR
00109     //speedLeft = 50;//SpeedL
00110     
00111         
00112     //Zustand, dass der Roboter nicht gestoppt hat
00113     stopRight = false;
00114     stopLeft = false;
00115     
00116     //Drehrichtung der Motoren
00117     controller.setDesiredSpeedRight(SpeedR);
00118     controller.setDesiredSpeedLeft(-(SpeedL));
00119         //printf("SpeedR in F2= %f\n",SpeedR);
00120         //printf("SpeedL in F2= %f\n",SpeedL);
00121     
00122     while(1){
00123        
00124        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00125        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00126         
00127         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00128         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts || disF <= 50){
00129             controller.setDesiredSpeedRight(0);
00130             stopRight = true;
00131             }
00132             
00133         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00134         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks || disF <= 50){
00135             controller.setDesiredSpeedLeft(0);
00136             stopLeft = true;
00137             }
00138      
00139         if(stopRight == true && stopLeft == true){
00140             return;
00141             }  
00142          
00143         }
00144         
00145     }
00146 
00147 //Implementation der Methode 90 Grad Rechtsdrehen
00148 void Fahren::rechts90() {
00149         
00150     //einlesen des aktuellen Encoder standes
00151     initialClicksRight = counterRight.read();   
00152     initialClicksLeft = counterLeft.read();
00153     
00154     //Anzahl Clicks die der Encoder zählen soll
00155     wegRechts = 863;
00156     wegLinks = 863;
00157     
00158     //Geschwindigkeit
00159     speedRight = 50;
00160     speedLeft = 50;
00161     
00162     //Zustand, dass der Roboter nicht gestoppt hat
00163     stopRight = false;
00164     stopLeft = false;
00165     
00166     //Drehrichtung der Motoren
00167     controller.setDesiredSpeedRight(SpeedR);
00168     controller.setDesiredSpeedLeft(SpeedL);
00169     //printf("SpeedR in F = %f\n", SpeedR);
00170     //printf("SpeedL in F = %f\n", SpeedL);
00171     while(1){
00172        
00173        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00174        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00175         
00176         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00177         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
00178             controller.setDesiredSpeedRight(0);
00179             stopRight = true;
00180             }
00181             
00182         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00183         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
00184             controller.setDesiredSpeedLeft(0);
00185             stopLeft = true;
00186             }
00187      
00188         if(stopRight == true && stopLeft == true){
00189             return;
00190             }  
00191          
00192         }
00193         
00194     }
00195     
00196     
00197     
00198 //Implementation der Methode 180 Grad Rechtsdrehen
00199 void Fahren::rechts180() {
00200         
00201     //einlesen des aktuellen Encoder standes
00202     initialClicksRight = counterRight.read();   
00203     initialClicksLeft = counterLeft.read();
00204     
00205     //Anzahl Clicks die der Encoder zählen soll
00206     wegRechts = 1752;
00207     wegLinks = 1752;
00208     
00209     //Geschwindigkeit
00210     //speedRight = 50;
00211     //speedLeft = 50;
00212     
00213     //Zustand, dass der Roboter nicht gestoppt hat
00214     stopRight = false;
00215     stopLeft = false;
00216     
00217     //Drehrichtung der Motoren
00218     controller.setDesiredSpeedRight((SpeedR));
00219     controller.setDesiredSpeedLeft((SpeedL));
00220     
00221     while(1){
00222        
00223        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00224        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00225         
00226         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00227         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
00228             controller.setDesiredSpeedRight(0);
00229             stopRight = true;
00230             }
00231             
00232         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00233         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
00234             controller.setDesiredSpeedLeft(0);
00235             stopLeft = true;
00236             }
00237      
00238         if(stopRight == true && stopLeft == true){
00239             return;
00240             }  
00241          
00242         }
00243         
00244     }
00245     
00246 
00247 //Implementation der Methode 90 Grad Linksdrehen
00248 void Fahren::links90() {
00249         
00250     //einlesen des aktuellen Encoder standes
00251     initialClicksRight = counterRight.read();   
00252     initialClicksLeft = counterLeft.read();
00253     
00254     //Anzahl Clicks die der Encoder zählen soll
00255     wegRechts = 878;
00256     wegLinks = 878;
00257     
00258     //Geschwindigkeit
00259     //speedRight = 50;
00260     //speedLeft = 50;
00261     
00262     //Zustand, dass der Roboter nicht gestoppt hat
00263     stopRight = false;
00264     stopLeft = false;
00265     
00266     //Drehrichtung der Motoren
00267     controller.setDesiredSpeedRight((-SpeedR));
00268     controller.setDesiredSpeedLeft((-SpeedL));
00269     
00270     while(1){
00271        
00272        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00273        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00274         
00275         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00276         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
00277             controller.setDesiredSpeedRight(0);
00278             stopRight = true;
00279             }
00280             
00281         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00282         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
00283             controller.setDesiredSpeedLeft(0);
00284             stopLeft = true;
00285             }
00286      
00287         if(stopRight == true && stopLeft == true){
00288             return;
00289             }  
00290          
00291         }
00292         
00293     }
00294     
00295         
00296 //Implementation der Methode Ziel erreicht
00297 void Fahren::ziel() {
00298         
00299     //einlesen des aktuellen Encoder standes
00300     initialClicksRight = counterRight.read();   
00301     initialClicksLeft = counterLeft.read();
00302     
00303     //Anzahl Clicks die der Encoder zählen soll
00304     wegRechts = 7050;
00305     wegLinks = 7050;
00306     
00307     //Geschwindigkeit
00308     //speedRight = 80;
00309     //speedLeft = 80;
00310     
00311     //Zustand, dass der Roboter nicht gestoppt hat
00312     stopRight = false;
00313     stopLeft = false;
00314     
00315     //Drehrichtung der Motoren
00316     controller.setDesiredSpeedRight((SpeedR));
00317     controller.setDesiredSpeedLeft((SpeedR));
00318     
00319     while(1){
00320        
00321        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00322        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00323         
00324         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00325         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
00326             controller.setDesiredSpeedRight(0);
00327             stopRight = true;
00328             }
00329             
00330         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00331         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
00332             controller.setDesiredSpeedLeft(0);
00333             stopLeft = true;
00334             }
00335      
00336         if(stopRight == true && stopLeft == true){
00337             return;  
00338             } 
00339         }
00340         
00341     }
00342 
00343 //Implementation der Methode stopp
00344 void Fahren::stopp(){
00345     
00346     //einlesen des aktuellen Encoder standes
00347     initialClicksRight = counterRight.read();   
00348     initialClicksLeft = counterLeft.read();
00349     
00350     //Anzahl Clicks die der Encoder zählen soll
00351     wegRechts = 0;
00352     wegLinks = 0;
00353         
00354     //Geschwindigkeit
00355     speedRight = 0;
00356     speedLeft = 0;
00357     
00358     //Zustand, dass der Roboter nicht gestoppt hat
00359     stopRight = false;
00360     stopLeft = false;
00361     
00362     //Drehrichtung der Motoren
00363     controller.setDesiredSpeedRight(speedRight);
00364     controller.setDesiredSpeedLeft(-(speedLeft));
00365     
00366     
00367     while(1){
00368        
00369        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
00370        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
00371         
00372         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
00373         if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
00374             controller.setDesiredSpeedRight(0);
00375             stopRight = true;
00376             }
00377             
00378         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
00379         if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
00380             controller.setDesiredSpeedLeft(0);
00381             stopLeft = true;
00382             }
00383      
00384         if(stopRight == true && stopLeft == true){
00385             return;
00386             }  
00387          
00388         }
00389         
00390     }