File content as of revision 21:245c676f9d72:

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

// To play with buttons and potmeters
AnalogIn pot2(A0);
AnalogIn pot1(A1);
DigitalIn but1(D2);
DigitalIn but2(D3);
DigitalIn but3(SW2);


// Usual stuff
MODSERIAL pc(USBTX, USBRX);

//ticker to call motor values
Ticker motor; 

// Direction and Velocity of the motors
DigitalOut motor1Direction(D7); 
FastPWM motor1Velocity(D6);
DigitalOut motor2Direction(D4);
FastPWM motor2Velocity(D5);

// Encoders 1 and 2 respectively
QEI Encoder1(D8,D9,NC,8400);
QEI Encoder2(D11,D10,NC,8400);


volatile double frequency = 17000.0;
volatile double period_signal = 1.0/frequency;
float timeinterval = 0.001f;
float motorvalue1;
float motorvalue2;
float yendeffector = 10.6159;
float xendeffector = 0;
float potmeter1 = 5+pot1.read()*60;
float potmeter2 = pot2.read()*60;
    float desiredmotorangle1;
    float desiredmotorangle2;

// ANTI WIND UP NEEDED

// --------------------   README ------------------------------------
// positive referenceposition corresponds to a counterclockwise motion
// negative referenceposition corresponds to a clockwise motion 

//PID control implementation (BiQuead)
double PID_controller1(float error1)
{
    // Define errors for motor 1 and 2
    static double error_integral1 = 0;
    static double error_prev1 = error1;
    
    // Low-pass filter
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    // PID variables: we assume them to be the same for both motors
    float Kp = 64.9;
    float Ki = 3.64;
    float Kd = 5;
    
    //Proportional part:
    double u_k1 = Kp * error1;
    
    // Integreal part
    error_integral1 = error_integral1 + error1 * timeinterval;
    double u_i1 = Ki*error_integral1;

    // Derivate part
    double error_derivative1 = (error1 - error_prev1)/timeinterval;
    double filtered_error_derivative1 = LowPassFilter.step(error_derivative1);
    double u_d1 = Kd * filtered_error_derivative1;
    error_prev1 = error1;
    
    //sum all parts and return it
    return u_k1 + u_i1 + u_d1;
}

double PID_controller2(float error2)
{
    // Define errors for motor 1 and 2
    static double error_integral2 = 0;
    static double error_prev2 = error2;
    
    // Low-pass filter
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    // PID variables: we assume them to be the same for both motors
    float Kp = 64.9;
    float Ki = 3.64;
    float Kd = 5;
    
    //Proportional part:
    double u_k2 = Kp * error2;
    
    // Integreal part
    error_integral2 = error_integral2 + error2 * timeinterval;
    double u_i2 = Ki*error_integral2;

    // Derivate part
    double error_derivative2 = (error2 - error_prev2)/timeinterval;
    double filtered_error_derivative2 = LowPassFilter.step(error_derivative2);
    double u_d2 = Kd * filtered_error_derivative2;
    error_prev2 = error2;
    
    //sum all parts and return it
    return u_k2 + u_i2 + u_d2;
}

//get the measured position
void getmeasuredposition(float & measuredposition1, float & measuredposition2)
{   
    // Obtain the counts of motors 1 and 2 from the encoder
    int countsmotor1;
    int countsmotor2;
    countsmotor1 = Encoder1.getPulses();
    countsmotor2 = Encoder2.getPulses();
    
    // Obtain the measured position for motor 1 and 2
    measuredposition1 = ((float)countsmotor1) / 8400.0f * 2.0f;
    measuredposition2 = ((float)countsmotor2) / 8400.0f * 2.0f;
}

//get the reference of the 
void getreferenceposition(float & referenceposition1, float & referenceposition2)
{
    //Measurements of the arm
    float L0=1.95;
    float L1=15;
    float L2=20;
    
    //Define the new counts that are needed
//    float desiredmotorangle1;
//    float desiredmotorangle2;
    
    //Inverse kinematics: given the end position, what are the desired motor angles of 1 and 2
    desiredmotorangle1 = (atan2(yendeffector,(L0+xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0+xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0+xendeffector,2)+pow(yendeffector,2))))*180/3.1415)-180;
    desiredmotorangle2 = (atan2(yendeffector,(L0-xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0-xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0-xendeffector,2)+pow(yendeffector,2))))*180/3.1415)-180;

    //Apply boundaries of the motor angles
//    if (desiredmotorangle1 < -125){
//        desiredmotorangle1 = -124.9;
//    }
//    if (desiredmotorangle1 > 28){
//        desiredmotorangle1 = 27.9;
//    }    
//    if (desiredmotorangle2 > 46){
//        desiredmotorangle2 = 45.9;
//    }    
//    if (desiredmotorangle2 < -130){
//        desiredmotorangle2 = -129.9;
//    }
    
    //Convert motor angles to counts
    float desiredmotorrounds1;
    float desiredmotorrounds2;
    desiredmotorrounds1 = (desiredmotorangle1)/360;
    desiredmotorrounds2 = (desiredmotorangle2)/360;
    
    //Assign this to new variables because otherwise it doesn't work
    referenceposition1 = desiredmotorrounds1;
    referenceposition2 = desiredmotorrounds2;
}   

