## c't-Projekte - Mailinglisten

[Voriger (Datum)] [Nächster (Datum)] [Voriger (Thread)] [Nächster (Thread)]
[Nach Datum][Nach Thread]

### Re: [ct-bot] Noch'n Sensorpatch zum Testen

Absender: Peter Jonas
Datum: Di, 04.04.2006 11:29:33

    > Von: Michail Brzitwa <michail@xxxxxxxxxx>
    > Nette Idee, ich würde das aber nicht in die "normale" Hexdatei tun,
    > sondern per Präprozessordirektive erlauben, ein vom Rest unabhängiges
    > Einmeßprogramm zu erzeugen, das insbesondere auch nicht die Motoren
    > ansteuert (damit das Dingen z.B. nicht gleich abhaut).

    Das habe ich auch so ähnlich vor. In der ct-bot.h kann die Routine per
    #define so aktiviert werden, wie es auch bei den Testroutinen gemacht wird.
    In der ct-bot.c erfolgt dann (wenn "defined") der Sprung aus der
    Hauptschleife in die Routine start_calibration(), die in der Datei
    sensor_low() abgelegt ist. Im Augenblick gibt es aus der Routine noch einen
    Rücksprung in den normalen Bot-Betrieb. Wenn man das nicht will (damit er
    nicht abhaut), kann das natürlich herausnehmen.

    Ich habe mal die geänderte sensor_low.c beigefügt. Die entsprechende Routine
    ist am Ende der Datei. Sie funktioniert aber vermutlich noch nicht richtig
    (ich habe momentan nur wenig Zeit dafür).

    Gruß
    Peter Jonas

    --
    GMX Produkte empfehlen und ganz einfach Geld verdienen!
    Satte Provisionen für GMX Partner: http://www.gmx.net/de/go/partner

    /*
     * c't-Bot
     *
     * This program is free software; you can redistribute it
     * and/or modify it under the terms of the GNU General
     * Public License as published by the Free Software
     * Foundation; either version 2 of the License, or (at your
     * option) any later version.
     * This program is distributed in the hope that it will be
     * useful, but WITHOUT ANY WARRANTY; without even the implied
     * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     * PURPOSE. See the GNU General Public License for more details.
     * You should have received a copy of the GNU General Public
     * License along with this program; if not, write to the Free
     * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
     * MA 02111-1307, USA.
     *
     */

    /*! @file 	sensor-low.c
     * @brief 	Low-Level Routinen fÃ¼r die Sensor Steuerung des c't-Bots
     * @author 	Benjamin Benz (bbe@xxxxxxxx)
     * @date 	01.12.05
    */

    #ifdef MCU

    #include <avr/io.h>
    #include <stdio.h>
    #include "adc.h"
    #include "global.h"

    #include "ena.h"
    #include "sensor.h"
    #include "mouse.h"
    #include "motor.h"
    #include "timer.h"
    #include "sensor_correction.h"
    #include "display.h"
    #include "rc5-codes.h"

    #ifdef CALIBRATE_DIST_SENSOR
    	#include <avr/eeprom.h>
    #endif

    // ADC-PINS
    #define SENS_ABST_L	0		/*!< ADC-PIN Abstandssensor Links */
    #define SENS_ABST_R	1		/*!< ADC-PIN Abstandssensor Rechts */
    #define SENS_M_L		2		/*!< ADC-PIN Liniensensor Links */
    #define SENS_M_R		3		/*!< ADC-PIN Liniensensor Rechts */
    #define SENS_LDR_L		4		/*!< ADC-PIN Lichtsensor Links */
    #define SENS_LDR_R		5		/*!< ADC-PIN Lichtsensor Rechts */
    #define SENS_KANTE_L	6		/*!< ADC-PIN Kantensensor Links */
    #define SENS_KANTE_R	7		/*!< ADC-PIN Kantensensor Rechts */

    // Sonstige Sensoren
    #define SENS_DOOR_PINR 	PIND	/*!< Port an dem der Klappensensor hÃ¤ngt */
    #define SENS_DOOR_DDR 		DDRD	/*!< DDR fÃ¼r den Klappensensor */
    #define SENS_DOOR			6		/*!< Pin  an dem der Klappensensor hÃ¤ngt */

    #define SENS_ENCL_PINR		PINB	/*!< Port an dem der linke Encoder hÃ¤ngt */
    #define SENS_ENCL_DDR		DDRB	/*!< DDR fÃ¼r den linken Encoder  */
    #define SENS_ENCL			4		/*!< Pin an dem der linke Encoder hÃ¤ngt */

    #define SENS_ENCR_PINR		PIND	/*!< Port an dem der rechte Encoder hÃ¤ngt */
    #define SENS_ENCR_DDR		DDRD	/*!< DDR fÃ¼r den rechten Encoder  */
    #define SENS_ENCR			3		/*!< Pin an dem der rechte Encoder hÃ¤ngt */

    #define SENS_ERROR_PINR	PINB	/*!< Port an dem die FehlerÃ¼berwachung hÃ¤ngt */
    #define SENS_ERROR_DDR		DDRB	/*!< DDR fÃ¼r die FehlerÃ¼berwachung */
    #define SENS_ERROR			2		/*!< Pin an dem die FehlerÃ¼berwachung hÃ¤ngt */

    #define SENS_TRANS_PINR	PINB	/*!< Port an dem die Transportfachueberwachung haengt */
    #define SENS_TRANS_PORT	PORTB	/*!< Port an dem die Transportfachueberwachung haengt */
    #define SENS_TRANS_DDR		DDRB	/*!< DDR fÃ¼r die Transportfachueberwachung */
    #define SENS_TRANS			0		/*!< Pin an dem die Transportfachueberwachung haengt */

    #define ENC_L ((SENS_ENCL_PINR >> SENS_ENCL) & 0x01)	/*!< Abkuerzung zum Zugriff auf Encoder */
    #define ENC_R ((SENS_ENCR_PINR >> SENS_ENCR) & 0x01)	/*!< Abkuerzung zum Zugriff auf Encoder */

    #define ENC_ENTPRELL	4		/*!< Nur wenn der Encoder ein paar mal den gleichen wert gibt uebernehmen */

    volatile uint8 enc_l=0;		/*!< Puffer fuer die letzten Encoder-Staende */
    volatile uint8 enc_r=0;		/*!< Puffer fuer die letzten Encoder-Staende */

    volatile uint8 enc_l_cnt=0;	/*!< Entprell-Counter fuer L-Encoder	 */
    volatile uint8 enc_r_cnt=0;	/*!< Entprell-Counter fuer R-Encoder	 */

    #ifdef CALIBRATE_DIST_SENSOR
    	extern uint16 ir_read(void);
    	uint8_t eeDist_TableL[300] EEMEM;
    	uint8_t eeDist_TableR[300] EEMEM;
    #endif

    /*!
     * Initialisiere alle Sensoren
     */
    void bot_sens_init(void){
    	ENA_init();
    	adc_init(0xFF);		// Alle ADC-Ports aktivieren

    	ENA_set(ENA_RADLED);		// Alle Sensoren bis auf die Radencoder deaktivieren
    	ENA_on(ENA_ABSTAND);		// Die Abstandssensoren ebenfalls dauerhaft an, da sie fast 50 ms zum booten brauchen

    	SENS_DOOR_DDR &= ~ (1<<SENS_DOOR);	// Input

    	SENS_ENCL_DDR &= ~ (1<<SENS_ENCL);	// Input
    	SENS_ENCR_DDR &= ~(1<<SENS_ENCR);	// Input

    	SENS_ERROR_DDR &= ~(1<<SENS_ERROR);	// Input

    	SENS_TRANS_DDR &= ~(1<<SENS_TRANS);	// Input
    	SENS_TRANS_PORT |= (1<<SENS_TRANS);  // Pullup an

    	SENS_ENCL_DDR &= ~(1<<SENS_ENCL);	// Input
    	SENS_ENCR_DDR &= ~(1<<SENS_ENCR);	// Input

    	timer_2_init();
    }

    /*!
     * Konvertiert einen Sensorwert vom analogen Abstandssensor in mm
     * Da die Sensoren eine nichtlineare Kennlinie haben,
     * ist ein wenig Umrechnung noetig.
     * Diese Funktion ist genau der richtige Ort dafuer
     * @param sensor_data Die Daten vom analogen Sensor
     * @return Distanz in mm
     */
    int16 sensor_abstandL(int16 sensor_data){
    	return eeprom_read_byte(&eeDist_TableL[sensor_data>>1]);
    }

    int16 sensor_abstandR(int16 sensor_data){
    	return eeprom_read_byte(&eeDist_TableR[sensor_data>>1]);
    }

    /*!
     * Alle Sensoren aktualisieren
     * Derzeit pollt diese Routine alle Sensoren. Insbesondere bei den
     * analogen dauert das eine Weile. Daher kann man hier einiges
     * an Performance gewinnen, wenn man die Routine aufspaltet und
     * zumindest die analogen Sensoren per Interrupt bearbeitet,
     * denn im Moment blockiert adc_read so lange, bis ein Sensorwert ausgelesen ist.
     * Die digitalen Sensoren liefern ihre Werte dagegen unmittelbar
     * Aber Achtung es lohnt auch nicht, immer alles so schnell als moeglich
     * zu aktualiseren, der Bot braucht auch Zeit zum nachdenken ueber Verhalten
     */
    void bot_sens_isr(void){
    	ENA_on(ENA_KANTLED|ENA_MAUS|ENA_SCHRANKE|ENA_KLAPPLED);

     	// Aktualisiere die Position des Maussensors
    	sensMouseDY = maus_sens_read(MOUSE_DELTA_Y_REG);

    	sensMouseDX = maus_sens_read(MOUSE_DELTA_X_REG);

    	// ---------- analoge Sensoren -------------------
    	sensLDRL = adc_read(SENS_LDR_L);
    	sensLDRR = adc_read(SENS_LDR_R);

    	sensBorderL = adc_read(SENS_KANTE_L);
    	sensBorderR = adc_read(SENS_KANTE_R);
    	ENA_off(ENA_KANTLED);

    	sensLineL = adc_read(SENS_M_L);
    	sensLineR = adc_read(SENS_M_R);
    	ENA_off(ENA_MAUS);

    	//Aktualisiere Distanz-Sensoren
    	// Die Distanzsensoren sind im Normalfall an, da sie 50 ms zum booten brauchen
    //	sensDistL= adc_read(SENS_ABST_L);
    //	sensDistR= adc_read(SENS_ABST_R);
    	sensDistL= sensor_abstandL(adc_read(SENS_ABST_L));
    	sensDistR= sensor_abstandR(adc_read(SENS_ABST_R));

    	// ------- digitale Sensoren ------------------
    	sensDoor = (SENS_DOOR_PINR >> SENS_DOOR) & 0x01;
    	sensTrans = (SENS_TRANS_PINR >> SENS_TRANS) & 0x01;
    	ENA_off(ENA_SCHRANKE|ENA_KLAPPLED);

    	sensError = (SENS_ERROR_PINR >> SENS_ERROR) & 0x01;

    	sensor_update();	// Weiterverarbeitung der rohen Sensordaten
    }

    /*!
     * Kuemmert sich um die Radencoder
     * Das muss schneller gehen als die anderen Sensoren,
     * daher Update per Timer-Interrupt und nicht per Polling
     */
    void bot_encoder_isr(void){
    	// --------------------- links ----------------------------
    	//Rad-Encoder auswerten
    	if ( ENC_L != enc_l){	// uns interesieren nur Veraenderungen
    		enc_l=ENC_L;			// neuen wert sichern
    		enc_l_cnt=0;			// Counter zuruecksetzen
    	} else {			// Zaehlen, wie lange Pegel bleibt
    		if (enc_l_cnt <= ENC_ENTPRELL) // Nur bis zur Entprell-Marke
    			enc_l_cnt++;
    	}

    	if (enc_l_cnt == ENC_ENTPRELL){// wenn lange genug konst
    		if (direction.left == DIRECTION_FORWARD)	// Drehrichtung beachten
    			sensEncL++;	//vorwaerts
    		else
    			sensEncL--;	//rueckwaerts
    	}

    	// --------------------- rechts ----------------------------
    	if (ENC_R != enc_r){	// uns interesieren nur Veraenderungen
    		enc_r=ENC_R;	// neuen wert sichern
    		enc_r_cnt=0;
    	} else{ // Zaehlen, wie lange Pegel bleibt
    		if (enc_r_cnt <= ENC_ENTPRELL) // Nur bis zur Entprell-Marke
    			enc_r_cnt++;
    	}

    	if (enc_r_cnt == ENC_ENTPRELL){// wenn lange genug konst
    		if (direction.right == DIRECTION_FORWARD)	// Drehrichtung beachten
    			sensEncR++;	//vorwaerts
    		else
    			sensEncR--;	//rueckwaerts
    	}
    }
    #endif

    #ifdef CALIBRATE_DIST_SENSOR
    /*! Diese Funktion fordert den Anwender auf, den Bot in vorgegebenen Abständen
     * zu einer Wand zu positionieren. Pro Abstandswert wird dann
     * der Abstand gemessen und aus diesem Wertepaar (Index, Wert)
     * eine Tabelle erzeugt, die einen Messwert eine Distanz zuweist.
     */

    void start_calibration(void) {
    	static uint8 actual_dist=5;
    	uint16 key, tmp_DistL,tmp_DistR, start,end,i,j,delta;
    	uint16 minL=9999;
    	uint16 minR=9999;
    	uint16 maxL=0;
    	uint16 maxR=0;

          // Zunächst löschen wir die Sensortabelle im EEPROM:
          for(i=0;i<=300;i++) {
    	   eeprom_write_byte(&eeDist_TableL[i], 0xFF);
    	   eeprom_write_byte(&eeDist_TableR[i], 0xFF);
          }

          display_screen=2;
    	display_cursor(1,1);
    	display_printf("Sensor-Kalibrierung");

    for(;;) {

    	display_cursor(2,1);
    	display_printf("Setze Abstand: %dcm",actual_dist);
    	display_cursor(3,1);
    	display_printf("<1 - weiter=2 - 3>");
    	display_cursor(4,1);
    	display_printf("4=fertig 0=Quit");

        key = RC5_MASK & ir_read();

     	switch (key) {

          case RC5_CODE_1:		// Abstand verkleinern
            actual_dist -= 5;
    		break;

          case RC5_CODE_2:		// Sensorwert fuer Abstand <actual_dist> lesen, mitteln und ins EEPROM schreiben
    		for (i=0,tmp_DistL=0,tmp_DistR=0;i<100;i++) {	// wir bilden den Mittelwert aus 100 Werten
     			sensDistL= adc_read(SENS_ABST_L);
    			sensDistR= adc_read(SENS_ABST_R);

    			if (sensDistL > 600) sensDistL=600;		// max. Sensorwerte begrenzen, sollte eigentlich
    			if (sensDistR > 600) sensDistR=600;		// nicht notwendig sein

    	 		display_cursor(1,1);
    			display_printf("                    ");	// unschoen, besser machen
    			display_cursor(1,1);
    			display_printf("L: %d, R: %d",sensDistL, sensDistR);	// Einzelmessung anzeigen
    			tmp_DistL += sensDistL;
    			tmp_DistR += sensDistR;
    		}
    		tmp_DistL=tmp_DistL/100;	// Mittelwert berechnen
    		tmp_DistR=tmp_DistR/100;
     		display_cursor(1,1);
    		display_printf("                    ");	// unschoen, besser machen
    		display_cursor(1,1);
    		display_printf("L: %d, R: %d",tmp_DistL, tmp_DistR);	// Mittelwert anzeigen

    		eeprom_write_byte(&eeDist_TableL[tmp_DistL>>1], actual_dist);	// und ins EEPROM damit
    		eeprom_write_byte(&eeDist_TableR[tmp_DistR>>1], actual_dist);	// Sensorwert <-> Abstand (cm)

    	   	if ((tmp_DistL)>maxL) maxL=tmp_DistL;	// wir merken uns den Beginn und das Ende der Tabelle
    	    	if ((tmp_DistR)>maxR) maxR=tmp_DistR;
    	    	if ((tmp_DistL)<minL) minL=tmp_DistL;
    	    	if ((tmp_DistR)<minR) minL=tmp_DistR;

     		while((RC5_MASK & ir_read()) == 0);		// auf Tastatureingabe warten

           	actual_dist += 5; 				// nach Ende einer Messung Distanz um 5cm erhöhen
    		break;

        case RC5_CODE_3:						// Abstand vergroessern
            actual_dist += 5;
    		break;

        case RC5_CODE_5:  						// TEST ROUTINE, liest EEprom aus
    		for (i=0;i<100;i++) {

    		display_printf("                    ");	// unschoen, besser machen
    		display_cursor(1,1);
    		display_printf("Sensor: %d = %dcm",eeprom_read_byte(i,&eeDist_TableL[i]));
    		while((RC5_MASK & ir_read()) == 0);		// auf Tastatureingabe warten

    		}
    		break;

        case RC5_CODE_4:						// Messwerte wurden in EEPROM gespeichert, jetzt muss im EEPROM
    									// zwischen den Messwerten interpoliert werden.
    		start=minL;						// Anfang der Tabelle
    		for (i=minL+1; i<=maxL;i++) {
    			if (eeprom_read_byte(&eeDist_TableL[i]) != 0xFF) {	// Stuetzwert gefunden
    				end=i;
    				// wir berechnen den Wert um den sich die Werte zwischen den Stützwerten verändern müssen
    				delta=(eeprom_read_byte(&eeDist_TableL[end])-eeprom_read_byte(&eeDist_TableL[start])) / (end - start);
    				for (j=start+1;j< end;j++) {	//Tabellenfelder zwischen den Stuetzwerten ausfüllen
    					eeprom_write_byte(&eeDist_TableL[j], eeprom_read_byte(&eeDist_TableL[j-1]+delta));
    				}
    				start=end;
    			}
         		}

    		start=minR;						// und das gleiche nochmal fuer die Tabelle des rechten Sensor
    		for (i=minR+1; i<=maxR;i++) {
    			if (eeprom_read_byte(&eeDist_TableR[i]) != 0xFF) {
    				end=i;
    				delta=(eeprom_read_byte(&eeDist_TableR[end])-eeprom_read_byte(&eeDist_TableR[start])) / (end - start);
    				for (j=start+1;j< end;j++) {
    					eeprom_write_byte(&eeDist_TableR[j], eeprom_read_byte(&eeDist_TableR[j-1]+delta));
    				}
    				start=end;
    			}
         		}
    		break;

        case RC5_CODE_0:						// wir haben fertig ...
            	display_screen=2;
    		return;

          default:
    		continue;

    	}
    }

    }

    #endif

- Voriger (Datum): Re: [ct-bot] Noch'n Sensorpatch zum Testen, Michail Brzitwa
- Nächster (Datum): [ct-bot] rc5_bot_next_behaviour -> Verhalten visualisieren, Menzel, Frank IT-OO4
- Voriger (Thread): Re: [ct-bot] Noch'n Sensorpatch zum Testen, Michail Brzitwa
- Nächster (Thread): [ct-bot] rc5-codes für Fernbedienung VIVANCO UR89, Andreas Staudenmayer
- Indizes:
  - Datum
  - Thread