Regler.cpp

00001 /*Roboshark V5
00002 Regler.cpp
00003 Erstellt: V.Ahlers
00004 geändert: V.Ahlers
00005 V.5.18
00006 Regler zum geradeaus fahren
00007 */
00008 
00009 #include <cmath>
00010 #include "Regler.h"
00011 #include "IRSensor.h"
00012 
00013 
00014 using namespace std;
00015 
00016 const float Regler :: PERIOD = 0.5f;
00017 const int Regler :: FIXSPEED = 50;
00018 float faktor = 0.1f;
00019 
00020 
00021 
00022 Regler::Regler(AnalogIn& IrRight, AnalogIn& IrLeft, IRSensor& iRSensor):
00023 IrRight (IrRight), IrLeft (IrLeft), iRSensor (iRSensor) {
00024         
00025         SpeedR = 0;
00026         SpeedL = 0;
00027         ticker.attach(callback(this, &Regler::setSpeed), PERIOD);
00028     }
00029      
00030 Regler::~Regler(){
00031     ticker.detach();
00032     }
00033     
00034     void Regler::setSpeed (){
00035         measR2 = iRSensor.readR();            // Converts and read the analog input value 
00036         measL2 = iRSensor.readL(); 
00037         
00038         if(measR2 == measL2){                                                   //DAS ISCH MIN GAGAREGLER zum Pröble gsi
00039                     SpeedR = FIXSPEED;
00040                     SpeedL = FIXSPEED;
00041                 }else if(measR2 < measL2 && measR2 < 300 && measL2 < 300){      //float style implementiert
00042                     SpeedR = FIXSPEED;
00043                     SpeedL = FIXSPEED*1.04;
00044                 }else if(measR2 > measL2 && measR2 < 300 && measL2 < 300){
00045                     SpeedR = FIXSPEED*1.06;
00046                     SpeedL = FIXSPEED;
00047                 }
00048                     
00049    /*     
00050         if((measR2 > 100) && (measL2 < 100)) {                //keine Wnad rechts
00051             if(measL2 > 40){
00052                 div3 = measL2 - 40;
00053                 kor3 = faktor*div3;
00054                 div2 = 0;
00055                 div1 = 0;
00056                 div4 = 0;
00057                 div5 = 0;
00058                 div6 = 0;
00059                 SpeedR = FIXSPEED;
00060                 SpeedL = FIXSPEED + kor3;
00061             } else if (measL2 < 45){
00062                 div4 = 45 - measR2;
00063                 kor4 = faktor*div4;
00064                 div2 = 0;
00065                 div1 = 0;
00066                 div3 = 0;
00067                 div5 = 0;
00068                 div6 = 0;
00069                 SpeedR = FIXSPEED;
00070                 SpeedL = FIXSPEED + kor4;
00071                 }else { 
00072             SpeedR = FIXSPEED;
00073             SpeedL = FIXSPEED;
00074             }
00075         }
00076         if((measL2 > 100) &&(measR2 < 100)) {                //keine Wnad links
00077             if(measR2 > 47){
00078                 div5 = measR2 - 47;
00079                 kor5 = faktor*div5;
00080                 div2 = 0;
00081                 div1 = 0;
00082                 div4 = 0;
00083                 div6 = 0;
00084                 div3 = 0;
00085                 SpeedR = FIXSPEED;
00086                 SpeedL = FIXSPEED + kor5;
00087             } else if (measR2 < 52){
00088                 div6 = 52 - measR2;
00089                 kor6 = faktor*div4;
00090                 div2 = 0;
00091                 div1 = 0;
00092                 div3 = 0;
00093                 div4 = 0;
00094                 div5 = 0;
00095                 SpeedR = FIXSPEED + kor6;
00096                 SpeedL = FIXSPEED;
00097             } else { 
00098             SpeedR = FIXSPEED;
00099             SpeedL = FIXSPEED;
00100             }
00101         }
00102         if ((measR2 < measL2)&& (measL2 - measR2 > 3)) {              //IR Sensor werte werden verglichen und die Korrektur wird berechnet
00103             div1 = measR2 - measL2;                             // An beiden seinen Wände
00104             kor1 = 0.1f*div1;
00105                 div2 = 0;
00106                 div3 = 0;
00107                 div4 = 0;
00108                 div5 = 0;
00109                 div6 = 0;
00110             SpeedR = FIXSPEED;
00111             SpeedL = FIXSPEED + kor1;
00112         } else if ((measR2 > measL2) && (measR2 - measL2 >3)) {
00113             div2 = measL2 - measR2;
00114             kor2 = 0.1f*div2;
00115                 div1 = 0;
00116                 div3 = 0;
00117                 div4 = 0;
00118                 div5 = 0;
00119                 div6 = 0;
00120             SpeedR = FIXSPEED + kor2;
00121             SpeedL = FIXSPEED;
00122         } else { 
00123             SpeedR = FIXSPEED;
00124             SpeedL = FIXSPEED;
00125             }
00126 //printf("Div1 = %f\n",div1);
00127 //printf("Div2 = %f\n",div2);  
00128 //printf("SpeedR1 = %f\n",SpeedR);
00129 //printf("SpeedL1 = %f\n",SpeedL);   
00130 
00131 */
00132      
00133 }
00134     float Regler :: getSpeedR(){
00135         SpeedR = SpeedR;
00136         //printf("SpeedR2 = %f\n",SpeedR);
00137         return SpeedR;
00138     }
00139     float Regler :: getSpeedL(){
00140         SpeedL = SpeedL;
00141         //printf("SpeedL2 = %f\n",SpeedL);  
00142         return SpeedL;
00143     }    
00144         
00145         
00146             
00147             
00148