Ododemo

Eine bestimmte Strecke soll mit dem ASURO zurückgelegt werden, Werte in odometrie.h Eintragen (grösse der Scheibe, Schwellwerte für links/rechts (aus 1. Programm Mittelwert) und die Anzahl Umdrehungen die das kleine Zahnrad machen soll)

odometrie.c

#include "asuro.h"
#include "odometrie.h"

int main(void)
{
    /** flag für aktuellen zustand der farbe */
    int Lnow = 0;
    int Rnow = 0;
    int Rold = 0;
    int Lold = 0;
    // haben sich die werte geändert
    unsigned int fl_Lch = 0;
    unsigned int fl_Rch = 0;
    /** zähler für farbwechsel */
    unsigned int Count1 = 0;
    unsigned int Count2 = 0;
    int diff = 0;
    /** Geschwindigkeit */
    int sl = 130;
    int sr = 130;
    /** Odometrie links/rechts */
    unsigned int data[2];
    
    Init();
    GREEN_LED_ON;

    while(1) {
        MotorDir(FWD,FWD);
        MotorSpeed(sl,sr);
        
        OdometrieData(data); // 0. links, 1. rechts
        
        // Schwellwert für schwarz/weiss linke Seite
        if (data[0] > THRES_L)
            Lnow = 1;
        else
            Lnow = 0;
        
        // Schwellwert für schwarz/weiss rechte Seite
        if (data[1] > THRES_R)
            Rnow = 1;
        else
            Rnow = 0;
        
        // wenn sich der linke Wert geändert hat zähler ++
        if (Lnow ^ Lold) {
            Count1++;
            fl_Lch = 1;
        }
        else {
            fl_Lch = 0;
        }
        
        // wenn sich der rechte Wert geändert hat zähler ++
        if (Rnow ^ Rold) {
            Count2++;
            fl_Rch = 1;
        }
        else {
            fl_Rch = 0;
        }

        // hat sich der Asuro bewegt ?
        if (fl_Rch || fl_Lch) {
            // alte werte merken für nächsten schleifendurchlauf
            Lold = Lnow;
            Rold = Rnow;
            
            diff = Count2 - Count1;
            if (diff > 0)
                sr--;
            else if (diff < 0)
                sl--;
            else {
                sr = 130;
                sl = 130;
            }
            if (sr < 50) {
                sr = 0;
            }
            if (sl < 50) {
                sl = 0;
            }
        }

        // nach einer bistimmten Strecke stehenbleiben
        if (Count1 == WEITE ) {
            break;
        }
    }
    
    MotorDir(BREAK, BREAK);
    MotorSpeed(0, 0);
    RED_LED_ON;
    while(1) {
    }
    
    return 0;
}

odometrie.h
/*******************************************************************************
 *
 * File Name:   odometrie.h
 * Project  :   ASURO
 *
 *
 * Ver.     Date         Author           Comments
 * -------  ----------   --------------   ------------------------------
 * 1.00     25.06.2005   Linux_80    	 
 */

#ifndef ODOMETRIE_H
#define ODOMETRIE_H

/** grösse der fächerscheibe 12 oder 8 */
#define ODOSIZE 8
// bei 5 dreht sich das Rad einmal -> 122mm !
#define UMDREH  15
#define WEITE ODOSIZE*UMDREH
/** Schwellwert für schwarz/weiss (530) */
#define THRES_L 540
#define THRES_R 540


#define _CLI() do { __asm__ __volatile__ ("cli"); } while (0)
#define _SEI() do { __asm__ __volatile__ ("sei"); } while (0)
#define _NOP() do { __asm__ __volatile__ ("nop"); } while (0)


#endif /* ODOMETRIE_H */

Erstellt: 3. Juli 2005