File content as of revision 3:0d98aaab6345:

/*
  ECE 4180 Final Project
  Yatong Bai
  
  Functionalities:
  Reads target speed from the serial port;
  Uses an encoder with 11 ticks to calculate heading and angular velocity;
  Uses a PID control algorithm to make the motor spin at the target speed;
  Estimate the heading between two encoder ticks;
  Lights up LED1 when heading is ~0 degrees
*/

#include "mbed.h"

#define tick 32.727272727272727 // 360 deg / 11 ticks
#define pi 3.14159265358979323846264338

InterruptIn encoder (p22);
PwmOut motor (p21);
Serial pc (USBTX, USBRX);
DigitalOut led(LED1); // LED1 blinks when heading is ~0 deg
DigitalOut blinker(LED2); // LED2 toggles when the interrupt is run
Timer t; // Used to calculate the time interval between two rising edges
Ticker ticker; // For timer interrupt

volatile double rev = 0, old_rev = 0, raw_rev = 0, print_rev = 0;
volatile double elapsed_time = 99999999999, cur_elapsed_time = 0;
volatile double angle = 0, raw_angle = 0, target = 0;
volatile double cur_angle = 0, print_angle = 0, num = 0, err = 0;

volatile float p = 0, i = 0, d = 0, spd = 0, up_lim = 255;
char buf[5];

void PID(){ // Helper function that performs PID control
    err = target-rev;
    p = err*(110*target+500) + target*5 + 40; // P term
    double temp = err*elapsed_time*(target*330+110); // I term increment
    if (temp >= 20) // Prevent abnormal data upsetting the I term
        i += 20;
    else if (temp <= -20)
        i -= 20;
    else
        i += temp;
    if (i < -65) // I term should generally be positive
        i = -65;
    else if (i > 110+target*70) // Limit max I to prevent wind-up during acceleration
        i = 110 + target * 70;
    d = (old_rev-rev)/elapsed_time*7; // D term
    spd = p + i + d; // PID controller
  
    if (target == 0) // Set speed to 0 when target speed = 0
        spd = 0;
    
    else{
         // Limit the PWM pulse width to prevent large current at low speed
        up_lim = target * 25 + 160;
    
        if (spd < 0)
            spd = 0;
        else{
            if (spd > up_lim)
                spd = up_lim;
            if (spd > 255)
                spd = 255;
        }
    }
    
    motor = spd / 255.0; // Set PWM output
}

void control() { // Interrupt function called when encoder generates a rising edge
    t.stop();
    elapsed_time = t.read(); // Calculate the time interval between two rising edges
    t.reset(); // Reset timer
    t.start(); // Start timer again
    
    blinker = !blinker; // Toggle LED2
  
    if (elapsed_time == 0) // Prevent dividing by 0 when this function is called for the first time
        raw_rev = 7;
    else
        raw_rev = tick/360.0/elapsed_time; // Calculate angular velocity
    rev = raw_rev;
  
    // Discard unrealistic rev readings caused by rising edges that are too close (debouncing)
    if (rev - 1.5*print_rev > 1.3 || rev > 6)
        rev = print_rev;
    else{
        old_rev = rev; // old_rev will be used for the D term of PID
        raw_angle += tick; // Increment the angle
        // Estimate the current angle based on angular velocity and tick increment
        angle = (raw_angle + 2.0*cur_angle) / 3.0; 
    }
    if (rev < 6.5) // Apply a low-pass filter on the angular velocity reading for display
        print_rev = (print_rev * 29.0 + rev) / 30.0;
  
    PID(); // Call the PID helper function to perform control
}

void output(){ // A timer interrupt function that sends the data to the PC
    pc.printf("Heading: %3.0f deg, Speed: %3.1f revs/sec\n", print_angle, print_rev);
}

int main() { // Main function
    pc.baud(38400); // Jack up PC baud rate
    motor.period_us(800); // Set PWM frequency to 1250Hz
    t.reset(); // Reset and start the timers
    t.start();
    encoder.rise(&control); // Attach the I/O interrupt function
    ticker.attach(&output, 0.08); // Attach the timer interrupt function
  
    while (true){
        if (pc.readable()){
            pc.scanf("%s", &buf); // Read data from PC
            num = atof(buf); // Convert string to double
            if (num > 6){ // A number > 6 is a signal for resetting the heading to 0
                angle -= cur_angle;
                raw_angle -= cur_angle;
                cur_angle = 0;
                elapsed_time = 99999999999;
                rev = 0;
            }
            else{ // Otherwise the number is the target speed
                target = num; // Set target speed
                motor = (target*40.0+50.0) / 255.0; // Set PWM pulse width to ease startup
                i = 0; // Clear the I term
            }
        }

        // Since the encoder has poor accuracy, the heading needs to be estimated between two edges
        // Keep reading from the timer to calculate the elapsed time since last rising edge
        // Calculate elapsed angle since last rising edge using the angular velocity
        cur_elapsed_time = t.read();
        cur_angle = angle + print_rev*360*cur_elapsed_time; // Estimate current angle
        if (cur_angle > angle + tick) // The estimated angle cannot exceed 1 tick
            cur_angle = angle + tick;
        if (cur_angle > 360.){ // Reset angle when the next turn begins
            raw_angle -= 360.;
            angle -= 360.;
            cur_angle -= 360.;
        }
        
        // If the interval between two edges is too long (longer than the previous interval)
        // Then it means the speed has reduced. Need to re-calculate the speed.
        // This is especially during start-up. If stuck, the PWM pulse width will continuously increase
        if ((target<5 && cur_elapsed_time>elapsed_time) || cur_elapsed_time>1){
            rev = tick/360.0/cur_elapsed_time;
            if (rev - 1.2*print_rev > 0.2 || rev > 5) // Discard unrealistic revs
                rev = print_rev;
            PID(); // Perform PID control
        }
    
        if (rev < 6.5){ // Apply low-pass filter on rev
            print_rev = (print_rev * 349. + rev) / 350.;
            print_angle = cur_angle;
        }
        
        // Light up the LED when heading is ~0 deg
        if (cur_angle < 10 || cur_angle > 350)
            led = 1;
        else
            led = 0;
    }
}