c't Projekte - c't-Bot und c't-Sim -
Mailinglisten
[Voriger (Datum)]
[Nächster (Datum)]
[Voriger (Thread)]
[Nächster (Thread)]
[Nach Datum][Nach Thread]
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