#ifndef CHEADER_MOTOR
# define CHEADER_MOTOR
typedef struct
{
    volatile long    Setpoint;           // 24.8 number
    volatile long    Encoder;            // Current position
    //char  Kp;
    //char  Kd;
    //int8_t  Td;
    //char  Ki;
    //char  Ko;                 // Output Scale
    volatile int PrevErr;
    volatile int Ierror;
    volatile int Velocity;
    volatile int VelocitySetpoint;
    volatile int Acceleration;
    volatile unsigned char pwm_out;
} MotorInfo, * pMotorInfo;

//signed char Kp;
//signed char Kd;
//signed char Ki;
//signed char Ko;

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

void MotorInit(void);
void AddToPosition(pMotorInfo, int);
void DoMotion(MotorInfo *p);
void DoPid(MotorInfo *p);
void motors(BehaviourInfo *pInfo);
inline void SynchWithMotion(void);
inline void EmergencyStop(void);
void AddToLong(volatile long *p, signed char o);
inline void QuadratureDecoder(void);
void WaitForMotionToStop(void);

#endif //CHEADER_MOTOR