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