Roboshark / Mbed 2 deprecated Roboshark_V10

Dependencies:   mbed

Fork of Roboshark_V9 by Roboshark

Committer:
ahlervin
Date:
Fri May 04 16:26:59 2018 +0000
Revision:
7:862d80e0ea2d
Parent:
6:7bbcdd07bc2d
Child:
8:d0a27278c108
Mit Regler 2.0

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