1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
|
/*
* 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@heise.de)
* @date 01.12.05
*/
#ifdef MCU
#include <avr/io.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 "bot-logic/available_behaviours.h"
#ifdef BEHAVIOUR_SERVO_AVAILABLE
#include "bot-logic/behaviour_servo.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 12 /*!< Nur wenn der Encoder ein paar mal den gleichen wert gibt uebernehmen */
/*!
* 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();
}
/*!
* 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);
#ifdef MAUS_AVAILABLE
// Aktualisiere die Position des Maussensors
sensMouseDX = maus_sens_read(MOUSE_DELTA_X_REG);
sensMouseDY = maus_sens_read(MOUSE_DELTA_Y_REG);
#endif
// ---------- 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
// Abfrage nur alle 100ms
static uint16 old_dist; // Zeit der letzten Messung der Distanzsensoren
static uint8 measure_count;
static int16 distLeft[3];
static int16 distRight[3];
register uint16 dist_ticks = TIMER_GET_TICKCOUNT_16;
if (dist_ticks-old_dist > MS_TO_TICKS(100)){
// Zeit fuer naechste Messung merken
old_dist=dist_ticks;
// wenn Kalibrierung gewuenscht, den Part Messen und Korrigieren kommentieren
// und Kalibrieren auskommentieren
// Kalibirieren
//distL=adc_read(SENS_ABST_L);
//distR=adc_read(SENS_ABST_R);
// Messwert merken
distLeft[measure_count]=adc_read(SENS_ABST_L);
#ifdef BEHAVIOUR_SERVO_AVAILABLE
if ((servo_active & SERVO1) == 0) // Wenn die Transportfachklappe bewegt wird, stimmt der Messwert des rechten Sensor nicht
#endif
distRight[measure_count]=adc_read(SENS_ABST_R);
measure_count++;
if (measure_count==3) measure_count=0;
// Schnittwert bilden
sensor_abstand((distLeft[0]+distLeft[1]+distLeft[2])/3,(distRight[0]+distRight[1]+distRight[2])/3);
}
// ------- 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){
static uint8 enc_l=0; /*!< Puffer fuer die letzten Encoder-Staende */
static uint8 enc_r=0; /*!< Puffer fuer die letzten Encoder-Staende */
static uint8 enc_l_cnt=0; /*!< Entprell-Counter fuer L-Encoder */
static uint8 enc_r_cnt=0; /*!< Entprell-Counter fuer R-Encoder */
// --------------------- 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
|
