File content as of revision 6:a9ab4e153009:

        #include "Motor.h"

/* Theory of operation of the tachometer reading system

    The interrupt is Tach, an InterruptIn object.

    To filter out the horrible transients, some debounce and stuff is being done. This function, tach_mark(), is called
    on a rising edge. It checks whether the risen edge is still there 10us later. (Using a blocking wait_us() call,
    which is bad practice, but not that bad considering how short 10 us is. I don't think it would be that difficult to instead have it \
    set up a 10us delay and nonblockingly interrupt-call a function using a Timeout.

    Anyway, if it is still there, it then records the read from TachTimer and resets the TachTimer, and adds the read value to
    the ring buffer and to the delta_t global.

    It *also* disables the Tach rising edge interrupt. (i.e disables itself) and enables a ~~falling~~ edge interrupt on Tach. This
    falling edge interupt then does mostly the same thing w/r/t filtering, and enables the rising edge after a confirmed falling edge.
    
*/
void addToRingBuf(int val, volatile int *ringBuf, volatile int *ringBufIdx)          //Is called in ISR. Adds stuff to ring buffer, obviously.
{
    ringBuf[*ringBufIdx] = val;
    *ringBufIdx =  (*ringBufIdx + 1) % RING_BUF_SZ; //avoid mod, do if
}


void tach_mark(volatile int *ringBuf,
               volatile int *ringBufIdx,
               InterruptIn *Tach, 
               Timer *TachTimer, 
               volatile bool *new_tach_flag, 
               void (*tach_antimark_handler)(void))      //PRIMARY ISR called on rising edge of encoder.
{
    int dt = TachTimer->read_us();
    
    wait_us(10);
    if(Tach->read()) {
        TachTimer->reset();
        *new_tach_flag = 1;
        addToRingBuf(dt, ringBuf, ringBufIdx);
        Tach->rise(NULL);
        Tach->fall(tach_antimark_handler);      // Reset after a falling edge.  //set to handler for antimark
    }
}


void tach_antimark(InterruptIn *Tach, void (*tach_mark_handler)(void))
{
    wait_us(10);                        // This can surely be done better with a Timeout. Medium priority. Highest is getting it in a
                                        //library/object and to work with two encoders at once.
    if(!Tach->read()) {
        Tach->fall(NULL);
        Tach->rise(tach_mark_handler);          //set to handler function for mark
    }
}


float get_speed(volatile bool *new_tach_flag, 
                Timer *TachTimer, 
                volatile int *ringBuf, 
                volatile int *ringBufIdx)
{
    if(new_tach_flag) {
        new_tach_flag = 0;
    }
    
    // if no pulse in 400ms, motor is probably stopped
    // 3% duty cycle is lowest the motor will run with, has <400ms period
    if(TachTimer->read_ms() > 400) {
        addToRingBuf(INT_MAX, ringBuf, ringBufIdx);
        TachTimer->reset();
    }

    int avg = 0;
    for (int i = 0; i < RING_BUF_SZ; i++) {
        if (ringBuf[i] == INT_MAX) {
            avg = INT_MAX;
            break;
        }
        avg += ringBuf[i];
    }

    if (avg == INT_MAX) {
        return 0;
    } else {
        avg = avg / RING_BUF_SZ;
        if (avg > 400000) { //400 ms
            return 0;
        } else {
            return WHEEL_CIRCUM / ((2.0 * (float)avg) / 1000.0);
        }
    }
}

void speed_control(volatile float target_spd, 
                   float actual_spd, 
                   float *iState, 
                   float iLimit, 
                   volatile float ki, 
                   volatile float kp, 
                   PwmOut *motor,
                   DigitalOut *brake)
{
    
    float delta_spd = target_spd - actual_spd;
    
    //if delta_spd large, brake this cycle.
    if(delta_spd < -0.35){
        motor->write(0.0);
        *brake = 1;
        return;
    } else{
        *brake = 0;
    }
    
    (*iState) += delta_spd;
    if ((*iState)*ki > iLimit) {
        (*iState) = iLimit;
    } else if ((*iState)*ki < -iLimit) {
        (*iState) = -iLimit;
    }
    
    // PI controller!
    motor->write(kp * delta_spd + ki * (*iState));    
}