File content as of revision 13:3482d315877c:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "BiQuad.h"

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

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

// for trajectory control:
double T = 1; // time to get to destination
double currentx, currenty;
double currentq1, currentq2;
// end    


float angle_resolution = (360.0/32.0)*(1.0/131.0);  //degrees/counts 
double Kp = 2;
double Ki = 1.02;
double Kd = 10;
extern double samplingfreq = 1000;
 
double L1 = 0.43;
double L2 = 0.43;
double x01 = 0.0;
double y01 = 0.0;

void forwardkinematics_function(double& q1, double& q2) {
    // input are joint angles, output are x and y position of end effector
    
    currentx = x01 + L1*cos(q1)-L2*cos(q2);
    currenty = y01 + L1 * sin(q1) - L2 * sin(q2);    
}
 
vector<double> inversekinematics_function(double& x, double& y, const double& T, double& q1, double& q2, double& des_vx, double& des_vy) {
    // x, y: positions of end effector | T: time to get to position | 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 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_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_vx-x*des_vy);
    q2_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_vx+1*(-y+y01)*des_vy); // CHECK THIS ONE!!!
    
    qref1 = q1+T*q1_star_des; // Yet to adapt all these equations
    qref2 = q2+T*(q2_star_des - q1_star_des); 
    
    vector<double> thetas;
    thetas.push_back(qref1);
    thetas.push_back(qref2);
    return thetas;
}


void PID_controller(double error1, double error2, float &u1, float &u2)
{   
    double u_k = Kp * error1;
    double u_k2 = Kp * error2;
    static double error_integral = 0;
    static double error_prev = error1; // initialization with this value only done once!
    static double error_prev2 = error2; // initialization with this value only done once!

    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    error_integral = error_integral + error1 * 1/samplingfreq;
    double u_i = Ki * error_integral;
    double error_derivative = (error1 - error_prev)*samplingfreq;
    double filtered_error_derivative = LowPassFilter.step(error_derivative);
    double u_d = Kd * filtered_error_derivative;
    error_prev = error1;
    error_prev2 = error2; 
    u1 =  float(u_k/360);//+u_i+u_d;
    u2 = float(u_k2/360);
    
   
   
}

int main()
{   
    pwmpin.period_us(60);
    int pulses1, pulses2 = 0;
    float angle1, angle2;
    int realangle1, realangle2;
    double ref1, error1;
    double ref2, error2;
    bool reached;
    
    double vx, vy;
    
    while (true) {
        pulses1 = wheel1.getPulses();
        angle1 = pulses1*angle_resolution;
        realangle1 = abs(int(angle1)) % 360;
        
        pulses2 = wheel2.getPulses();
        angle2 = pulses2*angle_resolution;
        realangle2 = abs(int(angle2)) % 360;
        
        currentq1 = angle1;
        currentq2 = angle2;
        
        //forwardkinematics_function(currentq1, currentq2);
        T = 2; // 2 seconds seems slow enough :D
        vx = 0.1;
        vy = 0;
    
        vector<double> refangles = inversekinematics_function(currentx, currenty, T, currentq1, currentq2, vx, vy);
        
        ref1 = 0;
        ref2 = 0;
        
        error1 = ref1 - realangle1;
        error2 = ref2 - realangle2;
        
        
        //PID_controller(error1, error2, u1, u2); 
        if(u1 > 1){
            u1 = 1;
        }
        if(u1 < -1){
            u1 = -1;
            }
        if(u2 > 1){
            u2 = 1;
        }
        if(u2 < -1){
            u2 = -1;
            } 

            
        pwmpin = fabs(u1);
        pwmpin2 = fabs(u2);
         
        // 1/true is negative, 0/false is positive 
        directionpin2 = u2 < 0;
        directionpin = u1 < 0;
        pc.printf("angle %f, error: %f, pwm: %f \r \n",realangle1, error1, u1);
        
         
        
    }
}