File content as of revision 1:fe23126b0389:

#include "mbed.h"
#include "QEI.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "PinDetect.h"
#include "main.h"
#include "biquadFilter.h"

Serial pc(USBTX, USBRX);

PinDetect stop(SW2);
PinDetect start(SW3);
AnalogIn pot1(m1_pot);
AnalogIn pot2(m2_pot);

// PWM Speed Control：
DigitalOut dir1(m1_dir);
PwmOut pwm1(m1_pwm);
DigitalOut dir2(m2_dir);
PwmOut pwm2(m2_pwm);

QEI enc1(m1_enc_a, m1_enc_b, NC, 1);
QEI enc2(m2_enc_a, m2_enc_b, NC, 1);

Ticker potTicker;
Ticker motorTicker;

Ticker graphTicker;
//HIDScope grapher(4);

float currentRotation1 = 0, currentRotation2 = 0;
float desiredRotation1 = 0, desiredRotation2 = 0;
double error1 = 0, error2 = 0;

double m1_error_integral = 0, m2_error_integral = 0;
double m1_error_derivative = 0, m2_error_derivative = 0;
biquadFilter m1_filter(m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2);
biquadFilter m2_filter(m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2);

bool shutup = true;
bool go_pot = false;
bool go_motor = false;
bool go_graph = false;

float getPotRad(AnalogIn pot)
{
    return pot.read() * 4.0f - 2.0f;
}

float toRadians(int pulses)
{
    int remaining = pulses;// % sigPerRev;
    float percent = (float) remaining / (float) sigPerRev;
    return percent * 2.0f;
}

void readPot()
{
    go_pot = true;
}

void getMotorRotation()
{
    go_motor = true;
}

void stopMotors()
{
    shutup = true;
}

void startMotors()
{
    shutup = false;
}

void sendGraph()
{
    go_graph = true;
}

double p_control(double error, double kp)
{
    return kp * error;
}

double pi_control(double error, double kp, double ki, double ts, double &error_integral)
{
    error_integral = error_integral + ts * error;
    double result =  kp * error + ki * error_integral;
    return result;
}

double pid_control(double error, double kp, double ki, double ts, double &error_integral, 
    double kd, double previous_error, double &error_derivative, biquadFilter filter)
{
    error_integral = error_integral + ts * error;
    error_derivative = (error - previous_error) / ts;
    // error_derivative = filter.step(error_derivative);
    
    double result = kp * error + ki * error_integral + kd * error_derivative;
    return result;
}

int getPDirection(double control, int motor)
{
    if (control >= 0)
        return (motor == 1)?1:0;
    else
        return (motor == 1)?0:1;
}

void initialize()
{
    pc.printf("Initializing...\r\n");

    // Set the shutup and start buttons
    stop.mode(PullUp);
    stop.attach_deasserted(&stopMotors);
    stop.setSampleFrequency();
    start.mode(PullUp);
    start.attach_deasserted(&startMotors);
    start.setSampleFrequency();
    
    pc.printf("Buttons done\r\n");

    // Set proper baudrate
    // pc.baud(115200);

    // Reset encoders
    enc1.reset();
    enc2.reset();
    pc.printf("Encoders reset\r\n");

    // Start tickers
    potTicker.attach(&readPot, tickRate);
    motorTicker.attach(&getMotorRotation, tickRate);
    graphTicker.attach(&sendGraph, graphTickRate);
    pc.printf("Tickers attached\r\n");

    pc.printf("Initialized\r\n");
}

int main()
{
    pc.printf("Started\r\n");
    initialize();

    while (true) {
        if (shutup) {
            pwm1 = 0;
            pwm2 = 0;
        } else {
            if (go_pot) {
                desiredRotation1 = getPotRad(pot1);
                desiredRotation2 = getPotRad(pot2);
                
                go_pot = false;
            }

            if (go_motor) {
                currentRotation1 = toRadians(enc1.getPulses());
                currentRotation2 = toRadians(enc2.getPulses());
                
                pc.printf("PULSE: %i     | CUR ROT: %f         | ERR: %f        | DES ROT: %f\r\n",enc2.getPulses(),currentRotation2, error2,desiredRotation2);

                double previous_error1 = error1;
                double previous_error2 = error2;

                error1 = desiredRotation1 - currentRotation1;
                error2 = desiredRotation2 - currentRotation2;
                // P control
                // double control1 = p_control(error1, motor1_kp);
                // double control2 = p_control(error2, motor2_kp);
                
                // PI control
                //double control1 = pi_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral);
                // double control2 = pi_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral);
                
                // PID control
                double control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
                double control2 = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);

                int d1 = getPDirection(control1,1);
                int d2 = getPDirection(control2,2);
                float speed1 = fabs(control1);
                float speed2 = fabs(control2);

                if (speed1 < 0.1f) speed1 = 0.0f;
                if (speed2 < 0.1f) speed2 = 0.0f;
                dir1 = d1;
                dir2 = d2;
                pwm1 = speed1;
                pwm2 = speed2;
                pc.printf("SPEED: %f      | DIR: %i\r\n",speed2,d2);

                go_motor = false;
            }

            /*if (go_graph) {
                pc.printf("Trying to send graph\r\n");
                pc.printf("Desired Rotation: %f %f\r\n", desiredRotation1, desiredRotation2);
                pc.printf("Rotation: %f %f\r\n", currentRotation1, desiredRotation2);
                grapher.set(0, desiredRotation1);
                grapher.set(1, currentRotation1);
                grapher.set(2, desiredRotation2);
                grapher.set(3, currentRotation2);
                grapher.send();
                go_graph = false;
            }*/
        }
    }
}