motortask.h

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

#ifndef CHEADER_MOTOR
# define CHEADER_MOTOR
typedef struct
{
    long    Setpoint;           // 24.8 number
    long    Encoder;            // Current position
    char    Kp;
    char    Kd;
    char    Ki;
    int     PrevErr;
    int     Ierror;
    int     Velocity;
    int     VelocitySetpoint;
    int     Acceleration;
} MotorInfo, * pMotorInfo;


extern MotorInfo Left, Right;
extern float X, Y, Theta;
extern int iX, iY, iTheta;      // Integers in 1/10" increment
extern Mutex MotorLoop;
extern int velocity, VelocitySp;

void AddToLong(long *, int);
void SetRightPWMDrive(short);
void SetLeftPWMDrive(short);
short DoBalance(int);
short DoPid(MotorInfo *, short);
short DoMotorDriveOffset(short);
void MotorInit(void);
void DoMotion(pMotorInfo);
void adjust(float*, int*);



// Default values for balancing act.
// Note, all numbers are 12.4 signed fractions
#define KANGLE  FRAC(4.0)
#define KRATE   FRAC(4.0)
#define KIANGLE FRAC(0.0)      // .5 works well
#define KP      FRAC(0.0)
#define KD      FRAC(0.0)
#define KI      FRAC(0.5)
#define ACCELERATION 1
#define MAXOUTPUT 0xFF          // 8 bit PWM + Sign
#define DRIVEOFFSET 20          // Friction in drive motors

extern char BehaviorType;
extern int VelocitySp;

#endif //CHEADER_MOTOR