File content as of revision 0:242dc469ac2b:

#include "mbed.h"
#include "Nucleo_Encoder_16_bits.h"

#define PI  3.1415926535898

Nucleo_Encoder_16_bits  _Lencoder   (TIM4);
Nucleo_Encoder_16_bits  _Rencoder   (TIM3);
PwmOut                  _Lpwm       (PA_9);
PwmOut                  _Rpwm       (PA_8);
DigitalOut              _LdirA      (PC_9);
DigitalOut              _LdirB      (PC_8);
DigitalOut              _RdirA      (PC_6);
DigitalOut              _RdirB      (PC_5);

Serial      pc          (PA_2, PA_3, 921600);                                   // Create a serial link to PC for communication

DigitalOut              led1        (PA_5);                                     // Added Led1 for test purpose
DigitalOut              led2        (PD_2);                                     // Added Led2 for test purpose
DigitalOut              disquette   (PA_12);                                    // Added baloon destructor command (without it, you might see baloon destructor motor be set to full speed)

Ticker      _tick;

void controlLoop(void);

/**
 * Set the Kp value
 *
 * @param  Kp (float) : value of Kp
 * @return The value of Kp (float)
 * @note  Can also be accessed using the global variable _Kp
 */
float setProportionnalValue (float KpValue);

/**
 * Set the Ki value
 *
 * @param  Ki (float) : value of Ki
 * @return The value of Ki (float)
 * @note  Can also be accessed using the global variable _Ki
 */
float setintegralValue (float KiValue);

/**
 * Set the Kd value
 *
 * @param  Kd (float) : value of Kd
 * @return The value of Kd (float)
 * @note  Can also be accessed using the global variable _Kd
 */
float setDerivativeValue (float KdValue);

/**
 * Set the Set point value of the speed for integrated full bridge
 *
 * @param left  (double) : Set point value for left  wheel speed (in mm/s)
 * @param right (double) : Set point value for right wheel speed (in mm/s)
 */
void setSpeed (double left, double right);

/**
 * Get position of the robot (in mm for X and Y and radian for Theta)
 *
 * @param x (double - passed by reference) : actual position of the center of the robot, in mm, along X axis (front of the robot at startup)
 * @param y (double - passed by reference) : actual position of the center of the robot, in mm, along Y axis (left of the robot at startup)
 * @param theta (double - passed by reference) : radian angle between the normal to the line passing through the 2 wheels and the angle of the robot at startup
 * @note the position is updated each time a motion computation take place (ie : each milliseconds)
 */
void getPosition (double *x, double *y, double *theta);

/**
 * Reset position of the robot (in mm for X and Y and radian for Theta)
 *
 * @note the positionis equal to 0 on all 3 axis (cannot be undone)
 */
void resetPosition (void);

/**
 * Get speed of the two wheels of the robot
 *
 * @param vG (double - passed by reference) : actual speed in mm/s of the left wheel
 * @param vD (double - passed by reference) : actual speed in mm/s of the right wheel
 */
void getSpeed (double *vG, double *vD);

/**
 * Global Variable of corrective values
 * @note  works well with Kp = 2.0, Ki = 0.4 and Kd = 1
 */
double                  _Kp, _Ki, _Kd;
/**
 * Global Variable of speed
 */
double                  _SpeedG, _SpeedD;
/**
 * Global Variable of measured speed
 */
double                  _measSpeedG, _measSpeedD;
/**
 * Global Variable of measured displacement
 */
double                  _measDistG, _measDistD;
/**
 * Global Variable to indicate that required wheel speed is unreachable (set if speed is unreachable)
 * @note    Must be cleared by user
 */
int                     _WheelStuckG, _WheelStuckD;
/**
 * Global Variable of wheel PWM value
 */
double                  _PwmValueG, _PwmValueD;
/**
 * Global Variable of gravity center of robot position (odometric, error increase with time)
 */
double                  _X, _Y, _THETA;

/**
 * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable)
 * @note    Must be cleared by user
 */
int                     RobotIsStuck;

int main ()
{
    _tick.attach(&controlLoop, 0.001);

    _Lpwm.period_us(50);
    _Lpwm = 0;

    _Rpwm.period_us(50);
    _Rpwm = 0;

    _Kp = 2.0;
    _Ki = 0.4;
    _Kd = 1.0;

    pc.printf ("\n\rHelloWorld\n");
    led1 = 1;
    led2 = 0;
    disquette = 0;
    resetPosition();
    wait(3);

    setSpeed (80,80);
    while (1);
}

float setProportionnalValue (float KpValue)
{
    _Kp = KpValue;
    return _Kp;
}

