File content as of revision 14:c4dbce7de430:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "BiQuad.h"
#include "math.h"
#include <vector>

// CHECK
PwmOut pwmpin(D6);
PwmOut pwmpin2(D5);
DigitalOut directionpin2(D4);
DigitalOut directionpin(D7);
MODSERIAL pc(USBTX, USBRX);
//Serial pc(USBTX, USBRX);

QEI wheel2 (D13, D12, NC, 32, QEI::X2_ENCODING);
QEI wheel1 (D11, D10, NC, 32, QEI::X2_ENCODING);
float u1,u2 = 0;

// for trajectory control:
double T = 1; // time to get to destination
double currentx, currenty;
double currentq1, currentq2;
// end
int pulses1, pulses2 = 0;
float angle1, angle2;
int realangle1, realangle2;
double error1;
double error2;
double actualoutput1, actualoutput2 = 0;
bool reached;


Ticker adjust_ref;
Ticker pid;

float angle_resolution = (360.0/32.0)*(1.0/131.0);  //degrees/counts 
double Kp = 0.1;
double Ki = 0.4;
double Kd = 4;
extern double samplingfreq = 1000;
 
double L1 = 0.4388;
double L2 = 0.4508;
double x01 = 0.0;
double y01 = 0.0;
double ref1, ref2;
double t = 0.0;
vector<double> inv_kin_ref;
vector<double> newconfig;
vector<double> currentxy;

double timetogetthere = 0.01;
double vx_des = 0.05;
double vy_des = -0.05;

float maxpwm = 1;

void PID_controller(double error1, double error2, float &u1, float &u2, double actualoutput1, double actualoutput2)
{   
    static double error_prev1 = error1; // initialization with this value only done once!
    static double error_prev2 = error2; // initialization with this value only done once!
    double u_pid2, u_pid1;
    static double error_integral1, error_integral2 = 0;
  
    double u_k1 = Kp * error1;
    double u_k2 = Kp * error2;
    
    static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    double error_derivative1 = (error1 - error_prev1)*1/samplingfreq;
    double filtered_error_derivative1 = LowPassFilter1.step(error_derivative1);
    double u_d1 = Kd * filtered_error_derivative1;
    
    double error_derivative2 = (error2 - error_prev2)*1/samplingfreq;
    double filtered_error_derivative2 = LowPassFilter2.step(error_derivative2);
    double u_d2 = Kd * filtered_error_derivative2;
    
    error_integral1 = error_integral1 +( error1 * 1/samplingfreq);
    error_integral2 = error_integral2 +( error2 * 1/samplingfreq);
    if (error_integral1 > 1){
        error_integral1 = 1;
    }
    else if (error_integral1 < -1){
        error_integral1 = -1;
    }
    if (error_integral2 > 1){
        error_integral2 = 1;
    }
    else if (error_integral2 < -1){
        error_integral2 = -1;
    }
    
    double u_i1 = Ki*error_integral1;
    double u_i2 = Ki*error_integral2;
    
    u_pid1 = u_k1 + u_d1 + u_i1;
    u_pid2 = u_k2 + u_d2 + u_i2;
    
    u1 = -u_pid1;
    u2 = -u_pid2;
    
    error_prev1 = error1;
    error_prev2 = error2; 
     
   
}

vector<double> forwardkinematics_function(double& q1, double& q2) {
    // input are joint angles, output are x and y position of end effector
    q1 = (q1 + 90)*(3.14/180);
    q2 = (q2 + 180)*(3.14/180);
    
    currentx = x01 + L1*cos(q1)-L2*cos(q2);
    currenty = y01 + L1 * sin(q1) - L2 * sin(q2); 
    
    vector<double> xy;
    xy.push_back(currentx);
    xy.push_back(currenty);
    return xy;
   
}
 
