diff options
Diffstat (limited to 'source/ct-Bot/sensor.c')
| -rw-r--r-- | source/ct-Bot/sensor.c | 347 |
1 files changed, 347 insertions, 0 deletions
diff --git a/source/ct-Bot/sensor.c b/source/ct-Bot/sensor.c new file mode 100644 index 0000000..7029d42 --- /dev/null +++ b/source/ct-Bot/sensor.c @@ -0,0 +1,347 @@ +/* + * c't-Sim - Robotersimulator fuer den 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.c + * @brief Architekturunabhaengiger Teil der Sensorsteuerung + * @author Benjamin Benz (bbe@heise.de) + * @date 15.01.05 +*/ +#include <stdio.h> +#include "ct-Bot.h" +#include "timer.h" +#include "bot-local.h" +#include "math.h" +#include "sensor_correction.h" +#ifdef SRF10_AVAILABLE + #include "srf10.h" +#endif + +// Defines einiger, haeufiger benoetigter Konstanten +#define DEG2RAD (2*M_PI/360) + + +int16 sensLDRL=0; /*!< Lichtsensor links */ +int16 sensLDRR=0; /*!< Lichtsensor rechts */ + +int16 sensDistL=1023; /*!< Distanz linker IR-Sensor in [mm], wenn korrekt umgerechnet wird */ +int16 sensDistR=1023; /*!< Distanz rechter IR-Sensor in [mm], wenn korrekt umgerechnet wird */ + +int16 sensBorderL=0; /*!< Abgrundsensor links */ +int16 sensBorderR=0; /*!< Abgrundsensor rechts */ + +int16 sensLineL=0; /*!< Lininensensor links */ +int16 sensLineR=0; /*!< Lininensensor rechts */ + +uint8 sensTrans=0; /*!< Sensor Ueberwachung Transportfach */ + +uint8 sensDoor=0; /*!< Sensor Ueberwachung Klappe */ + +uint8 sensError=0; /*!< Ueberwachung Motor oder Batteriefehler */ + +#ifdef MAUS_AVAILABLE + + int8 sensMouseDX; /*!< Maussensor Delta X, positive Werte zeigen querab der Fahrtrichtung nach rechts */ + int8 sensMouseDY; /*!< Maussensor Delta Y, positive Werte zeigen in Fahrtrichtung */ + + int16 sensMouseX; /*!< Mausposition X, positive Werte zeigen querab der Fahrtrichtung nach rechts */ + int16 sensMouseY; /*!< Mausposition Y, positive Werte zeigen in Fahrtrichtung */ +#endif + +volatile int16 sensEncL=0; /*!< Encoder linkes Rad */ +volatile int16 sensEncR=0; /*!< Encoder rechtes Rad */ +float heading_enc=0; /*!< Blickrichtung aus Encodern */ +float x_enc=0; /*!< X-Koordinate aus Encodern [mm] */ +float y_enc=0; /*!< Y-Koordinate aus Encodern [mm] */ +float v_enc_left=0; /*!< Abrollgeschwindigkeit des linken Rades in [mm/s] [-128 bis 127] relaisitisch [-50 bis 50] */ +float v_enc_right=0; /*!< Abrollgeschwindigkeit des linken Rades in [mm/s] [-128 bis 127] relaisitisch [-50 bis 50] */ +float v_enc_center=0; /*!< Schnittgeschwindigkeit ueber beide Raeder */ + +#ifdef PC + uint16 simultime=0; /*! Simulierte Zeit */ +#endif + +#ifdef MEASURE_MOUSE_AVAILABLE + float heading_mou=0; /*!< Aktuelle Blickrichtung relativ zur Startposition aus Mausmessungen */ + float x_mou=0; /*!< Aktuelle X-Koordinate in mm relativ zur Startposition aus Mausmessungen */ + float y_mou=0; /*!< Aktuelle Y-Koordinate in mm relativ zur Startposition aus Mausmessungen */ + float v_mou_center=0; /*!< Geschwindigkeit in mm/s ausschliesslich aus den Maussensorwerten berechnet */ + float v_mou_left=0; /*!< ...aufgeteilt auf linkes Rad */ + float v_mou_right=0; /*!< ...aufgeteilt auf rechtes Rad */ +#endif + +float heading=0; /*!< Aktuelle Blickrichtung aus Encoder-, Maus- oder gekoppelten Werten */ +float x_pos=0; /*!< Aktuelle X-Position aus Encoder-, Maus- oder gekoppelten Werten */ +float y_pos=0; /*!< Aktuelle Y-Position aus Encoder-, Maus- oder gekoppelten Werten */ +float v_left=0; /*!< Geschwindigkeit linkes Rad aus Encoder-, Maus- oder gekoppelten Werten */ +float v_right=0; /*!< Geschwindigkeit rechtes Rad aus Encoder-, Maus- oder gekoppelten Werten */ +float v_center=0; /*!< Geschwindigkeit im Zentrum des Bots aus Encoder-, Maus- oder gekoppelten Werten */ + +int8 sensors_initialized = 0; /*!< Wird 1 sobald die Sensorwerte zur Verfügung stehen */ + +#ifdef SRF10_AVAILABLE + uint16 sensSRF10; /*!< Messergebniss Ultraschallsensor */ +#endif + + +/*! Linearisiert die Sensorwerte + * @param left Linker Rohwert [0-1023] + * @param right Rechter Rohwert [0-1023] + */ +void sensor_abstand(uint16 left, uint16 right){ + if (left == SENSDISTOFFSETLEFT) // Vermeidet Div/0 + left++; + sensDistL = SENSDISTSLOPELEFT / (left - SENSDISTOFFSETLEFT); + // Korrigieren, wenn ungueltiger Wert + if (sensDistL > SENS_IR_MAX_DIST || sensDistL<=0) + sensDistL=SENS_IR_INFINITE; + + if (right == SENSDISTOFFSETRIGHT) // Vermeidet Div/0 + right++; + sensDistR = SENSDISTSLOPERIGHT / (right - SENSDISTOFFSETRIGHT); + // Korrigieren, wenn ungueltiger Wert + if (sensDistR > SENS_IR_MAX_DIST || sensDistR<=0) + sensDistR=SENS_IR_INFINITE; +} + +/*! Sensor_update +* Kuemmert sich um die Weiterverarbeitung der rohen Sensordaten +*/ +void sensor_update(void){ + static uint16 old_pos=0; /*!< Ticks fuer Positionsberechnungsschleife */ + static uint16 old_speed=0; /*!< Ticks fuer Geschwindigkeitsberechnungsschleife */ + #ifdef MEASURE_MOUSE_AVAILABLE + static int16 lastMouseX=0; /*!< letzter Mauswert X fuer Positionsberechnung */ + static int16 lastMouseY=0; /*!< letzter Mauswert Y fuer Positionsberechnung */ + static float lastDistance=0; /*!< letzte gefahrene Strecke */ + static float lastHead=0; /*!< letzter gedrehter Winkel */ + static float oldHead=0; /*!< Winkel aus dem letzten Durchgang */ + static float old_x=0; /*!< Position X aus dem letzten Durchgang */ + static float old_y=0; /*!< Position Y aus dem letzten Durchgang */ + float radius=0; /*!< errechneter Radius des Drehkreises */ + float s1=0; /*!< Steigung der Achsengerade aus dem letzten Durchgang */ + float s2=0; /*!< Steigung der aktuellen Achsengerade */ + float a1=0; /*!< Y-Achsenabschnitt der Achsengerade aus dem letzten Durchgang */ + float a2=0; /*!< Y-Achsenabschnitt der aktuellen Achsengerade */ + float xd=0; /*!< X-Koordinate Drehpunkt */ + float yd=0; /*!< Y-Koordinate Drehpunkt */ + float right_radius=0; /*!< Radius des Drehkreises des rechten Rads */ + float left_radius=0; /*!< Radius des Drehkreises des linken Rads */ + #endif + static int16 lastEncL =0; /*!< letzter Encoderwert links fuer Positionsberechnung */ + static int16 lastEncR =0; /*!< letzter Encoderwert rechts fuer Positionsberechnung */ + static int16 lastEncL1=0; /*!< letzter Encoderwert links fuer Geschwindigkeitsberechnung */ + static int16 lastEncR1=0; /*!< letzter Encoderwert rechts fuer Geschwindigkeitsberechnung */ + float dHead=0; /*!< Winkeldifferenz aus Encodern */ + float deltaY=0; /*!< errechneter Betrag Richtungsvektor aus Encodern */ + int16 diffEncL; /*!< Differenzbildung linker Encoder */ + int16 diffEncR; /*!< Differenzbildung rechter Encoder */ + float sl; /*!< gefahrene Strecke linkes Rad */ + float sr; /*!< gefahrene Strecke rechtes Rad */ + #ifdef MEASURE_MOUSE_AVAILABLE + int16 dX; /*!< Differenz der X-Mauswerte */ + int16 dY; /*!< Differenz der Y-Mauswerte */ + int8 modifiedAngles=False; /*!< Wird True, wenn aufgrund 90 Grad oder 270 Grad die Winkel veraendert werden mussten */ + + sensMouseY += sensMouseDY; /*!< Mausdelta Y aufaddieren */ + sensMouseX += sensMouseDX; /*!< Mausdelta X aufaddieren */ + #endif + + register uint16 ticks = TIMER_GET_TICKCOUNT_16; + if (ticks-old_pos > MS_TO_TICKS(50)){ + old_pos = ticks; + /* Gefahrene Boegen aus Encodern berechnen */ + diffEncL=sensEncL-lastEncL; + diffEncR=sensEncR-lastEncR; + lastEncL=sensEncL; + lastEncR=sensEncR; + sl=(float)diffEncL*WHEEL_PERIMETER/ENCODER_MARKS; + sr=(float)diffEncR*WHEEL_PERIMETER/ENCODER_MARKS; + /* Winkel berechnen */ + dHead=(float)(sr-sl)/(2*WHEEL_DISTANCE); + /* Winkel ist hier noch im Bogenmass */ + /* Position berechnen */ + /* dazu Betrag des Vektors berechnen */ + if (dHead==0) { + /* Geradeausfahrt, deltaY=diffEncL=diffEncR */ + deltaY=sl; + } else { + /* Laenge berechnen aus alpha/2 */ + deltaY=(sl+sr)*sin(dHead/2)/dHead; + } + /* Winkel in Grad umrechnen */ + dHead=dHead/DEG2RAD; + + /* neue Positionen berechnen */ + heading_enc+=dHead; + while (heading_enc>=360) heading_enc=heading_enc-360; + while (heading_enc<0) heading_enc=heading_enc+360; + + x_enc+=(float)deltaY*cos(heading_enc*DEG2RAD); + y_enc+=(float)deltaY*sin(heading_enc*DEG2RAD); + #ifdef MEASURE_MOUSE_AVAILABLE + dX=sensMouseX-lastMouseX; + /* heading berechnen */ + dHead=(float)dX*360/MOUSE_FULL_TURN; + heading_mou+=dHead; + lastHead+=dHead; + if (heading_mou>=360) heading_mou=heading_mou-360; + if (heading_mou<0) heading_mou=heading_mou+360; + /* x/y pos berechnen */ + dY=sensMouseY-lastMouseY; + x_mou+=(float)dY*cos(heading_mou*DEG2RAD)*25.4/MOUSE_CPI; + lastDistance+=dY*25.4/MOUSE_CPI; + y_mou+=(float)dY*sin(heading_mou*DEG2RAD)*25.4/MOUSE_CPI; + + lastMouseX=sensMouseX; + lastMouseY=sensMouseY; + #endif + #ifdef MEASURE_COUPLED_AVAILABLE + /* Werte der Encoder und der Maus mit dem Faktor G_POS verrechnen */ + x_pos=G_POS*x_mou+(1-G_POS)*x_enc; + y_pos=G_POS*y_mou+(1-G_POS)*y_enc; + /* Korrektur, falls mou und enc zu unterschiedlichen Seiten zeigen */ + if (fabs(heading_mou-heading_enc) > 180) { + /* wir nutzen zum Rechnen zwei Drehrichtungen */ + heading = heading_mou<=180 ? heading_mou*G_POS : (heading_mou-360)*G_POS; + heading += heading_enc<=180 ? heading_enc*(1-G_POS) : (heading_enc-360)*(1-G_POS); + /* wieder auf eine Drehrichtung zurueck */ + if (heading < 0) heading += 360; + } else + heading = G_POS*heading_mou + (1-G_POS)*heading_enc; + if (heading >= 360) heading -= 360; + #else + #ifdef MEASURE_MOUSE_AVAILABLE + /* Mauswerte als Standardwerte benutzen */ + heading=heading_mou; + x_pos=x_mou; + y_pos=y_mou; + #else + /* Encoderwerte als Standardwerte benutzen */ + heading=heading_enc; + x_pos=x_enc; + y_pos=y_enc; + #endif + #endif + } + ticks = TIMER_GET_TICKCOUNT_16; + if (ticks-old_speed > MS_TO_TICKS(250)){ + old_speed=ticks; + v_enc_left= (((sensEncL - lastEncL1) * WHEEL_PERIMETER) / ENCODER_MARKS)*4; + v_enc_right= (((sensEncR - lastEncR1) * WHEEL_PERIMETER) / ENCODER_MARKS)*4; + v_enc_center=(v_enc_left+v_enc_right)/2; + lastEncL1= sensEncL; + lastEncR1= sensEncR; + #ifdef MEASURE_MOUSE_AVAILABLE + /* Speed aufgrund Maussensormessungen */ + v_mou_center=lastDistance*4; + /* Aufteilung auf die Raeder zum Vergleich mit den Radencodern */ + /* Sonderfaelle pruefen */ + if (oldHead==90 || oldHead==270 || heading_mou==90 || heading_mou==270) { + float temp; + /* winkel um 90 Grad vergroessern */ + oldHead+=90; + heading_mou+=90; + // Koordinaten anpassen + temp=old_x; + old_x=old_y; + old_y=-temp; + temp=x_mou; + x_mou=y_mou; + y_mou=-temp; + modifiedAngles=True; + } + + /* Steigungen berechnen */ + s1=-tan(oldHead*DEG2RAD); + s2=-tan(heading_mou*DEG2RAD); + + /* Geradeausfahrt? (s1==s2) */ + if (s1==s2) { + /* Bei Geradeausfahrt ist v_left==v_right==v_center */ + v_mou_left=v_mou_right=v_mou_center; + } else { + /* y-Achsenabschnitte berechnen */ + a1=old_x-s1*old_y; + a2=x_mou-s2*y_mou; + /* Schnittpunkt berechnen */ + yd=(a2-a1)/(s1-s2); + xd=s2*yd+a2; + /* Radius ermitteln */ + radius=sqrt((x_mou-xd)*(x_mou-xd)+(y_mou-yd)*(y_mou-yd)); + /* Vorzeichen des Radius feststellen */ + if (lastHead<0) { + /* Drehung rechts, Drehpunkt liegt rechts vom Mittelpunkt + * daher negativer Radius */ + radius=-radius; + } + if (v_mou_center<0) { + /* rueckwaerts => links und rechts vertauscht, daher VZ vom Radius umdrehen */ + radius=-radius; + } + /* Geschwindigkeiten berechnen */ + right_radius=radius+WHEEL_DISTANCE; + left_radius=radius-WHEEL_DISTANCE; + v_mou_right=lastHead/360*2*M_PI*right_radius*4; + v_mou_left=lastHead/360*2*M_PI*left_radius*4; + } + /* Falls Koordinaten/Winkel angepasst wurden, nun wieder korrigieren */ + if (modifiedAngles){ + float temp; + /* Winkel wieder um 90 Grad reduzieren */ + oldHead-=90; + heading_mou-=90; + /* Koordinaten wieder korrigieren */ + temp=old_x; + old_x=-old_y; + old_y=temp; + temp=x_mou; + x_mou=-y_mou; + y_mou=temp; + } + lastDistance=0; + lastHead=0; + old_x=x_mou; + old_y=y_mou; + oldHead=heading_mou; + #endif + #ifdef MEASURE_COUPLED_AVAILABLE + v_left=G_SPEED*v_mou_left+(1-G_SPEED)*v_enc_left; + v_right=G_SPEED*v_mou_right+(1-G_SPEED)*v_enc_left; + v_center=G_SPEED*v_mou_center+(1-G_SPEED)*v_enc_center; + #else + #ifdef MEASURE_MOUSE_AVAILABLE + /* Mauswerte als Standardwerte benutzen */ + v_left=v_mou_left; + v_right=v_mou_right; + v_center=v_mou_center; + #else + /* Encoderwerte als Standardwerte benutzen */ + v_left=v_enc_left; + v_right=v_enc_right; + v_center=v_enc_center; + #endif + #endif + #ifdef SRF10_AVAILABLE + sensSRF10 = srf10_get_measure(); /*!< Messung Ultraschallsensor */ + #endif + } + + sensors_initialized=1; +} + |