File content as of revision 9:d04d028ccfe8:

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

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 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 PID_init(double angleA, double angleB)
{
    pc.printf("Initializing...\r\n");

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

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

    go_motor = true;
    shutup = false;

    // Start tickers
    potTicker.attach(&readPot, tickRate);
    motorTicker.attach(&getMotorRotation, tickRate);
    graphTicker.attach(&sendGraph, graphTickRate);
    pc.printf("Tickers attached\r\n");
    
    bool cont = true;
    double a =0.0;
    double b = 0.0;
    double prev = -1.0;
    while(cont) {
        a -= 0.1;
        pc.printf("%f\r\n",a);
        moveOneMotor(1,a);
        wait(1);
        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;
            offsetA = deg2rad(200)-temp;
        }
        else 
            prev = temp;
    }
    
    pc.printf("\r\nOFFSET: %f | OFFSET: %d\r\n",offsetA, rad2deg(offsetA));
    pc.printf("\r\nTO: %f | TO: %d",deg2rad(135)-offsetA, rad2deg(deg2rad(135)-offsetA));
    
    moveOneMotor(1,deg2rad(135)-offsetA);
    wait(3);
    
    cont = true;
    while(cont) {
        pwm1 = 0;
        b -= 0.1;
        moveOneMotor(2,b);
        wait(1);
        double temp = toRadians(enc2.getPulses());
        if(fabs(prev-temp) < 0.005) {
            pwm2 = 0;
            cont = false;
            offsetB = deg2rad(-45)-temp;
        }
        else 
            prev = temp;
    }

    //rotate(offsetA+angleA,offsetB+angleB);

    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)
{
    if (shutup) {
        pwm1 = 0;
        pwm2 = 0;
    } else {
        desiredRotation1 = a;
        desiredRotation2 = b;

        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);
        
        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;
        dir1 = d1;
        dir2 = d2;
        pwm1 = speed1;
        pwm2 = speed2;
    }
}

void moveOneMotor(int num, double angle) {
    if(num == 1) {
        desiredRotation1 = angle;
        currentRotation1 = toRadians(enc1.getPulses());
        double previous_error1 = error1;
        error1 = desiredRotation1 - currentRotation1;

        double control = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
        
        int d = getPDirection(control,1);
        float speed = fabs(control);
        pc.printf("SPEED: %f\r\n",speed);
        if (speed <= 0.1f) speed = 0.0f;
        if(speed > 0.35f) speed = 0.35f;
        speed = 0.3;
        
        dir1 = d;
        pwm1 = speed;
        
        pc.printf("DIR 1: %f | PWM 2: %f\r\n",d, speed); 
    }
    else {
        desiredRotation2 = angle;
        currentRotation2 = toRadians(enc2.getPulses());
        double previous_error2 = error2;
        error2 = desiredRotation2 - currentRotation2;

        double control = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
        
        int d = getPDirection(control,1);
        float speed = fabs(control);
        if (speed <= 0.1f) speed = 0.0f;
        if(speed > 0.35f) speed = 0.35f;
        speed = 0.3;
        
        dir2 = d;
        pwm2 = speed;
        
        pc.printf("DIR 2: %f | PWM 2: %f\r\n",d, speed); 
    }
}