File content as of revision 13:1687f97d4d82:

/*Roboshark V7
Fahren.cpp
Erstellt: J. Blunschi
geändert: V.Ahlers
V.5.18
*/

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


using namespace std;

const float Fahren :: PERIOD = 5.0f;

//Konstruktor
Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, IRSensor& iRSensor):
    controller(controller), counterLeft(counterLeft), counterRight(counterRight), iRSensor (iRSensor){
SpeedR = 120.0f; //120
SpeedL = 120.0f; //120
disF = 0.0f;
//ticker.attach(callback(this, &Fahren::reset), PERIOD);
}
//Dekonstruktor
Fahren::~Fahren() {
    //ticker.detach();
    }
    
//void Fahren::reset(){
//controller.resetEncoder();
//}

//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 = 1675;
    wegLinks = 1675;

    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    reglerEinR = 1;
    reglerEinL = 1;
    
    while(1){
    float diff = (iRSensor.readR() - iRSensor.readL())*0.81f;            //Regler 0.81
    if(iRSensor.readR()>80.0f) diff=0;
    if(iRSensor.readL()>80.0f) diff=0;
    //printf("diff =%f \n",diff);
    controller.setDesiredSpeedRight(SpeedR+diff);
    controller.setDesiredSpeedLeft(-(SpeedL-diff));
    
    wait(0.02); //0.02
       
       // 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 || iRSensor.readF() < 50.0f){    
            controller.setDesiredSpeedRight(0);
            stopRight = true;
            reglerEinR = 0;
            }
            
        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks || iRSensor.readF() < 50.0f){
            controller.setDesiredSpeedLeft(0);
            stopLeft = true;
            reglerEinL = 0;
            }
     
        if(stopRight == true && stopLeft == true){
            //controller.resetEncoder();
            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 = 868;
    wegLinks = 868;
    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    
    //Drehrichtung der Motoren
    controller.setDesiredSpeedRight((SpeedR));
    controller.setDesiredSpeedLeft((SpeedL));
    //printf("SpeedR in F = %f\n",SpeedR);
    //printf("SpeedL in F = %f\n",SpeedL);
    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 = 1752;
    wegLinks = 1752;
    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    
    //Drehrichtung der Motoren
    controller.setDesiredSpeedRight((SpeedR));
    controller.setDesiredSpeedLeft((SpeedL));
    
    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 = 868;
    wegLinks = 868;
    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    
    //Drehrichtung der Motoren
    controller.setDesiredSpeedRight((-SpeedR));
    controller.setDesiredSpeedLeft((-SpeedL));
    
    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;
    
    //Zustand, dass der Roboter nicht gestoppt hat
    stopRight = false;
    stopLeft = false;
    
    //Drehrichtung der Motoren
    controller.setDesiredSpeedRight((SpeedR));
    controller.setDesiredSpeedLeft((SpeedR));
    
    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;
            }  
         
        }
        
    }