Roboter.cpp

00001 #include "Roboter.h"
00002 
00003 
00004 Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* servo1, Servo* servo2, EncoderCounter* counterLeft, EncoderCounter* counterRight, DigitalOut* enableMotorDriver)
00005 
00006 {
00007     sensors[0].init( distance, bit0, bit1, bit2, 0);
00008     sensors[1].init( distance, bit0, bit1, bit2, 1);
00009     sensors[2].init( distance, bit0, bit1, bit2, 2);
00010     sensors[3].init( distance, bit0, bit1, bit2, 3);
00011     sensors[4].init( distance, bit0, bit1, bit2, 4);
00012     sensors[5].init( distance, bit0, bit1, bit2, 5);
00013 
00014     this->pwmR = pwmR;
00015     this->pwmL = pwmL;
00016     this->servo1 = servo1;
00017     this->servo2 = servo2;
00018     this->counterLeft = counterLeft;
00019     this->counterRight = counterRight;
00020     this->enableMotorDriver = enableMotorDriver;
00021 
00022     initSpeedcontrol();
00023 
00024     t1.attach(this, &Roboter::speedCtrl, PERIOD);
00025 
00026 }
00027 
00028 int Roboter::readSensors()            // Sensoren auslesen
00029 {
00030     int lr = 1;
00031     float x = 0.1;                 // Distanz ab welcher sensoren reagieren sollen
00032 
00033     if(sensors[1] < x || sensors[5] < x) {            // Sensor rechts und links
00034         lr = 6;
00035     }
00036     return lr;
00037 
00038 }
00039 
00040 
00041 void Roboter::ausweichen1()         // Hindernissen ausweichen
00042 {
00043     float x = 0.14;       // Distanz ab welcher sensoren reagieren sollen
00044 
00045     while(sensors[0] < (x-0.01f) && sensors[1] < x && sensors[5] < x) {  // alle sensoren aktiv, roboter fährt nach hinten
00046         desiredSpeedLeft = -v;
00047         desiredSpeedRight = v*4;
00048 
00049     }
00050 
00051     while(sensors[0] < x) { // sensor vorne, roboter dreht nach links
00052         desiredSpeedLeft = -v*2;
00053         desiredSpeedRight = -v*2;
00054 
00055     }
00056 
00057     while(sensors[1] < x) {     // sensor rechts, roboter dreht nach links
00058         desiredSpeedLeft = -v*2;
00059         desiredSpeedRight = -v*2;
00060 
00061     }
00062 
00063     while(sensors[5] < x) {     // sensor links, roboter dreht nach rechts
00064         desiredSpeedLeft = v*2;
00065         desiredSpeedRight = v*2;
00066 
00067     }
00068 
00069 }
00070 
00071 int Roboter::ausweichen2()         // Hindernissen ausweichen beim Anfahren des Klotzes
00072 {
00073     float x = 0.1;                 // Distanz ab welcher sensoren reagieren sollen
00074     static int l = 0;
00075     static int r = 0;
00076     static int ii = 0;
00077     int s = 6;
00078 
00079     if(sensors[1] < x && l == 0) {
00080         r = 1;
00081     }
00082 
00083     if(sensors[5] < x && r == 0) {
00084         l = 1;
00085     }
00086 
00087     if(r == 1) {
00088         if(ii < 500) {                      // rückwärts fahren
00089             desiredSpeedLeft = -(v+30);
00090             desiredSpeedRight = v+30;
00091         }
00092         if(ii > 499 && ii < 900) {         // links drehen
00093             desiredSpeedLeft = -v*2;
00094             desiredSpeedRight = -v*2;
00095         }
00096         if(ii > 899 && ii < 1800) {         // gerade fahren
00097             desiredSpeedLeft = v*2;
00098             desiredSpeedRight = -v*2;
00099         }
00100         if(ii > 1799 && ii < 2300) {        // rechts drehen
00101             desiredSpeedLeft = v*2;
00102             desiredSpeedRight = v*2;
00103         }
00104         ii++;
00105         if(ii > 2299) {                     // rücksetzen
00106             desiredSpeedLeft = 0;
00107             desiredSpeedRight = 0;
00108             r = 0;
00109             ii = 0;
00110             s = 1;
00111         }
00112     }
00113 
00114     if(l == 1) {
00115         if(ii < 500) {                    // rückwärts fahren
00116             desiredSpeedLeft = -(v+30);
00117             desiredSpeedRight = v+30;
00118         }
00119         if(ii > 499 && ii < 900) {       // rechts drehen
00120             desiredSpeedLeft = v*2;
00121             desiredSpeedRight = v*2;
00122         }
00123         if(ii > 899 && ii < 1800) {      // gerade fahren
00124             desiredSpeedLeft = v*2;
00125             desiredSpeedRight = -v*2;
00126         }
00127         if(ii > 1799 && ii < 2300) {      // links drehen
00128             desiredSpeedLeft = -v*2;
00129             desiredSpeedRight = -v*2;
00130         }
00131         ii++;
00132         if(ii > 2299) {
00133             desiredSpeedLeft = 0;
00134             desiredSpeedRight = 0;
00135             l = 0;
00136             ii = 0;
00137             s = 1;
00138         }
00139     }
00140     return s;
00141 }
00142 
00143 
00144 int Roboter::pickup()           // Klotz aufnehmen
00145 {
00146     static int ii = 0;
00147     int r;                      // Rückegabewert
00148     desiredSpeedLeft = 0;
00149     desiredSpeedRight = 0;
00150 
00151     if(ii < 650) {                   // Arm nach unten fahren
00152         servo2->SetPosition(2250);
00153     }
00154     if(ii > 649 && ii < 1100) {      // gerade fahren
00155         desiredSpeedLeft = v;
00156         desiredSpeedRight = -v;
00157     }
00158     if(ii > 1099 && ii < 1500) {     // gerade fahren + Greifer schliessen
00159         if(ii < 1400) {
00160             desiredSpeedLeft = v;
00161             desiredSpeedRight = -v;
00162         }
00163         servo1->SetPosition(500);
00164     }
00165     if(ii > 1499 && ii < 1900) {     // rückwärts fahren
00166         desiredSpeedLeft = -v;
00167         desiredSpeedRight = v;
00168     }
00169     if(ii > 1899 && ii < 2550) {    // Arm nach oben fahren
00170         desiredSpeedLeft = 0;
00171         desiredSpeedRight = 0;
00172         servo2->SetPosition(700);
00173     }
00174     if(ii > 2549 && ii < 3000) {    // Greifer öffnen
00175         servo1->SetPosition(1500);
00176     }
00177 
00178     ii++;
00179     r = 5;                          // Rückegabewert
00180 
00181     if( ii > 2999 ) {
00182         r = 1;
00183         ii = 0;
00184     }
00185 
00186     return r;
00187 }
00188 
00189 void Roboter::anfahren(float cam)
00190 {
00191     float speedValue;
00192     float maxWert = 20;
00193     if(cam == 100) {
00194         desiredSpeedLeft = -v/2;
00195         desiredSpeedRight = -v/2;
00196     }
00197     if(cam == 200) {
00198         desiredSpeedLeft = v/2;
00199         desiredSpeedRight = v/2;
00200     }
00201     if(cam > 100 && cam <200 ) {                  // links fahren, wenn wir Werte von 100 bis 199 haben
00202         cam = cam -99.0f;                           // cam nimmt die Werte von 1 bis 100 an
00203         speedValue = (v) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
00204         desiredSpeedLeft = -speedValue;
00205         desiredSpeedRight = -speedValue;
00206     }
00207     if(cam > 200 && cam < 300) {                   // rechts fahren, wenn wir Werte von 200 bis 299 haben
00208         cam = cam -199.0f;                          // cam nimmt die Werte von 1 bis 100 an
00209         speedValue = (v) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
00210         desiredSpeedLeft = speedValue;
00211         desiredSpeedRight = speedValue;
00212     }
00213     if(cam >= 300 && cam <400) {                   // gerade fahren, wenn wir Werte von 300 bis 399 haben
00214         cam = cam-299.0f;                           // cam nimmt die Werte von 1 bis 100 an
00215         speedValue = (v*3) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
00216         desiredSpeedLeft = speedValue;
00217         desiredSpeedRight = -speedValue;
00218     }
00219 }
00220 
00221 int Roboter::geradeFahren()
00222 {
00223     int r;
00224     static int time2 = 0;
00225     if(time2 < 25) {    // 1s gerade aus fahren
00226         desiredSpeedLeft = v*4;
00227         desiredSpeedRight = -v*4;
00228         time2 ++;
00229         r = 1;       // state
00230     } else {
00231         time2 = 0;
00232         desiredSpeedLeft = 0.0f;
00233         desiredSpeedRight = 0.0f;
00234         r = 2;       //state
00235     }
00236     return r;
00237 }
00238 
00239 int Roboter::back()
00240 {
00241     static int ii = 0;
00242     int s = 7;
00243 
00244     if(ii < 1000) {
00245         desiredSpeedLeft = -v;
00246         desiredSpeedRight = v;
00247     }
00248 
00249     if(ii > 999 && ii < 1800) {
00250         desiredSpeedLeft = v*2;
00251         desiredSpeedRight = v*2;
00252     }
00253 
00254     ii++;
00255 
00256     if(ii > 1799) {
00257         desiredSpeedLeft = 0;
00258         desiredSpeedRight = 0;
00259         ii = 0;
00260         s = 1;
00261     }
00262 
00263     return s;
00264 }
00265 
00266 int Roboter::kreisFahren()
00267 {
00268     int r;
00269     static int anzahl = 0;
00270     static int time1 = 0;
00271 
00272     if(time1 < 6 && anzahl%2 == 0) {       // 1s im Kreis drehen
00273         desiredSpeedLeft = v*2;
00274         desiredSpeedRight = v*2;
00275         time1 ++;
00276         r = 1;          // state
00277     } else {
00278         desiredSpeedLeft = -(v*2);
00279         desiredSpeedRight = -(v*2);
00280         time1 ++;
00281         r = 1;          // state
00282     }
00283     if(time1 == 6) {
00284         time1 = 0;
00285         anzahl ++;
00286         desiredSpeedLeft = 0.0f;
00287         desiredSpeedRight = 0.0f;
00288         r = 3;          // state
00289     }
00290     return r;
00291 }
00292 
00293 int Roboter::stateAuswahl(float cam, int tempState, int temp)
00294 {
00295     int s = 1;
00296     if(cam >= 100.0f && cam < 400.0f ) {           // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399
00297         s = 4;
00298     }
00299     if(cam == 400.0f && temp != 5) {     // Roboter in Position
00300         s = 5;
00301     }
00302     if(cam == 400.0f && temp == 5) {     // Roboter immer noch beim selben Klotz -> back()
00303         s = 7;
00304     }
00305     if(cam < 1) {
00306         s = tempState;
00307     }
00308     if(cam == 1) {
00309         s = 1;
00310     }
00311     return s;
00312 
00313 
00314 }
00315 
00316 void Roboter::initSpeedcontrol()
00317 {
00318     // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
00319     pwmL->period(0.00005f); // Setzt die Periode auf 50 μs
00320     pwmR->period(0.00005f);
00321     *pwmL = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
00322     *pwmR = 0.5f; // Duty-Cycle von 50%
00323 
00324     // Initialisieren von lokalen Variabeln
00325     previousValueCounterLeft = counterLeft->read();
00326     previousValueCounterRight = counterRight->read();
00327     speedLeftFilter.setPeriod(PERIOD);
00328     speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00329     speedRightFilter.setPeriod(PERIOD);
00330     speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00331 
00332     desiredSpeedLeft = 0.0f;
00333     desiredSpeedRight = 0.0f;
00334     actualSpeedLeft = 0.0f;
00335     actualSpeedRight = 0.0f;
00336 
00337     *enableMotorDriver = 1;
00338 }
00339 
00340 void Roboter::speedCtrl()
00341 {
00342     // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
00343     short valueCounterLeft = counterLeft->read();
00344     short valueCounterRight = counterRight->read();
00345     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
00346     short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
00347 
00348     previousValueCounterLeft = valueCounterLeft;
00349     previousValueCounterRight = valueCounterRight;
00350     actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
00351     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
00352 
00353     // Berechnen der Motorspannungen Uout
00354     float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
00355     float voltageRight = KP*(desiredSpeedRight*correction-actualSpeedRight)+desiredSpeedRight*correction/KN;
00356 
00357     // Berechnen, Limitieren und Setzen der Duty-Cycle
00358     float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
00359     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
00360     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
00361 
00362     *pwmL = dutyCycleLeft;
00363     float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
00364     if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
00365     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
00366 
00367     *pwmR = dutyCycleRight;
00368 }