//send value to motor
void sendtomotor(float & motorvalue1, float & motorvalue2)
{   
    // Define the absolute motor values
    float absolutemotorvalue1 = 0.0f;
    float absolutemotorvalue2 = 0.0f;
    absolutemotorvalue1 = fabs(motorvalue1);
    absolutemotorvalue2 = fabs(motorvalue2);
    
    // If absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
    absolutemotorvalue1 = absolutemotorvalue1 > 1.0f ? 1.0f : absolutemotorvalue1;  
    absolutemotorvalue2 = absolutemotorvalue2 > 1.0f ? 1.0f : absolutemotorvalue2;
    
    // Send the absolutemotorvalue to the motors
    motor1Velocity = absolutemotorvalue1;
    motor2Velocity = absolutemotorvalue2;
    
    // Determine the motor direction. Boolean output: true gives counterclockwise direction, false gives clockwise direction
    motor1Direction = (motorvalue1 > 0.0f); 
    motor2Direction = (motorvalue2 > 0.0f);
}

// function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
void measureandcontrol(void)
{
    // Get the reference positions of motor 1 and 2
    float reference1, reference2;
    getreferenceposition(reference1, reference2); 
    
    // Get the measured positions of motor 1 and 2
    float measured1, measured2;
    getmeasuredposition(measured1, measured2);
    
    // Calculate the motor values
    float motorvalue1, motorvalue2;
    motorvalue1 = PID_controller1(reference1 - measured1);
    motorvalue2 = PID_controller2(reference2 - measured2);
    sendtomotor(motorvalue1, motorvalue2);
}
int main()
{
    // Usual stuff
    pc.baud(115200);
    pc.printf("Starting...\r\n");
    
    // Something with pulses
    motor1Velocity.period(period_signal);
    motor2Velocity.period(period_signal);
    
    // Call the ticker function
    motor.attach(measureandcontrol, timeinterval); 
    while(true)
    {
        wait(0.01);
        float potmeter1 = 5+pot1.read()*60;
        float potmeter2 = pot2.read()*60;
//        pc.printf("Kp gives %f\r\n", potmeter1);
//        pc.printf("Ki gives %f\r\n", potmeter2);
        pc.printf("x is %f\r\n",xendeffector);
        pc.printf("y is %f\r\n",yendeffector);   
        pc.printf("motorangle1 is%f\r\n",desiredmotorangle1);
        pc.printf("motorangle2 is%f\r\n",desiredmotorangle2);
        
        float ditmoetkleinerzijndan = pow((xendeffector-1.95),2)+pow(yendeffector,2);
        float dit = 28*28;
        
        float ditmoetkleinerzijndan2 = pow((xendeffector-1.95),2)+pow(yendeffector,2);
        float dit2 = 28*28;
        
 

        // Control position    
        char a = pc.getcNb();
        if(xendeffector > 1.95){
            if(a == 'r' && ditmoetkleinerzijndan < dit){
                xendeffector=xendeffector+1;}
            else if(a=='l'){
                xendeffector=xendeffector-1;}
            else if(a=='u' && ditmoetkleinerzijndan2 < dit2){
                yendeffector=yendeffector+1;}
            else if(a=='d'){
                yendeffector=yendeffector-1;}
        }
        else if( 0 < xendeffector < 1.95){
            if(a == 'r'){
                xendeffector=xendeffector+1;}
            else if(a=='l' && pow(xendeffector-1.95,2)+pow(yendeffector,2) < 28*28){
                xendeffector=xendeffector-1;}
            else if(a=='u' && pow(xendeffector-1.95,2)+pow(yendeffector,2) < 28*28){
                yendeffector=yendeffector+1;}
            else if(a=='d'){
                yendeffector=yendeffector-1;}            
        }
        else if( -1.95 < xendeffector < 0){
            if(a == 'r' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
                xendeffector=xendeffector+1;}
            else if(a=='l'){
                xendeffector=xendeffector-1;}
            else if(a=='u' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
                yendeffector=yendeffector+1;}
            else if(a=='d'){
                yendeffector=yendeffector-1;}            
        }
        else if( xendeffector < -1.95){
            if(a == 'r'){
                xendeffector=xendeffector+1;}
            else if(a=='l' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
                xendeffector=xendeffector-1;}
            else if(a=='u' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
                yendeffector=yendeffector+1;}
            else if(a=='d'){
                yendeffector=yendeffector-1;}            
        }        
        
        // Go back to starting position    
        if (but1.read() == 0) { 
            xendeffector=0; 
            yendeffector=10.6159;
        }
        
        if (but2.read() == 0){
            xendeffector=-5; 
            yendeffector=10.6159;
            wait(0.5);
            
            xendeffector=10; 
            yendeffector=25.6159;
            wait(0.5);
            
            xendeffector=-14; 
            yendeffector=21.6159;
            wait(0.5);
            
            xendeffector=10; 
            yendeffector=11.6159;
            wait(0.5);
            
            xendeffector=0; 
            yendeffector=10.6159;
            wait(0.5);
            }
            
        if (but3.read() ==0){
           
            xendeffector=-5;
            yendeffector=10.6159;
            int i=0;
            wait(0.5);
            while(i<100){
                xendeffector=xendeffector+0.1;
                yendeffector=yendeffector+0.1;
                i++;
                wait(0.001);
            }
        }
            
            
            
        
        // type c to stop the program
        char c = pc.getcNb();
        if (c == 'c')
        {
            pc.printf("Program stopped running");
            motorvalue1 = 0;
            motorvalue2 = 0;
            sendtomotor(motorvalue1, motorvalue2);
            break;
        }
    }
}