heise online · c't · iX · Technology Review · Telepolis · mobil · Security · Netze · heise open · heise resale · Autos · c't-TV · Jobs · Kiosk
Zum Inhalt
c't

c't Projekte - c't-Bot und c't-Sim - Mailinglisten

c't-Bot und c't-Sim


[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	


Copyright © 2007 Heise Zeitschriften Verlag Kritik, Anregungen bitte an c't-WWW Datenschutzhinweis   Impressum