File content as of revision 5:937b2f34a1ca:

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

Serial pc(USBTX, USBRX);

PinName m1_enc_a = D12;
PinName m1_enc_b = D13;
PinName m1_pwm = D6;
PinName m1_dir = D7;

PinName m2_enc_a = D11;
PinName m2_enc_b = D10;
PinName m2_pwm = D5;
PinName m2_dir = D4;

PinName m2_pot = A0;
PinName m1_pot = A1;

const double pid_upper_limit = 1, pid_lower_limit = 0;

const double motor1_kp = 0.75f, motor1_ki = 0.001f, motor1_kd = 0.005f;
const double motor2_kp = 0.75f, motor2_ki = 0.001f, motor2_kd = 0.005f;

const double m1_f_a1 = 1.0, m1_f_a2 = 2.0, m1_f_b0 = 1.0, m1_f_b1 = 3.0, m1_f_b2 = 4.0;
const double m2_f_a1 = 1.0, m2_f_a2 = 2.0, m2_f_b0 = 1.0, m2_f_b1 = 3.0, m2_f_b2 = 4.0;

int sigPerRev = 4192;
float tickRate = 0.01f;
float graphTickRate = 0.01f;

float toRadians(int pulses);

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 move(double a, double b) {
    while (true) {
        if (shutup) {
            pwm1 = 0;
            pwm2 = 0;
        } else {
            if (go_pot) {
                desiredRotation1 = a;
                desiredRotation2 = b;
                
                go_pot = false;
            }

            if (go_motor) {
                currentRotation1 = toRadians(enc1.getPulses());
                currentRotation2 = toRadians(enc2.getPulses());

                double previous_error1 = error1;
                double previous_error2 = error2;

                error1 = desiredRotation1 - currentRotation1;
                error2 = desiredRotation2 - currentRotation2;
                
                // 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;

                go_motor = false;
            }
        }
    }
}