본문 바로가기

Robot/Project

자이로센서의 출력에 온도를 고려하기


(주의 ) 이 글은 작성 후 다시 검토해본 결과 오류가 다수 검출되고 검증되지 않은 글입니다. 읽으시는 분들은 참고하시길 바랍니다. (2011년 2월 25일)

이번글은 두개의 의문사항을 가지고있습니다. 혼자힘으로 좀 해결하기 어렵길래 혹시 지나가던 고수가 계시다면 살짝 도움말을 요청하는 그런 글이라고 보시면 되겠습니다. 뭔가 잘 되지 않아 질문을 할때는 여러가지 상황을 자세히 설명해야겠기에 포스팅하는 거라는....^^;

먼저 지난번에 자이로센서(myGyro300SPI)의 단순 테스트결과를 보여드렸었습니다. -자이로센서를 이용한 각도검출과 그 한계 - 그 글의 댓글에 류크님이나 님이 살짝 말씀하신데로 온도값도 받고 아날로그 출력을 가지는 자이로를 myGyro300SPI에 물려 SPI로 데이터를 받는 것을 테스트해보고 싶어졌습니다. 그래서 안내문서를 확인하고


어디를 수정해야될지 보니까


저 표시된 부분을


이렇게 바꿔가며 원하는 데이터를 받을 수 있더군요. 뭐 몹시 간단하다 생각했습니다. 0x8310보내고 자이로값받고, 0x8710보내고 온도값받고... 등등등....

그래서 별도로 함수화하고 어쩌고 하기위해 헤더 화일을 다시 만들었습니다.

<myGyro300.h>

#ifndef MYGYRO300_H_
#define MYGYRO300_H_

void InitSSI(void);
unsigned long ReadGyro(unsigned short gyroOutType);

#define Gyro0_CS_PERIPH			SYSCTL_PERIPH_GPIOD
#define Gyro0_CS_PORT_BASE		GPIO_PORTD_BASE
#define Gyro0_CS_PIN			GPIO_PIN_0
#define Gyro0_CS_ON()			GPIOPinWrite(Gyro0_CS_PORT_BASE, Gyro0_CS_PIN, 0)
#define Gyro0_CS_OFF()			GPIOPinWrite(Gyro0_CS_PORT_BASE, Gyro0_CS_PIN, Gyro0_CS_PIN)

#endif


<myGyro300.c>
/*
 * myGyro300.c
 *
 *  Created on: 2009. 6. 12
 *      Author: PinkWink
 */

#include "../../../hw_types.h"
#include "../../../hw_memmap.h"
#include "hw_ssi.h"
#include "ssi.h"
#include "sysctl.h"
#include "gpio.h"
#include "myGyro300.h"

void InitSSI(void)
{
	SysCtlPeripheralEnable(Gyro0_CS_PERIPH);
	GPIOPinTypeGPIOOutput(Gyro0_CS_PORT_BASE, Gyro0_CS_PIN);
	Gyro0_CS_OFF();

    SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI0);
    GPIOPinTypeSSI(GPIO_PORTA_BASE, GPIO_PIN_2 | GPIO_PIN_4 | GPIO_PIN_5);
    GPIOPadConfigSet(GPIO_PORTA_BASE, GPIO_PIN_2 | GPIO_PIN_4 | GPIO_PIN_5, GPIO_STRENGTH_4MA,
                     GPIO_PIN_TYPE_STD_WPU);
    SSIConfigSetExpClk(SSI0_BASE, SysCtlClockGet(), SSI_FRF_MOTO_MODE_2,
                        SSI_MODE_MASTER, 1000000, 16);
    SSIEnable(SSI0_BASE);
}

unsigned long ReadGyro(unsigned short gyroOutType)
{
	unsigned long data;

	Gyro0_CS_ON();
	SSIDataPut(SSI0_BASE, gyroOutType);
	Gyro0_CS_OFF();
	Gyro0_CS_ON();
	SSIDataGet(SSI0_BASE, &data);
	Gyro0_CS_OFF();

	return data;
}


<QEI_encoder.c>
//*****************************************************************************
//
// Withrobot에서 배포하는
// 타이머, 엔코더, ADC, myAccel3LV02_I2C, myGyro300SPI 예제를
// 적절히 통합하여 일정시간(Sampling Time = 10ms)마다
// 엔코더의 위치, 가속도센서의 3축, 자이로값을 시리얼통신으로 전송하게
// 2009.06.13에 PinkWink가 수정함. (www.pinkwink.kr)
//
//*****************************************************************************

