File content as of revision 10:f05bd773b66c:

#include "mbed.h"
#include "QEI.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "PinDetect.h"
#include "pidControl.h"
#include "biquadFilter.h"
#include "compute.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;

double offsetA;
double offsetB;

float toRadians(int pulses);

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);

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);

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;
}

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 PID_init(double angleA, double angleB)
{
    pc.printf("Initializing...\r\n");
    
   // pwm1.period(0.00001);
   // pwm1.pulsewidth(0.000005);
   // pwm2.period(0.00001);
   // pwm2.pulsewidth(0.000005);

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

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

    bool cont = true;
    double prev = -1.0;
    while(cont) {
        moveOneMotor(1,0);
        wait(0.5);
        double temp = toRadians(enc1.getPulses());
        //pc.printf("PREV | TEMP = %f | %f\r\n",prev,temp);
        if(fabs(prev-temp) < 0.005) {
            pwm1 = 0;
            cont = false;
        } else
            prev = temp;
    }

    enc1.reset();

    pc.printf("Angle B: %d\r\n\r\n", rad2deg(toRadians(enc1.getPulses())));

    pc.printf("RAD: 1 DEG: %d\r\n",rad2deg(1));

    moveOneMotor(1,1);
    wait(1.5);
    pwm1 = 0;

    cont = true;
    while(cont) {
        moveOneMotor(2,0);
        wait(0.5);
        double temp = toRadians(enc2.getPulses());
        if(fabs(prev-temp) < 0.005) {
            pwm2 = 0;
            cont = false;
        } else
            prev = temp;
    }

    enc2.reset();

    pc.printf("Angle B: %d\r\n\r\n", rad2deg(toRadians(enc2.getPulses())));

    moveOneMotor(2,1);
    wait(1.3);
    pwm2 = 0;

    offsetA = deg2rad(243);      // 243
    offsetB = deg2rad(-63);      // -63
    
    desiredRotation1 = toRadians(enc1.getPulses());
    desiredRotation2 = toRadians(enc2.getPulses());

    pc.printf("Angle A: %d | Angle B: %d\r\n\r\n", rad2deg(toRadians(enc1.getPulses())), rad2deg(toRadians(enc2.getPulses())));

    pc.printf("A: -%d + %d = %d\r\nB: -%d + %d = %d\r\n",rad2deg(offsetA),135,rad2deg(-offsetA+deg2rad(135)),rad2deg(offsetB),45,rad2deg(-offsetB+deg2rad(45)));

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

void getCurrent(double& a, double& b)
{
    a = toRadians(enc1.getPulses());
    b = toRadians(enc2.getPulses());
}

void rotate(double a, double b)
{
    desiredRotation1 = -(a - offsetA);
    desiredRotation2 = -(b - offsetB); 
}

double getOffset(int a) {
    if(a == 1) return offsetA;
    else return offsetB;
}
    

void PID_stop() {
    pwm1 = 0;
    pwm2 = 0;   
}

void moveTick()
{
    currentRotation1 = toRadians(enc1.getPulses());
    currentRotation2 = toRadians(enc2.getPulses());

    double previous_error1 = error1;
    double previous_error2 = error2;

    error1 = desiredRotation1 - currentRotation1;
    error2 = desiredRotation2 - currentRotation2;

    //pc.printf("Error1: %f | Error2: %f\r\n",error1, error2);

    // 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);

    //pc.printf("CUR ROT 1: %f\r\nCUR ROT 2: %f\r\n",control1, control2);

    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;
    
    //pc.printf("A: %d | B: %d \r\n",rad2deg(toRadians(enc1.getPulses())), rad2deg(toRadians(enc2.getPulses()))); 
    
    dir1 = d1;
    dir2 = d2;
    pwm1 = speed1;
    pwm2 = speed2;
}

void moveOneMotor(int num, int dir)
{
    if(num == 1) {
        dir1 = dir;
        pwm1 = 0.2;

        //pc.printf("DIR 1: %f\r\n",dir);
    } else {
        dir2 = dir;
        pwm2 = 0.2;

        // pc.printf("DIR 2: %f\r\n",dir);
    }
}