motortask.c

AVR单片机做的两轮自平衡机器人

#include <avr/io.h>
#include <avrx-io.h>
#include <math.h>
#include <avr/eeprom.h>
#include "avrx.h"
#include "hardware.h"
#include "MotorTask.h"
#include "Gyro_eeprom.h"
#include "SensorTask.h"

/*
    Globals/Statics etc.

    3/21/01 - Changed all references to Odometry and motor info to pointers.  GCC handles
    pointers much better than statics.  Much smaller and faster code results.
*/

MotorInfo Left, Right;
int iX, iY, iTheta;
#ifdef ODOMETRY
float X, Y, Theta;
#endif

TimerControlBlock MotorTimer;
Mutex MotorLoop;
int VelocitySp,
    velocity;


extern signed char RightEncoder, LeftEncoder;


void AddToLong(long *p, int o)  // Used by AddToPosition & AddToSetpoint macros
{
    char s = inp(SREG);
    BeginCritical();
    *p += o;
    outp(s, SREG);
}

AVRX_GCC_TASKDEF(MotorTask, 20, 2)
{
    signed char LeftEnc, RightEnc;

    int dAngleError = 0,
        iAngleError = 0,
        dVelocityError = 0,
        iVelocityError = 0,
        VelocityError,
        output;


//    MotorInit();
    while (ValidateEEPROM())
    {
        AvrXDelay(&MotorTimer, 10);
    }

    while(1)
    {
        AvrXStartTimer(&MotorTimer, MILLISECOND(24));   // About 40hz

        // Read encoder information and calculate overall velocity

        BeginCritical();
        LeftEnc = -LeftEncoder;
        LeftEncoder = 0;
        RightEnc = RightEncoder;
        RightEncoder = 0;
        EndCritical();

        AddToLong(&Left.Encoder, LeftEnc);
        AddToLong(&Right.Encoder, RightEnc);

        // LeftEnc = Left Wheel Velocity, RightEnc = Right Wheel Velocity
        // velocity = average robot velocity

        velocity = (LeftEnc + RightEnc)/2;


#ifdef ODOMETRY
        // Do the odometery calculations
        {
        float DeltaTheta = LeftEnc*(LEFT_DIST_PER_TICK/WHEEL_BASE*2)
                         - RightEnc*(RIGHT_DIST_PER_TICK/WHEEL_BASE*2);
        float dist = (LeftEnc+RightEnc)
                   * ((LEFT_DIST_PER_TICK + RIGHT_DIST_PER_TICK)/4.0);

        Theta += DeltaTheta;
        X -= dist * cos(Theta);
        Y -= dist * sin(Theta);
        Theta += DeltaTheta;

        if (Theta >= 2.0*M_PI)
            Theta -= 2.0*M_PI;       // Keep theta between 0-360
        else if (Theta < 0.0)
            Theta += 2.0*M_PI;

        // Keep integer versions around for user interface & Maze running

        iTheta = Theta * (3600 / (2*M_PI));  // .1 deg resolution.
        adjust(&X, &iX);
        adjust(&Y, &iY);
        }
#endif

        // Generate some terms

        dAngleError = iTenthsDegree - dAngleError;
        VelocityError = VelocitySp - velocity;
        dVelocityError = VelocityError - dVelocityError;

        //Generate Balance torque.

        output = iTenthsDegree * eeprom_read_word(&eeKangle) +
                 dAngleError * eeprom_read_word(&eeKrate) +
                 iAngleError * eeprom_read_word(&eeKiangle);

        // Generate Velocity Torque (essentially inverse)

        output -= VelocityError * eeprom_read_word(&eeKp) +
                 dVelocityError * eeprom_read_word(&eeKd) +
                 iVelocityError * eeprom_read_word(&eeKi);

        // Convert to whole number & Adjust output for motor friction

        output /= 16;
        output = DoMotorDriveOffset(output);    // Account for friction in drive motors

        // Update our differences values

        dAngleError = iTenthsDegree;
        dVelocityError = VelocityError;     // Update for next cycle

        // Accumulate Integral error *or* Limit output.
        // Stop accumulating when output saturates

        if (output >= MAXOUTPUT)
            output = MAXOUTPUT;
        else if (output <= -MAXOUTPUT)
            output = -MAXOUTPUT;
        else
        {
            iAngleError += iTenthsDegree;
            iVelocityError += VelocityError;
        }
        if (-250 > iTenthsDegree || iTenthsDegree > 250)   // Kill drive if tilted too much.
            output = 0;

        SetLeftPWMDrive(output);
        SetRightPWMDrive(output);

        AvrXWaitTimer(&MotorTimer);
        AvrXSetSemaphore(&MotorLoop);   // Synchronize other motion tasks
    }
}

// Account for friction in drive motors

short DoMotorDriveOffset(short drive)
{
    char offset = eeprom_read_word(&eeDriveOffset);
    if (drive > 0)
        drive += offset;
    else if (drive < 0)
        drive -= offset;
    return drive;
}

/*+ ---------------------------------------------------------
 void DoMotion(MotorInfo *p)

 Called at the loop rate to add "velocity" to the setpoint thus
 effecting a motion.

 Velocity is ramped up and down by "acceleration"

 Velocity and Acceleration are 8.8 numbers.  Velocity
 is added to the setpoint (an 24.8 number)
-*/

void DoMotion(MotorInfo *p)
{
    if (p->Velocity < p->VelocitySetpoint)
    {
        p->Velocity += p->Acceleration;
        if (p->Velocity > p->VelocitySetpoint)
            p->Velocity = p->VelocitySetpoint;
    }
    else
    {
        p->Velocity -= p->Acceleration;
        if (p->Velocity < p->VelocitySetpoint)
            p->Velocity = p->VelocitySetpoint;
    }
    AddToLong(&p->Setpoint, p->Velocity);
}

#ifdef ODOMETRY
/*
 Keep floating point position (X & Y) less than .1" so that floating point
 errors don't accumulate.

 iX and iY are the integer locations in .1" (+/- 32k, or ~ +/- 270')
 */

void adjust(float *Val, int * iVal)
{
    if ((-.1) > *Val)
    {
        *Val += .1;
        *iVal -= 1;
    }
    else if (*Val >= .1)
    {
        *Val -= .1;
        *iVal += 1;
    }
}

#endif


void SetLeftPWMDrive(short drive)
{
    if (drive > 0)
    {
        LEFT_FORWARD();
    }
    else
    {
        LEFT_REVERSE();
        drive = -drive;
    }
    outp(drive, LEFT_DRIVEL);
}

void SetRightPWMDrive(short drive)
{
    if (drive > 0)
    {
        RIGHT_FORWARD();
    }
    else
    {
        RIGHT_REVERSE();
        drive = -drive;
    }
    outp(drive, RIGHT_DRIVEL);
}
// Initialize Motion Control data BUG BUG BUG This isn't being used and won't work.

void MotorInit(void)
{
    Left.Kp = Right.Kp = eeprom_read_byte(&eeKp);
    Left.Kd = Right.Kd = eeprom_read_byte(&eeKd);
    Left.Ki = Right.Ki = eeprom_read_byte(&eeKi);
    Left.Acceleration = Right.Acceleration = eeprom_read_byte(&eeAcceleration);
}