Roboshark / Mbed 2 deprecated Roboshark_V10

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Roboshark_V9 by Roboshark

Fahren.cpp@8:d0a27278c108, 2018-05-07 (annotated)

Committer:
ahlervin
Date:
Mon May 07 14:11:06 2018 +0000
Revision:
8:d0a27278c108
Parent:
7:862d80e0ea2d
Child:
9:feabe0b7cea4
funkt regler

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ahlervin 7:862d80e0ea2d 1 /*Roboshark V6
ahlervin 6:7bbcdd07bc2d 2 Fahren.cpp
ahlervin 6:7bbcdd07bc2d 3 Erstellt: J. Blunschi
ahlervin 6:7bbcdd07bc2d 4 geändert: V.Ahlers
ahlervin 6:7bbcdd07bc2d 5 V.5.18
ahlervin 6:7bbcdd07bc2d 6 */
ahlervin 6:7bbcdd07bc2d 7
Jacqueline 0:6d0671ae4648 8 #include "Fahren.h"
Jacqueline 0:6d0671ae4648 9 #include <mbed.h>
ahlervin 6:7bbcdd07bc2d 10 #include "Regler.h"
Jacqueline 0:6d0671ae4648 11
Jacqueline 0:6d0671ae4648 12
ahlervin 6:7bbcdd07bc2d 13 using namespace std;
ahlervin 6:7bbcdd07bc2d 14
ahlervin 8:d0a27278c108 15 const float Fahren :: PERIOD = 0.2f;
ahlervin 6:7bbcdd07bc2d 16
Jacqueline 0:6d0671ae4648 17 //Konstruktor
ahlervin 8:d0a27278c108 18 Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, Regler& regler, int reglerEin, IRSensor& iRSensor):
ahlervin 8:d0a27278c108 19 controller(controller), counterLeft(counterLeft), counterRight(counterRight), regler(regler), reglerEin(reglerEin), iRSensor (iRSensor){
Jacqueline 0:6d0671ae4648 20
ahlervin 8:d0a27278c108 21 SpeedR = 80;
ahlervin 8:d0a27278c108 22 SpeedL = 80;
ahlervin 8:d0a27278c108 23 disF = 0;
ahlervin 6:7bbcdd07bc2d 24 ticker.attach(callback(this, &Fahren::getSpeed), PERIOD);
ahlervin 6:7bbcdd07bc2d 25 }
ahlervin 6:7bbcdd07bc2d 26
Jacqueline 0:6d0671ae4648 27 //Dekonstruktor
ahlervin 6:7bbcdd07bc2d 28 Fahren::~Fahren() {
ahlervin 6:7bbcdd07bc2d 29 ticker.detach();
ahlervin 6:7bbcdd07bc2d 30 }
ahlervin 7:862d80e0ea2d 31
ahlervin 7:862d80e0ea2d 32
ahlervin 6:7bbcdd07bc2d 33 //Korrigierte Geschwindigkeit aus der Klasse Regler aufrufen
ahlervin 6:7bbcdd07bc2d 34 void Fahren::getSpeed(){
ahlervin 8:d0a27278c108 35 //if((reglerEinR == 1) && (reglerEinL == 1)){
ahlervin 8:d0a27278c108 36 //SpeedR = regler.getSpeedR();
ahlervin 8:d0a27278c108 37 //SpeedL = regler.getSpeedL();
ahlervin 8:d0a27278c108 38 SpeedR = 80;
ahlervin 8:d0a27278c108 39 SpeedL = 80;
ahlervin 8:d0a27278c108 40
ahlervin 8:d0a27278c108 41 //disF = iRSensor.readF();
ahlervin 6:7bbcdd07bc2d 42
ahlervin 6:7bbcdd07bc2d 43 //printf("SpeedR in F = %f\n",SpeedR);
ahlervin 6:7bbcdd07bc2d 44 //printf("SpeedL in F = %f\n",SpeedL);
ahlervin 6:7bbcdd07bc2d 45 }
Jacqueline 0:6d0671ae4648 46
ahlervin 7:862d80e0ea2d 47
ahlervin 6:7bbcdd07bc2d 48 //Implementation der Methode geradeaus fahren ungeregelt
ahlervin 6:7bbcdd07bc2d 49 void Fahren::geradeausU(){
Jacqueline 0:6d0671ae4648 50
Jacqueline 0:6d0671ae4648 51 //einlesen des aktuellen Encoder standes
Jacqueline 0:6d0671ae4648 52 initialClicksRight = counterRight.read();
Jacqueline 0:6d0671ae4648 53 initialClicksLeft = counterLeft.read();
Jacqueline 0:6d0671ae4648 54
Jacqueline 0:6d0671ae4648 55 //Anzahl Clicks die der Encoder zählen soll
ahlervin 6:7bbcdd07bc2d 56 wegRechts = 1773;
ahlervin 6:7bbcdd07bc2d 57 wegLinks = 1773;
Jacqueline 0:6d0671ae4648 58
Jacqueline 0:6d0671ae4648 59 //Geschwindigkeit
ahlervin 7:862d80e0ea2d 60 //speedRight = 50;
ahlervin 7:862d80e0ea2d 61 //speedLeft = 50;
Jacqueline 0:6d0671ae4648 62
Jacqueline 0:6d0671ae4648 63 //Zustand, dass der Roboter nicht gestoppt hat
Jacqueline 0:6d0671ae4648 64 stopRight = false;
Jacqueline 0:6d0671ae4648 65 stopLeft = false;
ahlervin 8:d0a27278c108 66
Jacqueline 0:6d0671ae4648 67
Jacqueline 0:6d0671ae4648 68 //Drehrichtung der Motoren
ahlervin 7:862d80e0ea2d 69 controller.setDesiredSpeedRight(SpeedR);
ahlervin 7:862d80e0ea2d 70 controller.setDesiredSpeedLeft(-(SpeedL));
Jacqueline 0:6d0671ae4648 71
Jacqueline 0:6d0671ae4648 72
Jacqueline 0:6d0671ae4648 73 while(1){
Jacqueline 0:6d0671ae4648 74
Jacqueline 0:6d0671ae4648 75 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 76 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 77
Jacqueline 0:6d0671ae4648 78 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 79 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
Jacqueline 0:6d0671ae4648 80 controller.setDesiredSpeedRight(0);
Jacqueline 0:6d0671ae4648 81 stopRight = true;
Jacqueline 0:6d0671ae4648 82 }
Jacqueline 0:6d0671ae4648 83
Jacqueline 0:6d0671ae4648 84 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 85 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
Jacqueline 0:6d0671ae4648 86 controller.setDesiredSpeedLeft(0);
Jacqueline 0:6d0671ae4648 87 stopLeft = true;
Jacqueline 0:6d0671ae4648 88 }
Jacqueline 0:6d0671ae4648 89
Jacqueline 0:6d0671ae4648 90 if(stopRight == true && stopLeft == true){
Jacqueline 0:6d0671ae4648 91 return;
Jacqueline 0:6d0671ae4648 92 }
Jacqueline 0:6d0671ae4648 93
Jacqueline 0:6d0671ae4648 94 }
Jacqueline 0:6d0671ae4648 95
Jacqueline 0:6d0671ae4648 96 }
Jacqueline 0:6d0671ae4648 97
ahlervin 6:7bbcdd07bc2d 98 //Implementation der Methode geradeaus fahren geregelt
ahlervin 6:7bbcdd07bc2d 99 void Fahren::geradeausG(){
ahlervin 6:7bbcdd07bc2d 100
ahlervin 6:7bbcdd07bc2d 101 //einlesen des aktuellen Encoder standes
ahlervin 6:7bbcdd07bc2d 102 initialClicksRight = counterRight.read();
ahlervin 6:7bbcdd07bc2d 103 initialClicksLeft = counterLeft.read();
ahlervin 6:7bbcdd07bc2d 104
ahlervin 6:7bbcdd07bc2d 105 //Anzahl Clicks die der Encoder zählen soll
ahlervin 8:d0a27278c108 106 wegRechts = 1776;
ahlervin 8:d0a27278c108 107 wegLinks = 1776;
ahlervin 6:7bbcdd07bc2d 108
ahlervin 6:7bbcdd07bc2d 109 //Geschwindigkeit
ahlervin 6:7bbcdd07bc2d 110 //speedRight = 50;//SpeedR
ahlervin 6:7bbcdd07bc2d 111 //speedLeft = 50;//SpeedL
ahlervin 6:7bbcdd07bc2d 112
ahlervin 7:862d80e0ea2d 113
ahlervin 6:7bbcdd07bc2d 114
ahlervin 6:7bbcdd07bc2d 115 //Zustand, dass der Roboter nicht gestoppt hat
ahlervin 6:7bbcdd07bc2d 116 stopRight = false;
ahlervin 6:7bbcdd07bc2d 117 stopLeft = false;
ahlervin 8:d0a27278c108 118 reglerEinR = 1;
ahlervin 8:d0a27278c108 119 reglerEinL = 1;
ahlervin 6:7bbcdd07bc2d 120
ahlervin 6:7bbcdd07bc2d 121 //Drehrichtung der Motoren
ahlervin 8:d0a27278c108 122
ahlervin 8:d0a27278c108 123 //printf("SpeedR in F2= %f\n",SpeedR);
ahlervin 8:d0a27278c108 124 //printf("SpeedL in F2= %f\n",SpeedL);
Jacqueline 0:6d0671ae4648 125
ahlervin 6:7bbcdd07bc2d 126 while(1){
ahlervin 8:d0a27278c108 127 float diff = (iRSensor.readR() - iRSensor.readL())*0.9f;
ahlervin 8:d0a27278c108 128 if(diff > 7.0f) diff = 0;
ahlervin 8:d0a27278c108 129 if(iRSensor.readR()>60.0f) diff=0;
ahlervin 8:d0a27278c108 130 if(iRSensor.readL()>60.0f) diff=0;
ahlervin 8:d0a27278c108 131 //printf("diff =%f \n",diff);
ahlervin 8:d0a27278c108 132 controller.setDesiredSpeedRight(SpeedR+diff);
ahlervin 8:d0a27278c108 133 controller.setDesiredSpeedLeft(-(SpeedL-diff));
ahlervin 8:d0a27278c108 134
ahlervin 8:d0a27278c108 135 wait(0.05);
ahlervin 6:7bbcdd07bc2d 136
ahlervin 6:7bbcdd07bc2d 137 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
ahlervin 6:7bbcdd07bc2d 138 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
ahlervin 6:7bbcdd07bc2d 139
ahlervin 6:7bbcdd07bc2d 140 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
ahlervin 8:d0a27278c108 141 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts || iRSensor.readF() < 50.0f){
ahlervin 6:7bbcdd07bc2d 142 controller.setDesiredSpeedRight(0);
ahlervin 6:7bbcdd07bc2d 143 stopRight = true;
ahlervin 8:d0a27278c108 144 reglerEinR = 0;
ahlervin 6:7bbcdd07bc2d 145 }
ahlervin 6:7bbcdd07bc2d 146
ahlervin 6:7bbcdd07bc2d 147 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
ahlervin 8:d0a27278c108 148 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks || iRSensor.readF() < 50.0f){
ahlervin 6:7bbcdd07bc2d 149 controller.setDesiredSpeedLeft(0);
ahlervin 6:7bbcdd07bc2d 150 stopLeft = true;
ahlervin 8:d0a27278c108 151 reglerEinL = 0;
ahlervin 6:7bbcdd07bc2d 152 }
ahlervin 6:7bbcdd07bc2d 153
ahlervin 6:7bbcdd07bc2d 154 if(stopRight == true && stopLeft == true){
ahlervin 6:7bbcdd07bc2d 155 return;
ahlervin 6:7bbcdd07bc2d 156 }
ahlervin 6:7bbcdd07bc2d 157
ahlervin 6:7bbcdd07bc2d 158 }
ahlervin 6:7bbcdd07bc2d 159
ahlervin 6:7bbcdd07bc2d 160 }
ahlervin 6:7bbcdd07bc2d 161
Jacqueline 0:6d0671ae4648 162 //Implementation der Methode 90 Grad Rechtsdrehen
Jacqueline 0:6d0671ae4648 163 void Fahren::rechts90() {
Jacqueline 0:6d0671ae4648 164
Jacqueline 0:6d0671ae4648 165 //einlesen des aktuellen Encoder standes
Jacqueline 0:6d0671ae4648 166 initialClicksRight = counterRight.read();
Jacqueline 0:6d0671ae4648 167 initialClicksLeft = counterLeft.read();
Jacqueline 0:6d0671ae4648 168
Jacqueline 0:6d0671ae4648 169 //Anzahl Clicks die der Encoder zählen soll
ahlervin 7:862d80e0ea2d 170 wegRechts = 868;
ahlervin 7:862d80e0ea2d 171 wegLinks = 868;
Jacqueline 0:6d0671ae4648 172
Jacqueline 0:6d0671ae4648 173 //Geschwindigkeit
ahlervin 7:862d80e0ea2d 174 //speedRight = 50;
ahlervin 7:862d80e0ea2d 175 //speedLeft = 50;
Jacqueline 0:6d0671ae4648 176
Jacqueline 0:6d0671ae4648 177 //Zustand, dass der Roboter nicht gestoppt hat
Jacqueline 0:6d0671ae4648 178 stopRight = false;
Jacqueline 0:6d0671ae4648 179 stopLeft = false;
Jacqueline 0:6d0671ae4648 180
Jacqueline 0:6d0671ae4648 181 //Drehrichtung der Motoren
ahlervin 7:862d80e0ea2d 182 controller.setDesiredSpeedRight((SpeedR));
ahlervin 7:862d80e0ea2d 183 controller.setDesiredSpeedLeft((SpeedL));
ahlervin 7:862d80e0ea2d 184 //printf("SpeedR in F = %f\n",SpeedR);
ahlervin 7:862d80e0ea2d 185 //printf("SpeedL in F = %f\n",SpeedL);
Jacqueline 0:6d0671ae4648 186 while(1){
Jacqueline 0:6d0671ae4648 187
Jacqueline 0:6d0671ae4648 188 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 189 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 190
Jacqueline 0:6d0671ae4648 191 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 192 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
Jacqueline 0:6d0671ae4648 193 controller.setDesiredSpeedRight(0);
Jacqueline 0:6d0671ae4648 194 stopRight = true;
Jacqueline 0:6d0671ae4648 195 }
Jacqueline 0:6d0671ae4648 196
Jacqueline 0:6d0671ae4648 197 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 198 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
Jacqueline 0:6d0671ae4648 199 controller.setDesiredSpeedLeft(0);
Jacqueline 0:6d0671ae4648 200 stopLeft = true;
Jacqueline 0:6d0671ae4648 201 }
Jacqueline 0:6d0671ae4648 202
Jacqueline 0:6d0671ae4648 203 if(stopRight == true && stopLeft == true){
Jacqueline 0:6d0671ae4648 204 return;
Jacqueline 0:6d0671ae4648 205 }
Jacqueline 0:6d0671ae4648 206
Jacqueline 0:6d0671ae4648 207 }
Jacqueline 0:6d0671ae4648 208
Jacqueline 0:6d0671ae4648 209 }
Jacqueline 0:6d0671ae4648 210
Jacqueline 0:6d0671ae4648 211
Jacqueline 0:6d0671ae4648 212
Jacqueline 0:6d0671ae4648 213 //Implementation der Methode 180 Grad Rechtsdrehen
Jacqueline 0:6d0671ae4648 214 void Fahren::rechts180() {
Jacqueline 0:6d0671ae4648 215
Jacqueline 0:6d0671ae4648 216 //einlesen des aktuellen Encoder standes
Jacqueline 0:6d0671ae4648 217 initialClicksRight = counterRight.read();
Jacqueline 0:6d0671ae4648 218 initialClicksLeft = counterLeft.read();
Jacqueline 0:6d0671ae4648 219
Jacqueline 0:6d0671ae4648 220 //Anzahl Clicks die der Encoder zählen soll
ahlervin 7:862d80e0ea2d 221 wegRechts = 1752;
ahlervin 7:862d80e0ea2d 222 wegLinks = 1752;
Jacqueline 0:6d0671ae4648 223
Jacqueline 0:6d0671ae4648 224 //Geschwindigkeit
ahlervin 7:862d80e0ea2d 225 //speedRight = 50;
ahlervin 7:862d80e0ea2d 226 //speedLeft = 50;
Jacqueline 0:6d0671ae4648 227
Jacqueline 0:6d0671ae4648 228 //Zustand, dass der Roboter nicht gestoppt hat
Jacqueline 0:6d0671ae4648 229 stopRight = false;
Jacqueline 0:6d0671ae4648 230 stopLeft = false;
Jacqueline 0:6d0671ae4648 231
Jacqueline 0:6d0671ae4648 232 //Drehrichtung der Motoren
ahlervin 7:862d80e0ea2d 233 controller.setDesiredSpeedRight((SpeedR));
ahlervin 7:862d80e0ea2d 234 controller.setDesiredSpeedLeft((SpeedL));
Jacqueline 0:6d0671ae4648 235
Jacqueline 0:6d0671ae4648 236 while(1){
Jacqueline 0:6d0671ae4648 237
Jacqueline 0:6d0671ae4648 238 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 239 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 240
Jacqueline 0:6d0671ae4648 241 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 242 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
Jacqueline 0:6d0671ae4648 243 controller.setDesiredSpeedRight(0);
Jacqueline 0:6d0671ae4648 244 stopRight = true;
Jacqueline 0:6d0671ae4648 245 }
Jacqueline 0:6d0671ae4648 246
Jacqueline 0:6d0671ae4648 247 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 248 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
Jacqueline 0:6d0671ae4648 249 controller.setDesiredSpeedLeft(0);
Jacqueline 0:6d0671ae4648 250 stopLeft = true;
Jacqueline 0:6d0671ae4648 251 }
Jacqueline 0:6d0671ae4648 252
Jacqueline 0:6d0671ae4648 253 if(stopRight == true && stopLeft == true){
Jacqueline 0:6d0671ae4648 254 return;
Jacqueline 0:6d0671ae4648 255 }
Jacqueline 0:6d0671ae4648 256
Jacqueline 0:6d0671ae4648 257 }
Jacqueline 0:6d0671ae4648 258
Jacqueline 0:6d0671ae4648 259 }
Jacqueline 0:6d0671ae4648 260
Jacqueline 0:6d0671ae4648 261
Jacqueline 2:402b1a74fff6 262 //Implementation der Methode 90 Grad Linksdrehen
Jacqueline 2:402b1a74fff6 263 void Fahren::links90() {
Jacqueline 0:6d0671ae4648 264
Jacqueline 0:6d0671ae4648 265 //einlesen des aktuellen Encoder standes
Jacqueline 0:6d0671ae4648 266 initialClicksRight = counterRight.read();
Jacqueline 0:6d0671ae4648 267 initialClicksLeft = counterLeft.read();
Jacqueline 0:6d0671ae4648 268
Jacqueline 0:6d0671ae4648 269 //Anzahl Clicks die der Encoder zählen soll
ahlervin 8:d0a27278c108 270 wegRechts = 868;
ahlervin 8:d0a27278c108 271 wegLinks = 868;
Jacqueline 0:6d0671ae4648 272
Jacqueline 0:6d0671ae4648 273 //Geschwindigkeit
ahlervin 7:862d80e0ea2d 274 //speedRight = 50;
ahlervin 7:862d80e0ea2d 275 //speedLeft = 50;
Jacqueline 0:6d0671ae4648 276
Jacqueline 0:6d0671ae4648 277 //Zustand, dass der Roboter nicht gestoppt hat
Jacqueline 0:6d0671ae4648 278 stopRight = false;
Jacqueline 0:6d0671ae4648 279 stopLeft = false;
Jacqueline 0:6d0671ae4648 280
Jacqueline 0:6d0671ae4648 281 //Drehrichtung der Motoren
ahlervin 7:862d80e0ea2d 282 controller.setDesiredSpeedRight((-SpeedR));
ahlervin 7:862d80e0ea2d 283 controller.setDesiredSpeedLeft((-SpeedL));
Jacqueline 0:6d0671ae4648 284
Jacqueline 0:6d0671ae4648 285 while(1){
Jacqueline 0:6d0671ae4648 286
Jacqueline 0:6d0671ae4648 287 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 288 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 289
Jacqueline 0:6d0671ae4648 290 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 291 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
Jacqueline 0:6d0671ae4648 292 controller.setDesiredSpeedRight(0);
Jacqueline 0:6d0671ae4648 293 stopRight = true;
Jacqueline 0:6d0671ae4648 294 }
Jacqueline 0:6d0671ae4648 295
Jacqueline 0:6d0671ae4648 296 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 297 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
Jacqueline 0:6d0671ae4648 298 controller.setDesiredSpeedLeft(0);
Jacqueline 0:6d0671ae4648 299 stopLeft = true;
Jacqueline 0:6d0671ae4648 300 }
Jacqueline 0:6d0671ae4648 301
Jacqueline 0:6d0671ae4648 302 if(stopRight == true && stopLeft == true){
Jacqueline 0:6d0671ae4648 303 return;
Jacqueline 0:6d0671ae4648 304 }
Jacqueline 0:6d0671ae4648 305
Jacqueline 0:6d0671ae4648 306 }
Jacqueline 0:6d0671ae4648 307
Jacqueline 0:6d0671ae4648 308 }
Jacqueline 0:6d0671ae4648 309
ahlervin 4:767fd282dd9c 310
ahlervin 4:767fd282dd9c 311 //Implementation der Methode Ziel erreicht
ahlervin 4:767fd282dd9c 312 void Fahren::ziel() {
ahlervin 4:767fd282dd9c 313
ahlervin 4:767fd282dd9c 314 //einlesen des aktuellen Encoder standes
ahlervin 4:767fd282dd9c 315 initialClicksRight = counterRight.read();
ahlervin 4:767fd282dd9c 316 initialClicksLeft = counterLeft.read();
Jacqueline 0:6d0671ae4648 317
ahlervin 4:767fd282dd9c 318 //Anzahl Clicks die der Encoder zählen soll
ahlervin 4:767fd282dd9c 319 wegRechts = 7050;
ahlervin 4:767fd282dd9c 320 wegLinks = 7050;
ahlervin 4:767fd282dd9c 321
ahlervin 4:767fd282dd9c 322 //Geschwindigkeit
ahlervin 7:862d80e0ea2d 323 //speedRight = 80;
ahlervin 7:862d80e0ea2d 324 //speedLeft = 80;
ahlervin 4:767fd282dd9c 325
ahlervin 4:767fd282dd9c 326 //Zustand, dass der Roboter nicht gestoppt hat
ahlervin 4:767fd282dd9c 327 stopRight = false;
ahlervin 4:767fd282dd9c 328 stopLeft = false;
ahlervin 4:767fd282dd9c 329
ahlervin 4:767fd282dd9c 330 //Drehrichtung der Motoren
ahlervin 7:862d80e0ea2d 331 controller.setDesiredSpeedRight((SpeedR));
ahlervin 7:862d80e0ea2d 332 controller.setDesiredSpeedLeft((SpeedR));
ahlervin 4:767fd282dd9c 333
ahlervin 4:767fd282dd9c 334 while(1){
ahlervin 4:767fd282dd9c 335
ahlervin 4:767fd282dd9c 336 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
ahlervin 4:767fd282dd9c 337 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
ahlervin 4:767fd282dd9c 338
ahlervin 4:767fd282dd9c 339 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
ahlervin 4:767fd282dd9c 340 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
ahlervin 4:767fd282dd9c 341 controller.setDesiredSpeedRight(0);
ahlervin 4:767fd282dd9c 342 stopRight = true;
ahlervin 4:767fd282dd9c 343 }
ahlervin 4:767fd282dd9c 344
ahlervin 4:767fd282dd9c 345 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
ahlervin 4:767fd282dd9c 346 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
ahlervin 4:767fd282dd9c 347 controller.setDesiredSpeedLeft(0);
ahlervin 4:767fd282dd9c 348 stopLeft = true;
ahlervin 4:767fd282dd9c 349 }
ahlervin 4:767fd282dd9c 350
ahlervin 4:767fd282dd9c 351 if(stopRight == true && stopLeft == true){
ahlervin 4:767fd282dd9c 352 return;
ahlervin 4:767fd282dd9c 353 }
ahlervin 4:767fd282dd9c 354 }
ahlervin 4:767fd282dd9c 355
ahlervin 4:767fd282dd9c 356 }
Jacqueline 0:6d0671ae4648 357
ahlervin 4:767fd282dd9c 358 //Implementation der Methode stopp
ahlervin 4:767fd282dd9c 359 void Fahren::stopp(){
ahlervin 4:767fd282dd9c 360
ahlervin 4:767fd282dd9c 361 //einlesen des aktuellen Encoder standes
ahlervin 4:767fd282dd9c 362 initialClicksRight = counterRight.read();
ahlervin 4:767fd282dd9c 363 initialClicksLeft = counterLeft.read();
ahlervin 4:767fd282dd9c 364
ahlervin 4:767fd282dd9c 365 //Anzahl Clicks die der Encoder zählen soll
ahlervin 4:767fd282dd9c 366 wegRechts = 0;
ahlervin 4:767fd282dd9c 367 wegLinks = 0;
ahlervin 4:767fd282dd9c 368
ahlervin 4:767fd282dd9c 369 //Geschwindigkeit
ahlervin 4:767fd282dd9c 370 speedRight = 0;
ahlervin 4:767fd282dd9c 371 speedLeft = 0;
ahlervin 4:767fd282dd9c 372
ahlervin 4:767fd282dd9c 373 //Zustand, dass der Roboter nicht gestoppt hat
ahlervin 4:767fd282dd9c 374 stopRight = false;
ahlervin 4:767fd282dd9c 375 stopLeft = false;
ahlervin 4:767fd282dd9c 376
ahlervin 4:767fd282dd9c 377 //Drehrichtung der Motoren
ahlervin 4:767fd282dd9c 378 controller.setDesiredSpeedRight(speedRight);
ahlervin 4:767fd282dd9c 379 controller.setDesiredSpeedLeft(-(speedLeft));
ahlervin 4:767fd282dd9c 380
ahlervin 4:767fd282dd9c 381
ahlervin 4:767fd282dd9c 382 while(1){
ahlervin 4:767fd282dd9c 383
ahlervin 4:767fd282dd9c 384 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
ahlervin 4:767fd282dd9c 385 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
ahlervin 4:767fd282dd9c 386
ahlervin 4:767fd282dd9c 387 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
ahlervin 4:767fd282dd9c 388 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
ahlervin 4:767fd282dd9c 389 controller.setDesiredSpeedRight(0);
ahlervin 4:767fd282dd9c 390 stopRight = true;
ahlervin 4:767fd282dd9c 391 }
ahlervin 4:767fd282dd9c 392
ahlervin 4:767fd282dd9c 393 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
ahlervin 4:767fd282dd9c 394 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
ahlervin 4:767fd282dd9c 395 controller.setDesiredSpeedLeft(0);
ahlervin 4:767fd282dd9c 396 stopLeft = true;
ahlervin 4:767fd282dd9c 397 }
ahlervin 4:767fd282dd9c 398
ahlervin 4:767fd282dd9c 399 if(stopRight == true && stopLeft == true){
ahlervin 4:767fd282dd9c 400 return;
ahlervin 4:767fd282dd9c 401 }
ahlervin 4:767fd282dd9c 402
ahlervin 4:767fd282dd9c 403 }
ahlervin 4:767fd282dd9c 404
ahlervin 4:767fd282dd9c 405 }