sensortask.c

来自「AVR单片机做的两轮自平衡机器人」· C语言 代码 · 共 82 行

/*
 SensorTask.c

 Manages the inputs for the various sensors on our robot.
    Gyro - integrates for angle
    Tilt - used to adjust the angle offset when angular velocity is low
    Encoders - left and right wheel positions.

    The encoders are the highest speed event and need to be read on a 6khz cycle
        2 rps * 60.5 * 24 cpr = ~3000 cps
    or could be read by the interrupt handler version at ~721 cpr

Author: Larry Barello
        [e-mail]
*/
#include <avr/io.h>
#include <avr/eeprom.h>
#include "avrx.h"
#include <avr/interrupt.h>
#include "hardware.h"
#include "adc_driver.h"
#include "SensorTask.h"
#include "Gyro_eeprom.h"
#include "MotorTask.h"

//+----------------------------------------------------
// TASK!
//
// There are 8.516 ticks/deg/sec, multiply by loop rate to get conversion
// from lAngle to known units.  I.e. 8.516 * 125 = 1064.5 counts/deg
// This task provides 1/10'th degree, so I divide by 2*106.5 or 213.
//
// Dynamic range is +/- 75 deg/sec for a .25 to 4.75 volt output which
// corresponds to a 921 count output from our 10bit ADC.  So, full scale,
// we get 972 for +75 deg/sec rotation or 6.14 counts/deg/sec.  Our
// conversion factor should be the integral / sample rate * 6.14  or, for
// below, 75.75
//-


long lAngle;     // Integrated angle
int iTenthsDegree;

TimerControlBlock SensorTimer;

AVRX_GCC_TASKDEF(SensorTask, 6, 1)
{
    do
    {
        AvrXDelay(&SensorTimer, MILLISECOND(500));
    }
    while (ValidateEEPROM());

    while(1)
    {
        AvrXStartTimer(&SensorTimer, MILLISECOND(8));   // 125 hz (50hz BW)

        // lAngle = 28.4 signed fraction

        lAngle += (long)((int)eeprom_read_word((unsigned)&eeiGyroOffset));
        lAngle -= FRAC(GetADC(GYRO));

        iTenthsDegree = lAngle / eeprom_read_word((unsigned)&eeiAngleGain);

        AvrXWaitTimer(&SensorTimer);
    }
}

void SetAngle(int iTenths)
{
    long temp = iTenths * eeprom_read_word((unsigned)&eeiAngleGain);
    cli();
    lAngle = temp;
    sei();
}

void AdjustAngle(int iTenths)
{
    AddToLong(&lAngle, iTenths * eeprom_read_word((unsigned)&eeiAngleGain));
}