File content as of revision 12:84f2c63f9b98:

#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 motor1_kp = 0.75f, motor1_ki = 0.005f, motor1_kd = 0.05f;
const double motor2_kp = 0.75f, motor2_ki = 0.005f, motor2_kd = 0.05f;

const double motor1_push_kp = 1.5f, motor1_push_ki = 0.005f, motor1_push_kd = 0.1f;
const double motor2_push_kp = 1.5f, motor2_push_ki = 0.005f, motor2_push_kd = 0.1f;

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, offsetB;

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

bool pushing;

/*
* Calculates the number of radians given a number of pulses of an encoder
*   int pulses: The number of pulses to convert into radians
*/
float toRadians(int pulses)
{
    int remaining = pulses;
    float percent = (float) remaining / (float) sigPerRev;
    return percent * 2.0f;
}

/*
* Calculates the result of the PID Controller
*/
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;
}

/*
* Get the direction of the motor
*   double control: The desired direction
*   int motor:      The ID of the motor [1 == motor 1 | 2 == motor 2]
*/
int getPDirection(double control, int motor)
{
    if (control >= 0)
        return (motor == 1)?1:0;
    else
        return (motor == 1)?0:1;
}

/*
* The initializator of the PID Controller
*/
void PID_init()
{
    pc.printf("Initializing PID Controller...\r\n");

    /* Setting the pulse width modulation */
    pwm1.period_ms(0.01);
    pwm1.pulsewidth_ms(0.005);
    pwm2.period_ms(0.01);
    pwm2.pulsewidth_ms(0.005);
    
    /* Resetting the encoders */
    enc1.reset();
    enc2.reset();
    pc.printf("Encoders reset\r\n");

    /*  
    * Calibrate the first motor
    *   Rotate it until it hits the base, then reset the encoder and calculate the offset.  
    */
    bool cont = true;
    double prev = -1.0;
    while(cont) {
        moveOneMotor(1,0);
        wait(.1);
        double temp = toRadians(enc1.getPulses());
        if(fabs(prev-temp) < 0.005) {
            pwm1 = 0;
            cont = false;
        } else
            prev = temp;
    }
    enc1.reset();

    /* Move the first motor to a better starting position */
    moveOneMotor(1,1);
    wait(1.1);
    pwm1 = 0;

    /*  
    * Calibrate the second motor
    *   Rotate it until it hits the base, then reset the encoder and calculate the offset.  
    */
    cont = true;
    while(cont) {
        moveOneMotor(2,0);
        wait(.1);
        double temp = toRadians(enc2.getPulses());
        if(fabs(prev-temp) < 0.005) {
            pwm2 = 0;
            cont = false;
        } else
            prev = temp;
    }

    enc2.reset();

    /* Move the first motor to a better starting position */
    moveOneMotor(2,1);
    wait(1);
    pwm2 = 0;

    /* Calculate both offsets */
    offsetA = deg2rad(243);      // 243
    offsetB = deg2rad(-63);      // -63

    /* Set the default desired rotation */
    desiredRotation1 = toRadians(enc1.getPulses());
    desiredRotation2 = toRadians(enc2.getPulses());
}

/*
* Gives the current rotation of the motors.
*   double& a: The variable to store the rotation of motor 1
*   double& b: The variable to store the rotation of motor 2
*/
void getCurrent(double& a, double& b)
{
    a = toRadians(enc1.getPulses());
    b = toRadians(enc2.getPulses());
}

/*
* Rotates the motors to certain rotations
*   double a: The desired rotation of motor 1
*   double b: The desired rotation of motor 2
*/
void rotate(double a, double b)
{
    desiredRotation1 = -(a - offsetA);
    desiredRotation2 = -(b - offsetB);
}

/*
* Rotates the motors to certain rotations, while using the PID Controller used for the pushing motion
*   double a: The desired rotation of motor 1
*   double b: The desired rotation of motor 2
*/
void push(double a, double b)
{
    pushing = true;
    desiredRotation1 = -(a - offsetA);
    desiredRotation2 = -(b - offsetB);
}

/*
* Returns the offset of the motors calulated in the initialization phase.
*   int a: The id of the motor [1 == motor 1 | 2 == motor 2]
*/
double getOffset(int a)
{
    if(a == 1) return offsetA;
    else return offsetB;
}

/*
* Stops the motors.
*/
void PID_stop()
{
    pwm1 = 0;
    pwm2 = 0;
}

/*
* Calculates the speed and direction of the motors using a PID Controller and the current rotation of the motors, given a desired rotation.
*/
void moveTick()
{
    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, control2;
    if(pushing) {
        control1 = pid_control(error1, motor1_push_kp, motor1_push_ki, tickRate, m1_error_integral, motor1_push_kd, previous_error1, m1_error_derivative, m1_filter);
        control2 = pid_control(error2, motor2_push_kp, motor2_push_ki, tickRate, m2_error_integral, motor2_push_kd, previous_error2, m2_error_derivative, m2_filter);
    } else {
        control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
        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.02f) speed1 = 0.0f;
    if (speed2 < 0.02f) speed2 = 0.0f;
    
    dir1 = d1;
    dir2 = d2;
    pwm1 = speed1;
    pwm2 = speed2;
}

/* 
* Moves one motor with a speed of 0.2.
*   int num: the ID of the motor [1 == motor 1 | 2 == motor 2]
*   int dir: the direction of the motor
*/
void moveOneMotor(int num, int dir)
{
    if(num == 1) {
        dir1 = dir;
        pwm1 = 0.2;
    } else {
        dir2 = dir;
        pwm2 = 0.2;
    }
}