Spurhaltung.cpp

00001 #include "Spurhaltung.h"
00002 #include "IRSensorG.h"
00003 
00004 using namespace std;
00005 
00006 const int Spurhaltung::DISTANCE = 100;
00007 const float Spurhaltung::SPEED = 100;
00008 const int Spurhaltung::DIFF = 1;
00009 const int Spurhaltung::DIFF2 = 120;
00010 const int Spurhaltung::DIFF3 = 180;
00011 const int Spurhaltung::OFFSET = 30;
00012 const int Spurhaltung::TOLERANCE1 = 8;
00013 const int Spurhaltung::TOLERANCE2 = -8;
00014 
00015 Spurhaltung::Spurhaltung(IRSensorG& Sensor2, IRSensorG& Sensor3):
00016     Sensor2(Sensor2), Sensor3(Sensor3)
00017 
00018 {
00019     this->sr = sr;
00020     this->sl = sl;
00021 }
00022 
00023 Spurhaltung::~Spurhaltung() {}
00024 
00025 //------------------------------------------------------------------------------
00026 
00027 int Spurhaltung::speedl()
00028 {
00029     // Wand links und rechts
00030     if(abs(Sensor2.read()+OFFSET-Sensor3.read())<DIFF3) {
00031         int sl = Sensor3.read() * 1.825 - 30;
00032         printf("");
00033         if(Sensor3.read() > (DISTANCE + 50)) {
00034             sl = SPEED;
00035             return sl;
00036         } else {
00037             return sl;
00038         }
00039     }
00040     // nur Wand links
00041     else if ((Sensor2.read() < DIFF2) and (Sensor3.read() > DIFF3)) {
00042         if ((Sensor2.read() - DISTANCE + OFFSET) > TOLERANCE1) {
00043             int sl = 80 - DIFF;
00044             //printf("Fridolin\r\n");
00045             return sl;
00046         } else if ((Sensor2.read() - DISTANCE + OFFSET) < TOLERANCE2) {
00047             int sl = 80 + DIFF;
00048             return sl;
00049         } else {
00050             sl = SPEED;
00051             return sl;
00052         }
00053     }
00054 
00055     // nur Wand rechts
00056     else if ((Sensor3.read()<DIFF2) and (Sensor2.read()>DIFF3)) {
00057         if ((Sensor3.read() - DISTANCE) > TOLERANCE1) {
00058             sl = 80 + DIFF;
00059             //printf("Fridolin\r\n");
00060             return sl;
00061         } else if ((Sensor3.read() - DISTANCE) < TOLERANCE2) {
00062             int sl = 80 - DIFF;
00063             return sl;
00064         } else {
00065             sl = SPEED;
00066             return sl;
00067         }
00068         }
00069     }
00070 
00071 
00072 //------------------------------------------------------------------------------
00073 
00074 int Spurhaltung::speedr()
00075 {
00076     // Wand links und rechts
00077     if(abs(Sensor3.read()-Sensor2.read()+OFFSET)<DIFF3) {
00078         int sr = (Sensor2.read()) * 1.825;
00079         printf("");
00080         if(Sensor2.read() > (DISTANCE - OFFSET + 50)) {
00081             sr = SPEED;
00082             return sr;
00083         } else {
00084             return sr;
00085         }
00086     }
00087     // nur Wand links
00088     else if ((Sensor2.read() < DIFF2) and (Sensor3.read() > DIFF3)) {
00089         if ((Sensor2.read() - DISTANCE + OFFSET) > TOLERANCE1) {
00090             int sr = 80 + DIFF;
00091             //printf("Karolina\r\n");
00092             return sr;
00093         } else if ((Sensor2.read() - DISTANCE + OFFSET) < TOLERANCE2) {
00094             int sr = 80 - DIFF;
00095             return sr;
00096         } else {
00097             sr = SPEED;
00098             return sr;
00099         }
00100     }
00101     // nur Wand rechts
00102     else if((Sensor3.read() < DIFF2) and (Sensor2.read() > DIFF3)) {
00103         if ((Sensor3.read() - DISTANCE) > TOLERANCE1) {
00104             int sr = 80 - DIFF;
00105             //printf("Karolina\r\n");
00106             return sr;
00107         } else if ((Sensor3.read() - DISTANCE) < TOLERANCE2) {
00108             int sr = 80 + DIFF;
00109             return sr;
00110         } else {
00111             sr = SPEED;
00112             return sr;
00113         }
00114     }
00115 }