Beste Version
Dependencies: mbed
Fork of Roboshark_V8 by
Fahren.cpp
- Committer:
- ahlervin
- Date:
- 2018-05-09
- Revision:
- 13:1687f97d4d82
- Parent:
- 12:cd07a80f0a9a
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; } } }