File content as of revision 7:b2a16b1cf487:

#include "Fahren.h"
#include <mbed.h>
#include "Regler.h"
#include "Fahren.h"


using namespace std;

const float Fahren :: PERIOD = 2.0f;

//Konstruktor
Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight):
    controller(controller), counterLeft(counterLeft), counterRight(counterRight), regler(regler){
    
    SpeedR = 0;
    SpeedL = 0;
    ticker.attach(callback(this, &Fahren::getSpeed), PERIOD);
}

//Dekonstruktor
Fahren::~Fahren() {
    ticker.detach();
    }

void Fahren::getSpeed(){
    SpeedR = regler.get_SpeedR();
    SpeedL = regler.get_SpeedL();
    
    printf("SpeedR in F = %f\n",SpeedR);
    printf("SpeedL in F = %f\n",SpeedL);
}

  

//Implementation der Methode geradeaus fahren ungeregelt
void Fahren::geradeausU(){
    
    //einlesen des aktuellen Encoder standes
    initialClicksRight = counterRight.read();   
    initialClicksLeft = counterLeft.read();
    
    //Anzahl Clicks die der Encoder zählen soll
    wegRechts = 1767;
    wegLinks = 1767;
        
    //Geschwindigkeit
    speedRight = 25;
    speedLeft = 25;
    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    
    //Drehrichtung der Motoren
    controller.setDesiredSpeedRight(speedRight);
    controller.setDesiredSpeedLeft(-(speedLeft));
    
    
    while(1){
       
       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
        
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
            controller.setDesiredSpeedRight(0);
            stopRight = true;
            }
            
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
            controller.setDesiredSpeedLeft(0);
            stopLeft = true;
            }
     
        if(stopRight == true && stopLeft == true){
            return;
            }  
         
        }
        
    }
    
//Implementation der Methode geradeaus fahren geregelt
void Fahren::geradeausG(){
    
    //einlesen des aktuellen Encoder standes
    initialClicksRight = counterRight.read();   
    initialClicksLeft = counterLeft.read();
    
    //Anzahl Clicks die der Encoder zählen soll
    wegRechts = 1767;
    wegLinks = 1767;
        
    //Geschwindigkeit
    speedRight = 20; // SpeedR
    speedLeft = 20; // SpeedL
    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    
    //Drehrichtung der Motoren
    controller.setDesiredSpeedRight(speedRight);
    controller.setDesiredSpeedLeft(-(speedLeft));
    
    
    while(1){
       
       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
        
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
            controller.setDesiredSpeedRight(0);
            stopRight = true;
            }
            
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
            controller.setDesiredSpeedLeft(0);
            stopLeft = true;
            }
     
        if(stopRight == true && stopLeft == true){
            return;
            }  
         
        }
        
    }

    
//Implementation der Methode 90 Grad Rechtsdrehen
void Fahren::rechts90() {
        
    //einlesen des aktuellen Encoder standes
    initialClicksRight = counterRight.read();   
    initialClicksLeft = counterLeft.read();
    
    //Anzahl Clicks die der Encoder zählen soll
    wegRechts = 878;
    wegLinks = 878;
    
    //Geschwindigkeit
    speedRight = 50;
    speedLeft = 50;
    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    
    //Drehrichtung der Motoren
    controller.setDesiredSpeedRight((speedRight));
    controller.setDesiredSpeedLeft((speedLeft));
    
    while(1){
       
       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
        
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
            controller.setDesiredSpeedRight(0);
            stopRight = true;
            }
            
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
            controller.setDesiredSpeedLeft(0);
            stopLeft = true;
            }
     
        if(stopRight == true && stopLeft == true){
            return;
            }  
         
        }
        
    }
    
    
    
//Implementation der Methode 180 Grad Rechtsdrehen
void Fahren::rechts180() {
        
    //einlesen des aktuellen Encoder standes
    initialClicksRight = counterRight.read();   
    initialClicksLeft = counterLeft.read();
    
    //Anzahl Clicks die der Encoder zählen soll
    wegRechts = 1755;
    wegLinks = 1755;
    
    //Geschwindigkeit
    speedRight = 50;
    speedLeft = 50;
    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    
    //Drehrichtung der Motoren
    controller.setDesiredSpeedRight((speedRight));
    controller.setDesiredSpeedLeft((speedLeft));
    
    while(1){
       
       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
        
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
            controller.setDesiredSpeedRight(0);
            stopRight = true;
            }
            
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
            controller.setDesiredSpeedLeft(0);
            stopLeft = true;
            }
     
        if(stopRight == true && stopLeft == true){
            return;
            }  
         
        }
        
    }
    

//Implementation der Methode 90 Grad Linksdrehen
void Fahren::links90() {
        
    //einlesen des aktuellen Encoder standes
    initialClicksRight = counterRight.read();   
    initialClicksLeft = counterLeft.read();
    
    //Anzahl Clicks die der Encoder zählen soll
    wegRechts = 878;
    wegLinks = 878;
    
    //Geschwindigkeit
    speedRight = 50;
    speedLeft = 50;
    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    
    //Drehrichtung der Motoren
    controller.setDesiredSpeedRight((-speedRight));
    controller.setDesiredSpeedLeft((-speedLeft));
    
    while(1){
       
       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
        
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
            controller.setDesiredSpeedRight(0);
            stopRight = true;
            }
            
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
            controller.setDesiredSpeedLeft(0);
            stopLeft = true;
            }
     
        if(stopRight == true && stopLeft == true){
            return;
            }  
         
        }
        
    }
    
        
//Implementation der Methode Ziel erreicht
void Fahren::ziel() {
        
    //einlesen des aktuellen Encoder standes
    initialClicksRight = counterRight.read();   
    initialClicksLeft = counterLeft.read();
    
    //Anzahl Clicks die der Encoder zählen soll
    wegRechts = 7050;
    wegLinks = 7050;
    
    //Geschwindigkeit
    speedRight = 80;
    speedLeft = 80;
    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    
    //Drehrichtung der Motoren
    controller.setDesiredSpeedRight((speedRight));
    controller.setDesiredSpeedLeft((speedLeft));
    
    while(1){
       
       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
        
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
            controller.setDesiredSpeedRight(0);
            stopRight = true;
            }
            
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
            controller.setDesiredSpeedLeft(0);
            stopLeft = true;
            }
     
        if(stopRight == true && stopLeft == true){
            return;  
            } 
        }
        
    }

//Implementation der Methode stopp
void Fahren::stopp(){
    
    //einlesen des aktuellen Encoder standes
    initialClicksRight = counterRight.read();   
    initialClicksLeft = counterLeft.read();
    
    //Anzahl Clicks die der Encoder zählen soll
    wegRechts = 0;
    wegLinks = 0;
        
    //Geschwindigkeit
    speedRight = 0;
    speedLeft = 0;
    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    
    //Drehrichtung der Motoren
    controller.setDesiredSpeedRight(speedRight);
    controller.setDesiredSpeedLeft(-(speedLeft));
    
    
    while(1){
       
       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
        
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
            controller.setDesiredSpeedRight(0);
            stopRight = true;
            }
            
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
            controller.setDesiredSpeedLeft(0);
            stopLeft = true;
            }
     
        if(stopRight == true && stopLeft == true){
            return;
            }  
         
        }
        
    }