Antonia Baumgartner / Mbed 2 deprecated Versuch21

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 Versuch20 by Alexander Wyss

Classes/Spurhaltung.cpp@5:64d7b4b69fdf, 2018-05-12 (annotated)

Committer:
baumgant
Date:
Sat May 12 12:49:18 2018 +0000
Revision:
5:64d7b4b69fdf
Parent:
3:f44ef28cfb2d 
PES2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
baumgant 0:b886f13e4ac6 1 #include "Spurhaltung.h"
baumgant 0:b886f13e4ac6 2 #include "IRSensorG.h"
baumgant 0:b886f13e4ac6 3
baumgant 0:b886f13e4ac6 4 using namespace std;
baumgant 0:b886f13e4ac6 5
baumgant 5:64d7b4b69fdf 6 const int Spurhaltung::DISTANCE = 100;
baumgant 5:64d7b4b69fdf 7 const float Spurhaltung::SPEED = 100;
baumgant 5:64d7b4b69fdf 8 const int Spurhaltung::DIFF = 1;
baumgant 5:64d7b4b69fdf 9 const int Spurhaltung::DIFF2 = 120;
baumgant 5:64d7b4b69fdf 10 const int Spurhaltung::DIFF3 = 180;
baumgant 5:64d7b4b69fdf 11 const int Spurhaltung::OFFSET = 30;
baumgant 5:64d7b4b69fdf 12 const int Spurhaltung::TOLERANCE1 = 8;
baumgant 5:64d7b4b69fdf 13 const int Spurhaltung::TOLERANCE2 = -8;
baumgant 5:64d7b4b69fdf 14
baumgant 0:b886f13e4ac6 15 Spurhaltung::Spurhaltung(IRSensorG& Sensor2, IRSensorG& Sensor3):
baumgant 0:b886f13e4ac6 16 Sensor2(Sensor2), Sensor3(Sensor3)
baumgant 0:b886f13e4ac6 17
baumgant 0:b886f13e4ac6 18 {
baumgant 0:b886f13e4ac6 19 this->sr = sr;
baumgant 0:b886f13e4ac6 20 this->sl = sl;
baumgant 0:b886f13e4ac6 21 }
baumgant 0:b886f13e4ac6 22
baumgant 0:b886f13e4ac6 23 Spurhaltung::~Spurhaltung() {}
baumgant 0:b886f13e4ac6 24
baumgant 2:efa9a78591da 25 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 26
baumgant 0:b886f13e4ac6 27 int Spurhaltung::speedl()
baumgant 0:b886f13e4ac6 28 {
baumgant 5:64d7b4b69fdf 29 // Wand links und rechts
baumgant 5:64d7b4b69fdf 30 if(abs(Sensor2.read()+OFFSET-Sensor3.read())<DIFF3) {
baumgant 5:64d7b4b69fdf 31 int sl = Sensor3.read() * 1.825 - 30;
baumgant 5:64d7b4b69fdf 32 printf("");
baumgant 5:64d7b4b69fdf 33 if(Sensor3.read() > (DISTANCE + 50)) {
baumgant 5:64d7b4b69fdf 34 sl = SPEED;
baumgant 5:64d7b4b69fdf 35 return sl;
baumgant 5:64d7b4b69fdf 36 } else {
baumgant 5:64d7b4b69fdf 37 return sl;
baumgant 5:64d7b4b69fdf 38 }
baumgant 0:b886f13e4ac6 39 }
baumgant 5:64d7b4b69fdf 40 // nur Wand links
baumgant 5:64d7b4b69fdf 41 else if ((Sensor2.read() < DIFF2) and (Sensor3.read() > DIFF3)) {
baumgant 5:64d7b4b69fdf 42 if ((Sensor2.read() - DISTANCE + OFFSET) > TOLERANCE1) {
baumgant 5:64d7b4b69fdf 43 int sl = 80 - DIFF;
baumgant 5:64d7b4b69fdf 44 //printf("Fridolin\r\n");
baumgant 5:64d7b4b69fdf 45 return sl;
baumgant 5:64d7b4b69fdf 46 } else if ((Sensor2.read() - DISTANCE + OFFSET) < TOLERANCE2) {
baumgant 5:64d7b4b69fdf 47 int sl = 80 + DIFF;
baumgant 5:64d7b4b69fdf 48 return sl;
baumgant 5:64d7b4b69fdf 49 } else {
baumgant 5:64d7b4b69fdf 50 sl = SPEED;
baumgant 5:64d7b4b69fdf 51 return sl;
baumgant 5:64d7b4b69fdf 52 }
baumgant 5:64d7b4b69fdf 53 }
baumgant 5:64d7b4b69fdf 54
baumgant 5:64d7b4b69fdf 55 // nur Wand rechts
baumgant 5:64d7b4b69fdf 56 else if ((Sensor3.read()<DIFF2) and (Sensor2.read()>DIFF3)) {
baumgant 5:64d7b4b69fdf 57 if ((Sensor3.read() - DISTANCE) > TOLERANCE1) {
baumgant 5:64d7b4b69fdf 58 sl = 80 + DIFF;
baumgant 5:64d7b4b69fdf 59 //printf("Fridolin\r\n");
baumgant 5:64d7b4b69fdf 60 return sl;
baumgant 5:64d7b4b69fdf 61 } else if ((Sensor3.read() - DISTANCE) < TOLERANCE2) {
baumgant 5:64d7b4b69fdf 62 int sl = 80 - DIFF;
baumgant 5:64d7b4b69fdf 63 return sl;
baumgant 5:64d7b4b69fdf 64 } else {
baumgant 5:64d7b4b69fdf 65 sl = SPEED;
baumgant 5:64d7b4b69fdf 66 return sl;
baumgant 5:64d7b4b69fdf 67 }
baumgant 5:64d7b4b69fdf 68 }
baumgant 5:64d7b4b69fdf 69 }
baumgant 5:64d7b4b69fdf 70
baumgant 0:b886f13e4ac6 71
baumgant 2:efa9a78591da 72 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 73
mrbb2 1:6ef5bc60e69c 74 int Spurhaltung::speedr()
mrbb2 1:6ef5bc60e69c 75 {
baumgant 5:64d7b4b69fdf 76 // Wand links und rechts
baumgant 5:64d7b4b69fdf 77 if(abs(Sensor3.read()-Sensor2.read()+OFFSET)<DIFF3) {
baumgant 5:64d7b4b69fdf 78 int sr = (Sensor2.read()) * 1.825;
baumgant 5:64d7b4b69fdf 79 printf("");
baumgant 5:64d7b4b69fdf 80 if(Sensor2.read() > (DISTANCE - OFFSET + 50)) {
baumgant 5:64d7b4b69fdf 81 sr = SPEED;
baumgant 5:64d7b4b69fdf 82 return sr;
baumgant 5:64d7b4b69fdf 83 } else {
baumgant 5:64d7b4b69fdf 84 return sr;
baumgant 5:64d7b4b69fdf 85 }
baumgant 0:b886f13e4ac6 86 }
baumgant 5:64d7b4b69fdf 87 // nur Wand links
baumgant 5:64d7b4b69fdf 88 else if ((Sensor2.read() < DIFF2) and (Sensor3.read() > DIFF3)) {
baumgant 5:64d7b4b69fdf 89 if ((Sensor2.read() - DISTANCE + OFFSET) > TOLERANCE1) {
baumgant 5:64d7b4b69fdf 90 int sr = 80 + DIFF;
baumgant 5:64d7b4b69fdf 91 //printf("Karolina\r\n");
baumgant 5:64d7b4b69fdf 92 return sr;
baumgant 5:64d7b4b69fdf 93 } else if ((Sensor2.read() - DISTANCE + OFFSET) < TOLERANCE2) {
baumgant 5:64d7b4b69fdf 94 int sr = 80 - DIFF;
baumgant 5:64d7b4b69fdf 95 return sr;
baumgant 5:64d7b4b69fdf 96 } else {
baumgant 5:64d7b4b69fdf 97 sr = SPEED;
baumgant 5:64d7b4b69fdf 98 return sr;
baumgant 5:64d7b4b69fdf 99 }
baumgant 5:64d7b4b69fdf 100 }
baumgant 5:64d7b4b69fdf 101 // nur Wand rechts
baumgant 5:64d7b4b69fdf 102 else if((Sensor3.read() < DIFF2) and (Sensor2.read() > DIFF3)) {
baumgant 5:64d7b4b69fdf 103 if ((Sensor3.read() - DISTANCE) > TOLERANCE1) {
baumgant 5:64d7b4b69fdf 104 int sr = 80 - DIFF;
baumgant 5:64d7b4b69fdf 105 //printf("Karolina\r\n");
baumgant 5:64d7b4b69fdf 106 return sr;
baumgant 5:64d7b4b69fdf 107 } else if ((Sensor3.read() - DISTANCE) < TOLERANCE2) {
baumgant 5:64d7b4b69fdf 108 int sr = 80 + DIFF;
baumgant 5:64d7b4b69fdf 109 return sr;
baumgant 5:64d7b4b69fdf 110 } else {
baumgant 5:64d7b4b69fdf 111 sr = SPEED;
baumgant 5:64d7b4b69fdf 112 return sr;
baumgant 5:64d7b4b69fdf 113 }
baumgant 5:64d7b4b69fdf 114 }
baumgant 5:64d7b4b69fdf 115 }