File content as of revision 0:929eee0b13f2:

#include "mbed.h"
#include "PID.h"
#include "QEI.h"

/*
    Demo of setting motor speed using encoder in pid feedback. 
    
    Parts used for this demo:
    HN-GH12-1634T Gear Motor
    LMD18200 H-Bridge Breakout Board
    E4P-100-079 Quadrature Encoder
    
    TODO: Implement a "rolling"/"moving" average in callback 
    for speed feedback
*/

float clip(float value, float lower, float upper);

static float setpoint, feedback, output;
const float output_lower_limit = -1.0;          
const float output_upper_limit = 1.0;
const float FEEDBACK_SCALE = 1.0/3000.0;        // Scale feedback to 1rev/3000cnts

const float kp = 0.01;
const float ki = 0.01;
const float kd = 0.0;
const float Ts = 0.04;                          // 25Hz Sample Freq (40ms Sample Time);
                                                // Used for PID Sample time and used
                                                // to calculate callback() interrupt
                                                // time                                                

PID pid(&setpoint, &feedback, &output, output_lower_limit, output_upper_limit,
        kp, ki,  kd, Ts);
QEI encoder(p15, p16);
PwmOut mtr_pwm(p25);
DigitalOut mtr_dir(p24);
void pid_callback();                               // Updates encoder feedback and motor output
Ticker motor;

int main() {
    // Wait for me to plug in motor battery
    wait(5);
    
    // Clear encoder count
    encoder.reset();
    
    // Init the motor
    mtr_dir = 0;            // CW
    mtr_pwm = 0.0;   // Quarter speed
    
    // Update sensors and feedback twice as fast as PID sample time;
    // this makes pid react in real-time avoiding errors due to 
    // missing counts etc. 
    motor.attach(&pid_callback, Ts/2.0);
    
    // Init the pid
    /*TODO: Implement PID Change Param Method in the PID class
    and use it to init gains here*/
    setpoint = 0.0;
    feedback = encoder.read();
    output = 0.0;
    pid.start();

    while(1){
        int flag;
        float userInput;
        do{ 
            printf("Enter Speed/RPM (-100.0 to 100.0)\r\n");
            flag = scanf("%f", &userInput);
        }while(flag == EOF);
        setpoint = userInput;
        do{   
            printf("Setpoint: %1.2f\t\tFeedback: %1.2f\t\tError: %1.2f\t\tOuput: %1.2f\r\n", 
                    setpoint, feedback, pid.getError(), output);
            wait(0.25);
        }while(pid.getError() < -0.006 || 0.006 < pid.getError());
        printf("Speed Reached!\r\n");
        printf("Setpoint: %1.2f\t\tFeedback: %1.2f\t\tError: %1.2f\t\tOuput: %1.2f\r\n", 
                setpoint, feedback, pid.getError(), output);
    }
}

/*
    Updates feedback and output, interrupt driven so that paramaters
    are updated in real-time, i.e. avoids update lag due to main
    code overhead and printfs which can be slow.
*/
void pid_callback(){
    // Update motor
    if(setpoint >= 0.0) mtr_dir = 1;       // Set motor direction based on setpoint
    else mtr_dir = 0;
    if(-0.001 < setpoint && setpoint < 0.001){ 
        /* Setpoint = 0 is a special case, we allow output to control speed AND 
        direction to fight intertia and/or downhill roll. */
        if(output >= 0.0) mtr_dir = 1;
        else mtr_dir = 0;
        mtr_pwm = abs(output);
    }
    else{    
        if(mtr_dir == 1){                      // If CW then apply positive outputs
            if(output >= 0.0) mtr_pwm = output;
            else mtr_pwm = 0.0;
        }
        else{                                // If CCW then apply negative outputs
            if(output <= 0.0) mtr_pwm = abs(output);
            else mtr_pwm = 0.0;
        }                          
    } 
//    if(mtr_dir == 1){                     // If CW then apply positive outputs
//        if(output >= 0.0) mtr_pwm = output;
//        else mtr_pwm = mtr_pwm.read() - abs(output); // Take negative output value out of full range
//                                          // helps avoid bumpiness in motor
//    }
//    else{                                // If CCW then apply negative outputs
//        if(output <= 0.0) mtr_pwm = abs(output);
//        else mtr_pwm = mtr_pwm.read() - abs(output);
//    }                          
//        
    // Running average
    float k = Ts/2.0;                        // Discrete time, (Ts/2 because this callback is called
                                             // at interval of Ts/2... or twice as fast as pid controller)
    
    /* TODO: Implement a "rolling"/"moving" average */
    static int last_count = 0;
    int count = encoder.read();
    float raw_speed = ((count - last_count)*FEEDBACK_SCALE) / k; 
    float rpm_speed = raw_speed * 60.0;     // Convert speed to RPM
    
    last_count = count;                     // Save last count
    feedback = rpm_speed;              
}

/*
    Clips value to lower/ uppper
    @param value    The value to clip
    @param lower    The mininum allowable value
    @param upper    The maximum allowable value
    @return         The resulting clipped value
*/
float clip(float value, float lower, float upper){
    return std::max(lower, std::min(value, upper));
}