File content as of revision 2:916e20f507f2:

 
/***************************< File comment >***************************/  
/* project:   Lamborghini48                                           */ 
/* project description : mobile robot class                           */ 
/*--------------------------------------------------------------------*/ 
/* version : 0.1                                                      */ 
/* board : NUCLEO-F411RE                                              */ 
/* detail : DC motor control                                          */
/*--------------------------------------------------------------------*/ 
/* create : 26/10/2018                                                */ 
/* programmer : Worasuchad Haomachai                                  */ 
/**********************************************************************/ 
 
/*--------------------------------------------------------------------*/ 
/*                           Include file                             */ 
/*--------------------------------------------------------------------*/
#include "math.h"
#include "QEI.h"                        // https://os.mbed.com/cookbook/QEI

Serial pc(USBTX, USBRX);

//Use X4 encoding.
//QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
//Use X2 encoding by default.
QEI wheel (D2, D3, NC, 200, QEI::X4_ENCODING);

/*--------------------------------------------------------------------*/ 
/*                         Global variable                            */ 
/*--------------------------------------------------------------------*/
PwmOut pwm(D5);                         // this is the PWM pin for the motor for how much we move it to correct for its error
DigitalOut dir1(D6);                    //these pins are to control the direction of the motor (clockwise/counter-clockwise)
DigitalOut dir2(D7);

double angle = 0;                       //set the angles

double setpoint = 360;                  // 3072;        // I am setting it to move through 100 degrees
double Kp = 0.05;                       // 0.015;       // you can set these constants however you like depending on trial & error
double Ki = 0.001;                        // 0.000071;    // 0.000109;
double Kd = 0.0;                        // 0.93866;     // 0.1009;

float last_error = 0;
float _error = 0;
float changeError = 0;
float totalError = 0;
float pidTerm = 0;
float pidTerm_scaled = 0;               // if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|

/*--------------------------------------------------------------------*/ 
/*                           prototype fun                            */ 
/*--------------------------------------------------------------------*/
void PIDcalculation();              // find PID value

/*--------------------------------------------------------------------*/ 
/*                              main                                  */ 
/*--------------------------------------------------------------------*/   
int main() {
    pc.baud(115200);
    while(1)
    {
        PIDcalculation();               // find PID value
        
        if (angle < setpoint) 
        {
            dir1 = 1;                   // Forward motion
            dir2 = 0;
        } 
        else 
        {
            dir1 = 0;                   // Reverse motion
            dir2 = 1;
        }
    
        pwm.write(pidTerm_scaled / 255.00f);
        
        // Serial.println("WHEEL ANGLE:");
        pc.printf("%.3f\t\t", setpoint);
        pc.printf("%.3f\n\r", angle);
        //pc.printf("%d\n\r", wheel.getPulses());
    }

}
/*--------------------------------------------------------------------*/ 
/*                              PID                                   */ 
/*--------------------------------------------------------------------*/ 
void PIDcalculation(){
    /* 
    *  angle = ( 0.1171875 * count); // count to angle conversion = 360/(64*12*4)
    *  reduction ratio: 64:1 , pulses per revolution: 12CPR
    */
    
    angle = ( 0.004591 * wheel.getPulses());        // 360/(98*200*4)
    _error = setpoint - angle;
    
    changeError = _error - last_error;              //derivative term
    totalError += _error;                           //accumalate errors to find integral term
    pidTerm = (Kp * _error) + (Ki * totalError) + (Kd * changeError);//total gain
    
    // pidTerm = constrain(pidTerm, -255, 255);     //constraining to appropriate value
    if(pidTerm > 255)
    {
      pidTerm = 255;
    }
    else if(pidTerm < -255)
    {
      pidTerm = -255;  
    }
    
    pidTerm_scaled = abs(pidTerm);                  //make sure it's a positive value
    
    last_error = _error;
}