float setintegralValue (float KiValue)
{
    _Ki = KiValue;
    return _Ki;
}

float setDerivativeValue (float KdValue)
{
    _Kd = KdValue;
    return _Kd;
}

void setSpeed (double left, double right)
{
    _SpeedG = left;
    _SpeedD = right;
}

void getSpeed (double *vG, double *vD)
{
    *vG = _measSpeedG;
    *vD = _measSpeedD;
}

void getPosition (double *x, double *y, double *theta)
{
    *x = _X;
    *y = _Y;
    *theta = _THETA;
}

void resetPosition (void)
{
    _X = 0;
    _Y = 0;
    _THETA = 0;
}

void controlLoop()
{
    long            PositionG, PositionD;
    static float    integralErrorG = 0, integralErrorD = 0;
    static float    oldErrorG, oldErrorD;
    double          errorG, commandeG;
    double          errorD, commandeD;
    double          meanDist, diffDist, deltaX, deltaY, deltaTheta;
    static long     oldPositionG=0, oldPositionD=0;

    PositionG =  _Lencoder.GetCounter();
    PositionD = -_Rencoder.GetCounter();

    // As we use mm/s for speed unit and we convert all mesure to this unit

    // Converting step to millimeters
    _measDistG = ((double)PositionG - (double)oldPositionG) / 63.662;
    _measDistD = ((double)PositionD - (double)oldPositionD) / 63.662;

    // Converting position to speed
    _measSpeedG = 1000.0 * _measDistG;
    _measSpeedD = 1000.0 * _measDistD;

    // Computing errors
    errorG = _SpeedG - _measSpeedG;
    errorD = _SpeedD - _measSpeedD;
    integralErrorG += errorG;
    integralErrorD += errorD;

    // Limiting integral error
    if (integralErrorG > 10000) {
        _WheelStuckG = 1;
        integralErrorG = 10000;
    } else {
        if (integralErrorG < -10000) {
            _WheelStuckG = 1;
            integralErrorG = -10000;
        } else {
            _WheelStuckG = 0;
        }
    }

    if (integralErrorD > 10000) {
        _WheelStuckD = 1;
        integralErrorD = 10000;
    } else {
        if (integralErrorD < -10000) {
            _WheelStuckD = 1;
            integralErrorD = -10000;
        } else {
            _WheelStuckD = 0;
        }
    }

    // Correcting system (empiric test tells me that Kp = 4, Ki = 1 and Kd = 2, but this is probably not the good setting)
    commandeG = _Kp * errorG + _Ki * integralErrorG + _Kd * (errorG - oldErrorG);
    commandeD = _Kp * errorD + _Ki * integralErrorD + _Kd * (errorD - oldErrorD);

    // Convert to PWM
    _PwmValueG = commandeG / 1300.0;
    if (_PwmValueG >  1) _PwmValueG =  1;
    if (_PwmValueG < -1) _PwmValueG = -1;

    if (_PwmValueG < 0) {
        _Lpwm  = -_PwmValueG;
        _LdirA = 0;
        _LdirB = 1;
    } else {
        _Lpwm = _PwmValueG;
        _LdirA = 1;
        _LdirB = 0;
    }

    _PwmValueD = commandeD/1300.0;
    if (_PwmValueD >  1) _PwmValueD =  1;
    if (_PwmValueD < -1) _PwmValueD = -1;

    if (_PwmValueD < 0) {
        _Rpwm  = -_PwmValueD;
        _RdirA = 1;
        _RdirB = 0;
    } else {
        _Rpwm = _PwmValueD;
        _RdirA = 0;
        _RdirB = 1;
    }

    //odometry (segments approx.)
    meanDist = (_measDistG + _measDistD) / 2.0;
    diffDist =  _measDistD - _measDistG;

    deltaX = meanDist * cos (_THETA);
    deltaY = meanDist * sin (_THETA);
    deltaTheta = diffDist / 230;

    _X += deltaX;
    _Y += deltaY;
    _THETA += deltaTheta;
    if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586;
    if (_THETA < -3.141592653589793) _THETA += 6.283185307179586;

    oldPositionG = PositionG;
    oldPositionD = PositionD;
    oldErrorG = errorG;
    oldErrorD = errorD;

    pc.printf ("\rG = %5.1lf - %5.1lf = %5.1lf -> %5.1lf -> %4.2lf\n", _SpeedG, _measDistG, errorG, commandeG, _PwmValueG);
    pc.printf ("\rD = %5.1lf - %5.1lf = %5.1lf -> %5.1lf -> %4.2lf\n", _SpeedD, _measDistD, errorD, commandeD, _PwmValueD);
}