File content as of revision 0:8bded3c460c7:

/*
Simple PID-based Motor RPM Controller
Purpose:    This code reads motor RPM with encoder, 
            generate ESC-compatible control signal with PID controller
            
Note:   LED1: System Status Indicator, Blink = Main Loop is Running
        LED2: Controller Timer Indicator, Blink = Controller is Updating        
        ESC is connected to Pin 26
        Encoder is connected to Pin29,Pin30, and Pin27 if has index channel
        PID gains:  Kp, Ki, Kd

Caution: This is a simple test code, no guarantee to performance or robustness 
      
Author: Qiuyang Tao
*/
 
#include "mbed.h"
#include "Servo.h"
#include "QEI.h"
#define PULSE_PER_REV 200
  
Ticker ctrl_updt_timer;     //Timer
Servo motor_ESC(p26);       //ESC is connected to Pin 26
Serial pc(USBTX, USBRX);    //debug serial
QEI wheel (p29, p30, NC, PULSE_PER_REV);  // <500,000 pulses per second//Use X4 encoding.
//QEI wheel (p29, p30, p27, PULSE_PER_REV); // If encoder has index channel 

//All LEDs Blinks at start up, indicates mbed is setting up ESC
DigitalOut led1(LED1); //System Status Indicator, Blink = Main Loop is Running
DigitalOut led2(LED2); //Controller Status Indicator, Blink = Controller is Updating  
DigitalOut led3(LED3); //Controller Status Indicator, On = Motor RPM < Desired RPM
//                                                         Motor is Aaccelerating
//                                                   Off = Motor RPM > Desired RPM
//                                                         Motor is De-Aaccelerating
DigitalOut led4(LED4); //Encoder Counter Overflow


//PID Related Variables - User 
float setpoint_rpm=10000;   //Desired RPM = 10,000
float dt=0.02; //Update Controller at 50Hz
float Kp=0;
float Ki=0;
float Kd=0;


//PID Related Variables 
float error_rpm=0;
float prev_error_rpm=0;
float u=0;  //controller output, range = [0,1]
float u_p=0;
float u_i=0;
float u_d=0;

//Global Variables - User
int use_index_channel=0;

//use Unsigned long long if overflows frequently
//unsigned long long motor_revolutions = 0;
int motor_revolutions=0; //number of revolutions recorded by the encoder on the index channel
int prev_motor_revolutions=0; //number of revolutions recorded by the encoder on the index channel
int motor_pulses=0;      //Number of pulses recorded by the encoder
int prev_motor_pulses=0;      //Number of pulses recorded by the encoder
float motor_RPM=0;         //Motor angular velocity


void controller_update_ISR() {
    // get motor RPM
    if (use_index_channel) {
        motor_revolutions=wheel.getRevolutions();
        motor_RPM=(motor_revolutions-prev_motor_revolutions)/dt;
        prev_motor_revolutions=motor_revolutions;
        //reset encoder counter if overflows
        if(motor_revolutions>2147483647-setpoint_rpm*dt*10){
                           //18446744073709551615; for Unsigned long long
            wheel.reset();    
            prev_motor_revolutions=0;
            led4 = !led4;
        }
        
    }else{
        motor_pulses=wheel.getPulses();     
        motor_RPM=(motor_pulses-prev_motor_pulses)/PULSE_PER_REV/dt;
        prev_motor_pulses=motor_pulses;
        //reset encoder counter if overflows
        if(motor_pulses>2147483647-setpoint_rpm*dt*PULSE_PER_REV*10){
                           //18446744073709551615; for Unsigned long long
            wheel.reset();    
            prev_motor_pulses=0;
            led4 = !led4;
        }
    }
    
    //PID controller
    error_rpm=setpoint_rpm-motor_RPM;   //e=r-u;
    if (error_rpm>0){led3=1;} else {led3=0;} //Update Motor Status
    u_p=Kp*error_rpm;  
    u_i=u_i+Ki*dt*error_rpm;
    u_d=Kd*(error_rpm-prev_error_rpm)/dt;
    u= u_p + u_i + u_d;
    prev_error_rpm=error_rpm;
    
    //Controller Saturation
    if (u>1){u=1;}
    if (u<0){u=0;}
    
    //Output ESC signal
    motor_ESC = u;
    
    //Update LED2 status, tell user PID is updating
    led2 = !led2;   
    
    //Watch dog can be added here in the future
}
 
int main() {
    //pc.printf("Debug mode: \n");
    led1 = 0;   //init LED1, Main Loop status
    led2 = 0;   //init LED2, PID status
    led3 = 0;   //init LED3, Motor status
    led4 = 0;   //init LED4, Encoder Counter status
    wait(0.5);
    
    //All LEDs Blinks at start up, indicates mbed is setting up ESC
    motor_ESC = 0; //Generate zero speed signal for ESC startup
    led1 = 1;led2 = 1;led3 = 1;led1 = 1;
    wait(0.5);
    led1 = 0;led2 = 0;led3 = 0;led1 = 0;
    wait(0.5);
    led1 = 1;led2 = 1;led3 = 1;led1 = 1;
    wait(0.5);
    led1 = 0;led2 = 0;led3 = 0;led1 = 0;
    wait(0.5);
    
    
    ctrl_updt_timer.attach(&controller_update_ISR, dt); //Update Controller at (1/dt) Hz
    
    while(1) {  //blink LED1
        led1 = !led1;
        wait(0.2);
    }
}