#include 
#include 
#include 
#include "../../../hw_types.h"
#include "../../../hw_memmap.h"
#include "hw_ssi.h"
#include "ssi.h"
#include "i2c.h"
#include "sysctl.h"
#include "gpio.h"
#include "uart.h"
#include "qei.h"
#include "ustdlib.h"
#include "timer.h"
// #include "adc.h"
#include "interrupt.h"
#include "myGyro300.h"

#include "myAccel3LV02.h"
#include "myAccel3LV02_hal_i2c.h"

#ifdef DEBUG
void
__error__(char *pcFilename, unsigned long ulLine)
{
}
#endif

#define BUFFER_LEN          	32

static void UARTPutString(char * str);
void UARTSend(const char *pucBuffer, unsigned long ulCount);
static void InitTIMERINT(void);
static void InitUART(void);
static void InitQEI(void);
// static void InitADC(void);
static void TimerIntHandler(void);

int main(void)
{
    SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_8MHZ);

    InitTIMERINT();
    InitUART();
    InitQEI();
    InitSSI();
    MA3_Init();
    //InitADC();

    // Loop Start.
    while(1)
    {
    }
}

static void TimerIntHandler(void)
{
    unsigned long pos;
    unsigned long data;
    char buffer[BUFFER_LEN];
    // short data_short_array[3];
    // unsigned long adc_result[16];
    // unsigned long cnt;

	// Clear interrupt flag
	TimerIntClear( TIMER0_BASE, TIMER_TIMA_TIMEOUT );

	// Read Encoder Data
    pos = QEIPositionGet(QEI0_BASE);
    usnprintf(buffer, BUFFER_LEN, "%d, ", pos);
    buffer[BUFFER_LEN - 1] = 0;
    UARTPutString(buffer);

    // Read Gyro angle myGyro300SPI Data, SPI
    data = ReadGyro(0x8310);
    usnprintf(buffer, BUFFER_LEN, "%d,  ", data & 0x0FFF);
    buffer[BUFFER_LEN - 1] = 0;
    UARTPutString(buffer);

    // Read Gyro temp Data, SPI
    data = ReadGyro(0x8710);
    usnprintf(buffer, BUFFER_LEN, "%d;\n  ", data & 0x0FFF);
    buffer[BUFFER_LEN - 1] = 0;
    UARTPutString(buffer);

    /*
    // Read Gyro NT-Gyro300 X Data, SPI
    data = ReadGyro(0x8B10);
    usnprintf(buffer, BUFFER_LEN, "%d;\n  ", data & 0x0FFF);
    buffer[BUFFER_LEN - 1] = 0;
    UARTPutString(buffer);
    */

    /*
    // Read Gyro NT-Gyro300 Y Data, SPI
    data = ReadGyro(0x8F10);
    usnprintf(buffer, BUFFER_LEN, "%d,  ", data & 0x0FFF);
    buffer[BUFFER_LEN - 1] = 0;
    UARTPutString(buffer);
    */

    /*
    // Read Gyro NT-Gyro300 X Data, ADC
    cnt = ADCSequenceDataGet(ADC_BASE, 0, adc_result);
	usnprintf(buffer, BUFFER_LEN, "%d;\n  ", adc_result[0]);
	buffer[BUFFER_LEN - 1] = 0;
	UARTSend(buffer, strlen(buffer));
	*/

    /*
	// Read Accel Data
    MA3_Read(MA3_REG_OUTX_L, (unsigned char *)data_short_array, sizeof(data_short_array));
	usnprintf(buffer, BUFFER_LEN, "%d,  %d,  %d;\n", data_short_array[0], data_short_array[1], data_short_array[2]);
	buffer[BUFFER_LEN - 1] = 0;
	UARTPutString(buffer);
	*/
}

static void UARTPutString(char * str)
{
    while(*str)
        UARTCharPut(UART0_BASE, *str++);
}

void UARTSend(const char *pucBuffer, unsigned long ulCount)
{
    //
    // Loop while there are more characters to send.
    //
    while(ulCount--)
    {
        //
        // Write the next character to the UART.
        //
        UARTCharPut(UART0_BASE, *pucBuffer++);
    }
}

