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;
}