vector<double> inversekinematics_function(const double& T, double& q1, double& q2, double& des_vx, double& des_vy, double ref1, double ref2) {
    // x, y: positions of end effector | T: time to hold current velocity | qref1, qref2: reference thetas | q1, q2: current thetas | vx, vy: desired x, y velocities
    
    // pseudo inverse jacobian to get joint speeds
    // input are desired vx and vy of end effector, output joint angle speeds
 
    double q1_star_des; // desired joint velocity of q1_star
    double q2_star_des; // same as above but then for q2_star
    double C;
    double qref1, qref2;
 
    // The calculation below assumes that the end effector position is calculated before this function is executed
    // In our case the determinant will not equal zero, hence no problems with singularies I think.
    //
    //
    
    q1 = (ref1 + 90)*(3.14/180);
    q2 = (ref2 + 180)*(3.14/180);
    
    C = L1*L2*sin(q1)*cos(q2) - L1*L2*sin(q2)*cos(q1);
    q1_star_des = double(1/C)*(-L2*cos(q2)*des_vy - L2*sin(q2)*des_vx);
    q2_star_des = double(1/C)*((L2*cos(q2)-L1*cos(q1))*des_vy + (L2*sin(q2)-L1*sin(q1))*des_vx);
    
    qref1 = T*q1_star_des;
    qref2 = T*(q2_star_des+q1_star_des);
    
    qref1 = qref1*(180/3.14); 
    qref2 = qref2*(180/3.14);
    
    double testq1 = ref1+qref1;
    double testq2 = ref2+qref2;
    newconfig = forwardkinematics_function(testq1, testq2);
    
    if (sqrt(pow(newconfig[0],2)+pow(newconfig[1],2)) > 0.73)
    {
        qref1 = ref1;
        qref2 = ref2;
    }
    else
    {
        qref1 = ref1 + qref1;
        qref2 = ref2 + qref2;
    }

    vector<double> thetas;
    thetas.push_back(qref1);
    thetas.push_back(qref2);
    return thetas;
}

void control(){
        // Calculate angle of motor 1
        pulses1 = wheel1.getPulses();
        angle1 = -pulses1*angle_resolution;
        realangle1 = int(angle1) % 360;
        
        // Calculate angle of motor 2
        pulses2 = wheel2.getPulses();
        angle2 = pulses2*angle_resolution;
        realangle2 = int(angle2) % 360;
        
        // Q1 and Q2 current
        currentq1 = angle1;
        currentq2 = angle2;
        
        // modify the current x and y pos
        currentxy = forwardkinematics_function(currentq1, currentq2);
        currentx = currentxy[0];
        currenty = currentxy[1];
        
        // calculate the reference position using inverse kinematics:
        inv_kin_ref = inversekinematics_function(timetogetthere, currentq1, currentq2, vx_des, vy_des, ref1, ref2);
        ref1 = inv_kin_ref[0];
        ref2 = inv_kin_ref[1];
        
        error1 = ref1 - realangle1;
        error2 = ref2 - realangle2;
              
        // control stuff:
        PID_controller(error1, error2, u1, u2, actualoutput1, actualoutput2);
        
        
        // Check if output is larger than maximum pwm 
        if (abs(u1) > maxpwm){
            if (u1 > 0){
                u1 = maxpwm;
            }
            else{
                u1 = -maxpwm;
            }  
        }   
        if (abs(u2) > maxpwm){
            if (u2 > 0){
                u2 = maxpwm;
            }
            else{
                u2 = -maxpwm;
            }  
        }
                   
        pwmpin = fabs(u1);
        pwmpin2 = fabs(u2);

        // 1/true is negative, 0/false is positive 
        directionpin2 = u2 < 0;
        directionpin = u1 < 0;
}
void measure()
{
    
}

int main()
{   
    pc.baud(115200);
    pwmpin.period_us(60);
    pid.attach(&control,0.01);
    ref1 = 0;
    ref2 = 0;
    
    while (true) {
    pc.printf("error1: %f, error2: %f, pwm1: %f, pwm2: %f \r \n", error1, error2, u1, u2);  
    vector<double> currentthing;    
    currentthing = forwardkinematics_function(ref1,ref2);
    double cx = currentthing[0];
    double cy = currentthing[1];
    pc.printf("x: %f, y: %f \r\n",cx,cy);
    }
}