348 lines
13 KiB
C
348 lines
13 KiB
C
|
/*
|
||
|
|
* 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;
|
||
|
|
}
|
||
|
|
|