static void InitTIMERINT(void)
{
    SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
    TimerConfigure( TIMER0_BASE, TIMER_CFG_32_BIT_PER );
    TimerLoadSet( TIMER0_BASE, TIMER_A, SysCtlClockGet() / 100 );	// 10ms
    TimerIntRegister( TIMER0_BASE, TIMER_A, TimerIntHandler );
    IntMasterEnable();
    TimerIntEnable( TIMER0_BASE, TIMER_TIMA_TIMEOUT );
    TimerEnable( TIMER0_BASE, TIMER_A );
}

static void InitUART(void)
{
    SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
    GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
    UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), 115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
}

static void InitQEI(void)
{
    SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);

    GPIOPinTypeQEI(GPIO_PORTC_BASE, GPIO_PIN_4 | GPIO_PIN_6);
    QEIConfigure(QEI0_BASE, (QEI_CONFIG_CAPTURE_A_B | QEI_CONFIG_NO_RESET | QEI_CONFIG_QUADRATURE | QEI_CONFIG_NO_SWAP), 0xffffffff);
    QEIVelocityConfigure(QEI0_BASE, QEI_VELDIV_1, SysCtlClockGet() / 100);

    QEIEnable(QEI0_BASE);
    QEIVelocityEnable(QEI0_BASE);
}

/*
static void InitADC(void)
{
	SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC);
	SysCtlADCSpeedSet(SYSCTL_ADCSPEED_500KSPS);
	ADCSequenceDisable(ADC_BASE, 0);
	ADCSequenceConfigure(ADC_BASE, 0, ADC_TRIGGER_ALWAYS, 0);
	ADCSequenceStepConfigure(ADC_BASE, 0, 0, ADC_CTL_END);
	ADCSequenceEnable(ADC_BASE, 0);
}
*/


나머지는 엔코더예제에 덥어쓴거라 뭐 확인이 금방되실겁니다. 그런데... 출력해서 시리얼 통신으로 받아보니


이렇게 나오는 군요...이..이럴수가... 전 분명

[엔코더값], [자이로출력], [자이로온도];

이렇게 보내라고 했는데

[엔코더값], [자이로온도], [자이로출력];

이렇게 전송이 되는군요... 이유를... 모르겠네요... .ㅜ.ㅜ 도대체....
 물론 하나하나 받을때는 아주 잘 들어옵니다...ㅜ.ㅜ
(이게 첫 의문입니다. 잉~ 구세주 같은 분은 없나요?)

여하튼 순서가 좀 이상하지만, 뭐가 온도값이고 뭐가 자이로 값인지 아니까 보정해볼려고 했습니다. 그런데.... 잘 모르겠더라는... 음... 정확하게 어떻게 상수를 계산하고 그 계산된 상수는 또 어떻게 처리하는건지... 그럼 뭐 실험해야지요^^ 사실 데이터시트에 나와있던데. 오랜 밤샘등으로 머리도 아프고 모조리 5V에 대한 그래프던데 3.3V인 경우는 뭐 어떻게 달라지는지... 등등을 생각할려니 그냥 실험해서 일치시켜도 괜찮지 않을까? 하고 생각한것 뿐입니다. 역시 실험은 이전 자이로 실험처럼 흔들리는 진자에다가 고정시켜 엔코더값과 비교할겁니다.


전 위에서처럼 전송받은 자이로 출력에 온도에 대한 보정식을 생각했습니다. 저렇게 생각하고난다음 뒤에 곱해지는 0.08이라는 수치를 실험을 통해 찾은것입니다.


위 그림은 보정없는 자이로의 원 데이터를 적분한 것입니다. 대략 60초가 지났을때 5도의 드리프트가 확인됩니다.


아이고, 세로축이 잘못 표시되었네요. 전부 각도(degree)입니다. 위 두 그래프는 대략 70초까지는 가만히 나둬서 드리프트를 확인해봤고 그 후 진자를 흔들어서 그 기울어진 각도를 잘 따라가는지 엔코더의 출력(파랑색)과 확인한 것입니다. 이것이 온도보정을 나름대로(^^)한 것입니다. 응답속도는 이전과 비슷하고 그 자세한 그래프는 이전실험에서 보였으니 이번엔 그냥 저렇게 큼지막한 그래프로 드리프트만 확인하겠습니다. 괜찮다는 생각이 듭니다. 틈틈히 계속 실험해서 결과를 확인해봐야겠습니다.

(여기서 두번째 의문이 들지요. 자이로를 많이 다루시는 분들은 원래 온도에대한 보정이라는 것을 어떻게 하십니까? 저처럼 하면 안되는 건가요?^^)

반응형