Roboshark / Mbed 2 deprecated Roboshark_V3

inkl Line Sensor

Dependencies:   mbed

Fork of Roboshark_V2 by Roboshark

Fahren.cpp@5:e715d157ced5, 2018-04-26 (annotated)

Committer:
ahlervin
Date:
Thu Apr 26 05:58:07 2018 +0000
Revision:
5:e715d157ced5
Parent:
4:767fd282dd9c 
bug fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jacqueline 0:6d0671ae4648 1 #include "Fahren.h"
Jacqueline 0:6d0671ae4648 2 #include <mbed.h>
Jacqueline 0:6d0671ae4648 3
Jacqueline 0:6d0671ae4648 4
Jacqueline 0:6d0671ae4648 5 //Konstruktor
Jacqueline 0:6d0671ae4648 6 Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight):
Jacqueline 0:6d0671ae4648 7 controller(controller), counterLeft(counterLeft), counterRight(counterRight)
Jacqueline 0:6d0671ae4648 8
Jacqueline 0:6d0671ae4648 9 {}
Jacqueline 0:6d0671ae4648 10 //Dekonstruktor
Jacqueline 0:6d0671ae4648 11 Fahren::~Fahren() {}
Jacqueline 0:6d0671ae4648 12
Jacqueline 0:6d0671ae4648 13
Jacqueline 0:6d0671ae4648 14
Jacqueline 0:6d0671ae4648 15 //Implementation der Methode geradeaus fahren
Jacqueline 0:6d0671ae4648 16 void Fahren::geradeaus(){
Jacqueline 0:6d0671ae4648 17
Jacqueline 0:6d0671ae4648 18 //einlesen des aktuellen Encoder standes
Jacqueline 0:6d0671ae4648 19 initialClicksRight = counterRight.read();
Jacqueline 0:6d0671ae4648 20 initialClicksLeft = counterLeft.read();
Jacqueline 0:6d0671ae4648 21
Jacqueline 0:6d0671ae4648 22 //Anzahl Clicks die der Encoder zählen soll
Jacqueline 0:6d0671ae4648 23 wegRechts = 1767;
Jacqueline 0:6d0671ae4648 24 wegLinks = 1767;
Jacqueline 0:6d0671ae4648 25
Jacqueline 0:6d0671ae4648 26 //Geschwindigkeit
ahlervin 4:767fd282dd9c 27 speedRight = 25;
ahlervin 4:767fd282dd9c 28 speedLeft = 25;
Jacqueline 0:6d0671ae4648 29
Jacqueline 0:6d0671ae4648 30 //Zustand, dass der Roboter nicht gestoppt hat
Jacqueline 0:6d0671ae4648 31 stopRight = false;
Jacqueline 0:6d0671ae4648 32 stopLeft = false;
Jacqueline 0:6d0671ae4648 33
Jacqueline 0:6d0671ae4648 34 //Drehrichtung der Motoren
Jacqueline 0:6d0671ae4648 35 controller.setDesiredSpeedRight(speedRight);
Jacqueline 0:6d0671ae4648 36 controller.setDesiredSpeedLeft(-(speedLeft));
Jacqueline 0:6d0671ae4648 37
Jacqueline 0:6d0671ae4648 38
Jacqueline 0:6d0671ae4648 39 while(1){
Jacqueline 0:6d0671ae4648 40
Jacqueline 0:6d0671ae4648 41 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 42 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 43
Jacqueline 0:6d0671ae4648 44 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 45 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
Jacqueline 0:6d0671ae4648 46 controller.setDesiredSpeedRight(0);
Jacqueline 0:6d0671ae4648 47 stopRight = true;
Jacqueline 0:6d0671ae4648 48 }
Jacqueline 0:6d0671ae4648 49
Jacqueline 0:6d0671ae4648 50 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 51 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
Jacqueline 0:6d0671ae4648 52 controller.setDesiredSpeedLeft(0);
Jacqueline 0:6d0671ae4648 53 stopLeft = true;
Jacqueline 0:6d0671ae4648 54 }
Jacqueline 0:6d0671ae4648 55
Jacqueline 0:6d0671ae4648 56 if(stopRight == true && stopLeft == true){
Jacqueline 0:6d0671ae4648 57 return;
Jacqueline 0:6d0671ae4648 58 }
Jacqueline 0:6d0671ae4648 59
Jacqueline 0:6d0671ae4648 60 }
Jacqueline 0:6d0671ae4648 61
Jacqueline 0:6d0671ae4648 62 }
Jacqueline 0:6d0671ae4648 63
Jacqueline 0:6d0671ae4648 64
Jacqueline 0:6d0671ae4648 65
Jacqueline 0:6d0671ae4648 66 //Implementation der Methode 90 Grad Rechtsdrehen
Jacqueline 0:6d0671ae4648 67 void Fahren::rechts90() {
Jacqueline 0:6d0671ae4648 68
Jacqueline 0:6d0671ae4648 69 //einlesen des aktuellen Encoder standes
Jacqueline 0:6d0671ae4648 70 initialClicksRight = counterRight.read();
Jacqueline 0:6d0671ae4648 71 initialClicksLeft = counterLeft.read();
Jacqueline 0:6d0671ae4648 72
Jacqueline 0:6d0671ae4648 73 //Anzahl Clicks die der Encoder zählen soll
Jacqueline 3:e4264cb5b9a9 74 wegRechts = 878;
Jacqueline 3:e4264cb5b9a9 75 wegLinks = 878;
Jacqueline 0:6d0671ae4648 76
Jacqueline 0:6d0671ae4648 77 //Geschwindigkeit
Jacqueline 0:6d0671ae4648 78 speedRight = 50;
Jacqueline 0:6d0671ae4648 79 speedLeft = 50;
Jacqueline 0:6d0671ae4648 80
Jacqueline 0:6d0671ae4648 81 //Zustand, dass der Roboter nicht gestoppt hat
Jacqueline 0:6d0671ae4648 82 stopRight = false;
Jacqueline 0:6d0671ae4648 83 stopLeft = false;
Jacqueline 0:6d0671ae4648 84
Jacqueline 0:6d0671ae4648 85 //Drehrichtung der Motoren
Jacqueline 0:6d0671ae4648 86 controller.setDesiredSpeedRight((speedRight));
Jacqueline 0:6d0671ae4648 87 controller.setDesiredSpeedLeft((speedLeft));
Jacqueline 0:6d0671ae4648 88
Jacqueline 0:6d0671ae4648 89 while(1){
Jacqueline 0:6d0671ae4648 90
Jacqueline 0:6d0671ae4648 91 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 92 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 93
Jacqueline 0:6d0671ae4648 94 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 95 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
Jacqueline 0:6d0671ae4648 96 controller.setDesiredSpeedRight(0);
Jacqueline 0:6d0671ae4648 97 stopRight = true;
Jacqueline 0:6d0671ae4648 98 }
Jacqueline 0:6d0671ae4648 99
Jacqueline 0:6d0671ae4648 100 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 101 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
Jacqueline 0:6d0671ae4648 102 controller.setDesiredSpeedLeft(0);
Jacqueline 0:6d0671ae4648 103 stopLeft = true;
Jacqueline 0:6d0671ae4648 104 }
Jacqueline 0:6d0671ae4648 105
Jacqueline 0:6d0671ae4648 106 if(stopRight == true && stopLeft == true){
Jacqueline 0:6d0671ae4648 107 return;
Jacqueline 0:6d0671ae4648 108 }
Jacqueline 0:6d0671ae4648 109
Jacqueline 0:6d0671ae4648 110 }
Jacqueline 0:6d0671ae4648 111
Jacqueline 0:6d0671ae4648 112 }
Jacqueline 0:6d0671ae4648 113
Jacqueline 0:6d0671ae4648 114
Jacqueline 0:6d0671ae4648 115
Jacqueline 0:6d0671ae4648 116 //Implementation der Methode 180 Grad Rechtsdrehen
Jacqueline 0:6d0671ae4648 117 void Fahren::rechts180() {
Jacqueline 0:6d0671ae4648 118
Jacqueline 0:6d0671ae4648 119 //einlesen des aktuellen Encoder standes
Jacqueline 0:6d0671ae4648 120 initialClicksRight = counterRight.read();
Jacqueline 0:6d0671ae4648 121 initialClicksLeft = counterLeft.read();
Jacqueline 0:6d0671ae4648 122
Jacqueline 0:6d0671ae4648 123 //Anzahl Clicks die der Encoder zählen soll
Jacqueline 0:6d0671ae4648 124 wegRechts = 1755;
Jacqueline 0:6d0671ae4648 125 wegLinks = 1755;
Jacqueline 0:6d0671ae4648 126
Jacqueline 0:6d0671ae4648 127 //Geschwindigkeit
Jacqueline 0:6d0671ae4648 128 speedRight = 50;
Jacqueline 0:6d0671ae4648 129 speedLeft = 50;
Jacqueline 0:6d0671ae4648 130
Jacqueline 0:6d0671ae4648 131 //Zustand, dass der Roboter nicht gestoppt hat
Jacqueline 0:6d0671ae4648 132 stopRight = false;
Jacqueline 0:6d0671ae4648 133 stopLeft = false;
Jacqueline 0:6d0671ae4648 134
Jacqueline 0:6d0671ae4648 135 //Drehrichtung der Motoren
Jacqueline 0:6d0671ae4648 136 controller.setDesiredSpeedRight((speedRight));
Jacqueline 0:6d0671ae4648 137 controller.setDesiredSpeedLeft((speedLeft));
Jacqueline 0:6d0671ae4648 138
Jacqueline 0:6d0671ae4648 139 while(1){
Jacqueline 0:6d0671ae4648 140
Jacqueline 0:6d0671ae4648 141 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 142 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 143
Jacqueline 0:6d0671ae4648 144 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 145 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
Jacqueline 0:6d0671ae4648 146 controller.setDesiredSpeedRight(0);
Jacqueline 0:6d0671ae4648 147 stopRight = true;
Jacqueline 0:6d0671ae4648 148 }
Jacqueline 0:6d0671ae4648 149
Jacqueline 0:6d0671ae4648 150 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 151 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
Jacqueline 0:6d0671ae4648 152 controller.setDesiredSpeedLeft(0);
Jacqueline 0:6d0671ae4648 153 stopLeft = true;
Jacqueline 0:6d0671ae4648 154 }
Jacqueline 0:6d0671ae4648 155
Jacqueline 0:6d0671ae4648 156 if(stopRight == true && stopLeft == true){
Jacqueline 0:6d0671ae4648 157 return;
Jacqueline 0:6d0671ae4648 158 }
Jacqueline 0:6d0671ae4648 159
Jacqueline 0:6d0671ae4648 160 }
Jacqueline 0:6d0671ae4648 161
Jacqueline 0:6d0671ae4648 162 }
Jacqueline 0:6d0671ae4648 163
Jacqueline 0:6d0671ae4648 164
Jacqueline 2:402b1a74fff6 165 //Implementation der Methode 90 Grad Linksdrehen
Jacqueline 2:402b1a74fff6 166 void Fahren::links90() {
Jacqueline 0:6d0671ae4648 167
Jacqueline 0:6d0671ae4648 168 //einlesen des aktuellen Encoder standes
Jacqueline 0:6d0671ae4648 169 initialClicksRight = counterRight.read();
Jacqueline 0:6d0671ae4648 170 initialClicksLeft = counterLeft.read();
Jacqueline 0:6d0671ae4648 171
Jacqueline 0:6d0671ae4648 172 //Anzahl Clicks die der Encoder zählen soll
Jacqueline 2:402b1a74fff6 173 wegRechts = 878;
Jacqueline 2:402b1a74fff6 174 wegLinks = 878;
Jacqueline 0:6d0671ae4648 175
Jacqueline 0:6d0671ae4648 176 //Geschwindigkeit
Jacqueline 0:6d0671ae4648 177 speedRight = 50;
Jacqueline 0:6d0671ae4648 178 speedLeft = 50;
Jacqueline 0:6d0671ae4648 179
Jacqueline 0:6d0671ae4648 180 //Zustand, dass der Roboter nicht gestoppt hat
Jacqueline 0:6d0671ae4648 181 stopRight = false;
Jacqueline 0:6d0671ae4648 182 stopLeft = false;
Jacqueline 0:6d0671ae4648 183
Jacqueline 0:6d0671ae4648 184 //Drehrichtung der Motoren
Jacqueline 2:402b1a74fff6 185 controller.setDesiredSpeedRight((-speedRight));
Jacqueline 2:402b1a74fff6 186 controller.setDesiredSpeedLeft((-speedLeft));
Jacqueline 0:6d0671ae4648 187
Jacqueline 0:6d0671ae4648 188 while(1){
Jacqueline 0:6d0671ae4648 189
Jacqueline 0:6d0671ae4648 190 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 191 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
Jacqueline 0:6d0671ae4648 192
Jacqueline 0:6d0671ae4648 193 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 194 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
Jacqueline 0:6d0671ae4648 195 controller.setDesiredSpeedRight(0);
Jacqueline 0:6d0671ae4648 196 stopRight = true;
Jacqueline 0:6d0671ae4648 197 }
Jacqueline 0:6d0671ae4648 198
Jacqueline 0:6d0671ae4648 199 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
Jacqueline 0:6d0671ae4648 200 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
Jacqueline 0:6d0671ae4648 201 controller.setDesiredSpeedLeft(0);
Jacqueline 0:6d0671ae4648 202 stopLeft = true;
Jacqueline 0:6d0671ae4648 203 }
Jacqueline 0:6d0671ae4648 204
Jacqueline 0:6d0671ae4648 205 if(stopRight == true && stopLeft == true){
Jacqueline 0:6d0671ae4648 206 return;
Jacqueline 0:6d0671ae4648 207 }
Jacqueline 0:6d0671ae4648 208
Jacqueline 0:6d0671ae4648 209 }
Jacqueline 0:6d0671ae4648 210
Jacqueline 0:6d0671ae4648 211 }
Jacqueline 0:6d0671ae4648 212
ahlervin 4:767fd282dd9c 213
ahlervin 4:767fd282dd9c 214 //Implementation der Methode Ziel erreicht
ahlervin 4:767fd282dd9c 215 void Fahren::ziel() {
ahlervin 4:767fd282dd9c 216
ahlervin 4:767fd282dd9c 217 //einlesen des aktuellen Encoder standes
ahlervin 4:767fd282dd9c 218 initialClicksRight = counterRight.read();
ahlervin 4:767fd282dd9c 219 initialClicksLeft = counterLeft.read();
Jacqueline 0:6d0671ae4648 220
ahlervin 4:767fd282dd9c 221 //Anzahl Clicks die der Encoder zählen soll
ahlervin 4:767fd282dd9c 222 wegRechts = 7050;
ahlervin 4:767fd282dd9c 223 wegLinks = 7050;
ahlervin 4:767fd282dd9c 224
ahlervin 4:767fd282dd9c 225 //Geschwindigkeit
ahlervin 4:767fd282dd9c 226 speedRight = 80;
ahlervin 4:767fd282dd9c 227 speedLeft = 80;
ahlervin 4:767fd282dd9c 228
ahlervin 4:767fd282dd9c 229 //Zustand, dass der Roboter nicht gestoppt hat
ahlervin 4:767fd282dd9c 230 stopRight = false;
ahlervin 4:767fd282dd9c 231 stopLeft = false;
ahlervin 4:767fd282dd9c 232
ahlervin 4:767fd282dd9c 233 //Drehrichtung der Motoren
ahlervin 4:767fd282dd9c 234 controller.setDesiredSpeedRight((speedRight));
ahlervin 4:767fd282dd9c 235 controller.setDesiredSpeedLeft((speedLeft));
ahlervin 4:767fd282dd9c 236
ahlervin 4:767fd282dd9c 237 while(1){
ahlervin 4:767fd282dd9c 238
ahlervin 4:767fd282dd9c 239 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
ahlervin 4:767fd282dd9c 240 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
ahlervin 4:767fd282dd9c 241
ahlervin 4:767fd282dd9c 242 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
ahlervin 4:767fd282dd9c 243 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
ahlervin 4:767fd282dd9c 244 controller.setDesiredSpeedRight(0);
ahlervin 4:767fd282dd9c 245 stopRight = true;
ahlervin 4:767fd282dd9c 246 }
ahlervin 4:767fd282dd9c 247
ahlervin 4:767fd282dd9c 248 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
ahlervin 4:767fd282dd9c 249 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
ahlervin 4:767fd282dd9c 250 controller.setDesiredSpeedLeft(0);
ahlervin 4:767fd282dd9c 251 stopLeft = true;
ahlervin 4:767fd282dd9c 252 }
ahlervin 4:767fd282dd9c 253
ahlervin 4:767fd282dd9c 254 if(stopRight == true && stopLeft == true){
ahlervin 4:767fd282dd9c 255 return;
ahlervin 4:767fd282dd9c 256 }
ahlervin 4:767fd282dd9c 257 }
ahlervin 4:767fd282dd9c 258
ahlervin 4:767fd282dd9c 259 }
Jacqueline 0:6d0671ae4648 260
ahlervin 4:767fd282dd9c 261 //Implementation der Methode stopp
ahlervin 4:767fd282dd9c 262 void Fahren::stopp(){
ahlervin 4:767fd282dd9c 263
ahlervin 4:767fd282dd9c 264 //einlesen des aktuellen Encoder standes
ahlervin 4:767fd282dd9c 265 initialClicksRight = counterRight.read();
ahlervin 4:767fd282dd9c 266 initialClicksLeft = counterLeft.read();
ahlervin 4:767fd282dd9c 267
ahlervin 4:767fd282dd9c 268 //Anzahl Clicks die der Encoder zählen soll
ahlervin 4:767fd282dd9c 269 wegRechts = 0;
ahlervin 4:767fd282dd9c 270 wegLinks = 0;
ahlervin 4:767fd282dd9c 271
ahlervin 4:767fd282dd9c 272 //Geschwindigkeit
ahlervin 4:767fd282dd9c 273 speedRight = 0;
ahlervin 4:767fd282dd9c 274 speedLeft = 0;
ahlervin 4:767fd282dd9c 275
ahlervin 4:767fd282dd9c 276 //Zustand, dass der Roboter nicht gestoppt hat
ahlervin 4:767fd282dd9c 277 stopRight = false;
ahlervin 4:767fd282dd9c 278 stopLeft = false;
ahlervin 4:767fd282dd9c 279
ahlervin 4:767fd282dd9c 280 //Drehrichtung der Motoren
ahlervin 4:767fd282dd9c 281 controller.setDesiredSpeedRight(speedRight);
ahlervin 4:767fd282dd9c 282 controller.setDesiredSpeedLeft(-(speedLeft));
ahlervin 4:767fd282dd9c 283
ahlervin 4:767fd282dd9c 284
ahlervin 4:767fd282dd9c 285 while(1){
ahlervin 4:767fd282dd9c 286
ahlervin 4:767fd282dd9c 287 // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
ahlervin 4:767fd282dd9c 288 // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
ahlervin 4:767fd282dd9c 289
ahlervin 4:767fd282dd9c 290 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
ahlervin 4:767fd282dd9c 291 if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
ahlervin 4:767fd282dd9c 292 controller.setDesiredSpeedRight(0);
ahlervin 4:767fd282dd9c 293 stopRight = true;
ahlervin 4:767fd282dd9c 294 }
ahlervin 4:767fd282dd9c 295
ahlervin 4:767fd282dd9c 296 //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
ahlervin 4:767fd282dd9c 297 if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
ahlervin 4:767fd282dd9c 298 controller.setDesiredSpeedLeft(0);
ahlervin 4:767fd282dd9c 299 stopLeft = true;
ahlervin 4:767fd282dd9c 300 }
ahlervin 4:767fd282dd9c 301
ahlervin 4:767fd282dd9c 302 if(stopRight == true && stopLeft == true){
ahlervin 4:767fd282dd9c 303 return;
ahlervin 4:767fd282dd9c 304 }
ahlervin 4:767fd282dd9c 305
ahlervin 4:767fd282dd9c 306 }
ahlervin 4:767fd282dd9c 307
ahlervin 4:767fd282dd9c 308 }