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