Hallo recycle,
Bei meinem UCBalBot habe ich mit dem digitalen PSD Sensor eine Auflösung von etwa 0,5 Grad / Stufe erreicht.
Der Neigungswinkel 0 liegt bei einem gemessenen Abstand von etwa 14 cm (RAWZERO).
Eine einfach Linearisierung mit dem Faktor M_ALPHA um diesen Punkt der Kennlinie reicht aus.
Siehe auch den beiliegenden Programmauszug.
Der Gyro wird mit einem 10Bit ADC ausgewertet. Aufgrund des Rauschens und sonstiger Störungen
sind davon aber höchstens 7-8Bit relevant. Deshalb müsste eine Auflösung von 0,5 bis 1 Grad/s / Stufe ausreichend sein.
(Der CG-16 von Tokin hat eine Empfindlichkeit von etwa 90 Grad/s, das /100 ergibt 0,9 Grad/s)
Code:/********************************************************
| File: SensorGP2D02.c
| Project: UCBalBot
| Authors: Ulrich Czarkowski
| u.czarkowski@t-online.de
| Created: 18.01.04
| last changes: 20.03.04 16:24
|
| Description: IR remote balance control with const Timing
|
| 13.03.04 16:32 First balancing on the spot for more than 20 sec
| K [5, 6, 2, 2]
| 21.03.04 20:37 K [6,6,2,2]
|
*****************************************************************/
#include "irtv.h"
#include <stdio.h>
#include "qsmregs.h"
/* Nokia remote control codes */
#define CODE_0 0x6142
#define CODE_1 0x61fd
#define CODE_2 0x6102
#define CODE_3 0x60fd
#define CODE_4 0x6082
#define CODE_5 0x617d
#define CODE_6 0x6182
#define CODE_7 0x607d
#define CODE_8 0x6042
#define CODE_9 0x61bd
/* key => function */
#define CODE_FWD 0x6112 /* "play" => forward */
#define CODE_BACK 0x606d /* "stop" => backward */
#define CODE_LEFT 0x60ed /* "rewind" => left */
#define CODE_RIGHT 0x61ed /* "ffwd" => right */
#define CODE_STOP 0x61dd /* "+" => stop */
#define CODE_STOPM 0x6122 /* "-" => stop moving */
#define CODE_BALAN 0x619D /* "TIMER" => balance */
#define CODE_END 0x61a2 /* "power" => end */
#define CODE_OK 0x6166 /* "OK" => tbd */
/* Normal speed */
#define NORMAL_V 0.03
#define NORMAL_W 0.10
/* angle calculation */
/* Sensor: GP2D120 */
#define MS 31742000 /* M-Strich * 1000 */
#define BS -6 /* B-Strich */
#define KS 4000 /* K-Strich * 1000 */
#define HR 116000 /* Sensor z-distance µm */
#define BR 92000 /* Sensor x-distance µm */
#define PI_BR 16 /* Rad/radius: (1/(180/PI/BR)) * 100 = 1605 */
/* angle calculation */
/* Sensor: GP2D02 */
#define M_ALPHA 50 /* 0.50 * 100 */
#define M_ALPHAM -58 /* 0.50 * 100 */
#define RAWZERO 78 /* 0 deg */
#define RAWMIN 29 /* +20 deg */
#define RAWMAX 103 /* -20 deg */
#define TILTSENSOR 2 /* ad channel 02 */
#define GYROSENSOR 3 /* ad channel 03 */
#define TEMPSENSOR 5 /* ad channel 05 */
#define GYRONULL 615 /* Initial Null */
#define TEMPNULL 631 /* Initial Temp 24°C */
/* Gain of state space controller */
#define K1 42
#define K2 170
#define K3 10
#define K4 10
#define KNORM 1.0/100000.0
#define FTORQUEMAX 1.9
#define FTORQUEMIN -1.9
/* Integrating constant */
#define KI 10
#define Ti 21
/* define Timings */
#define TA 1 /* 1 * 10 ms = 10 ms cycle time */
#define TVW 1 /* 1 * 10 ms = 10 ms VW-IRQ */
/* define globals */
int iAdcValue;
int iGyroNull;
static int tiltangle = 0;
static int raw = 0;
static int tiltraw = 0;
static int iLastAngle = 0;
static int angle = 0;
static int angle_velocity = 0;
static int sim_angle = 0;
static int torque = 0;
static int k1_theta = 0;
static int k2_thetadot = 0;
static int k3_x = 0;
static int k3_x_old = 0;
static int k4_xdot = 0;
static float ftorque = 0;
static float v = 0;
static float w = 0;
/* define Handles */
PSDHandle psd_handle;
VWHandle vw_handle;
TimerHandle timer_handle;
/* num_code:
returns the integer value for a numeric remote key (-1 for other keys) */
int num_code(int code)
{
int i;
const int CodeNum[10] =
{CODE_0, CODE_1, CODE_2, CODE_3, CODE_4,
CODE_5, CODE_6, CODE_7, CODE_8, CODE_9};
for (i=0; i<10; i++) {
if (code == CodeNum[i]) return i;
}
return -1;
}
/* num_input: wait for remote key 0..9 */
int num_input(void)
{
int num;
do {
num = num_code(IRTVGet());
if (num < 0) AUTone(500, 150);
} while (num < 0);
LCDPrintf("%d\n", num);
return num;
}
/******************************************
* iAdcRead(int iChan)
* Disable Interrupt, clear SPIF, double Read
*******************************************/
int iAdcRead(int iChan)
{ int iMax;
int i, dummy;
iMax = 2;
for (i = 0; i < iMax; i++)
{
/* dummy = OSDisable(); */
iAdcValue = OSGetAD(iChan);
spsr = 0; /* clear SPIF */
/* dummy = OSEnable(); */
}
return iAdcValue;
}
int iGyroInit(void)
{ int i, iGyroValue = 0;
for (i = 0; i < 1000; i++)
{
iGyroValue += iAdcRead(GYROSENSOR);
}
return (iGyroValue /1000);
}
/***************************************
* Tilt angle measurement
* Sensor: GP2D02
***************************************/
static int tiltangle_calc(void)
{
if (PSDCheck()) raw = PSDGetRaw(psd_handle); /* read value if valid */
if (raw > RAWMAX) raw = RAWMAX;
else if (raw < RAWMIN) raw = RAWMIN;
tiltangle = (RAWZERO - raw)* M_ALPHA;
tiltraw = raw;
return tiltangle;
}
