File content as of revision 11:ca91fc47ad02:

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

DigitalIn button1(D12);
AnalogIn pot2(A0);
AnalogIn pot1(A1);
Ticker motor; //ticker to call motor values
DigitalOut motor1Direction(D7); // is a boolean
FastPWM motor1absolutemotorvalueocity(D6);
MODSERIAL pc(USBTX, USBRX);
QEI Encoder(D8,D9,NC,8400);


volatile double frequency = 17000.0;
volatile double period_signal = 1.0/frequency;
float timeinterval = 0.001f;
int counts;
float measuredposition;
float motorvalue;
double u_i;
float potmeter1 = pot1.read();
float yendeffector = 20;
float xendeffector = 0;
float Kp;





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

//PID control implementation (BiQuead)
double PID_controller(double error)
{
    static double error_integral = 0;
    static double error_prev = error;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    float Kp = 30;
    float Ki = 2;
    float Kd = 0.6;
    
    //Proportional part:
    double u_k = Kp * error;
    
    // Integreal part
    error_integral = error_integral + error * timeinterval;
    double u_i = Ki*error_integral;
    // anti wind up
    if (u_i > 10)
    {
        u_i = 10 ;   
    }
    
    // Derivate part
    double error_derivative = (error - error_prev)/timeinterval;
    double filtered_error_derivative = LowPassFilter.step(error_derivative);
    double u_d = Kd * filtered_error_derivative;
    error_prev = error;
    
    //sum all parts and return it
    return u_k + u_i + u_d;
}

//get the measured position
double getmeasuredposition()
{   
    counts = Encoder.getPulses();
    measuredposition = ((float)counts) / 8400.0f * 2.0f;
    
    return measuredposition;
}

//get the reference of the absolutemotorvalueocity
double getreferenceposition()
{
    float L0 = 1.95;
    float L1=15;
    float L2=20;
    float motorcounts1;
    float tangens = atan2(yendeffector,(L0+xendeffector))*180/3.1415;
    float cosinus = 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;
    motorcounts1=tangens+cosinus;
    //printf("motorcounts1 is %f\r\n", motorcounts1);
    float tangens2 = atan2(yendeffector,(L0-xendeffector))*180/3.1415;
    float cosinus2 = 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;
    //motorcounts2=tangens2+cosinus2;
    float referenceposition;
    float variable;
    variable = motorcounts1/360;
    referenceposition = variable; //this determines the amount of spins
    return referenceposition;
}   

//send value to motor
void sendtomotor(float motorvalue)
{   
    float absolutemotorvalue = 0.0f;
    absolutemotorvalue = fabs(motorvalue);
    absolutemotorvalue = absolutemotorvalue > 1.0f ? 1.0f : absolutemotorvalue;   //if absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
    motor1absolutemotorvalueocity = absolutemotorvalue;

    motor1Direction = (motorvalue > 0.0f); //boolean output: true gives counterclockwise direction, false gives clockwise direction
}

// function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
void measureandcontrol(void)
{
    float referenceposition = getreferenceposition(); 
    measuredposition = getmeasuredposition();
    motorvalue = PID_controller(referenceposition - measuredposition);
    sendtomotor(motorvalue);
}
int main()
{
    pc.baud(115200);
    pc.printf("Starting...\r\n");
    motor1absolutemotorvalueocity.period(period_signal);
    motor.attach(measureandcontrol, timeinterval); 
    while(true)
    {
        wait(0.5);
        pc.printf("number of counts %i\r\n", counts);
        pc.printf("measured position is %f\r\n", measuredposition);
        pc.printf("motorvalue is %f\r\n", motorvalue);
        pc.printf("u_i is %d\r\n", u_i);
        pc.printf("potmeter 1 gives %f\r\n", potmeter1);
        pc.printf("x is %f\r\n",xendeffector);
        pc.printf("y is %f\r\n",yendeffector);
        pc.printf("Kp is %f\r\n", Kp);
        
            
    char a = pc.getc();
    
    if(a == 'r'){
        xendeffector=xendeffector+5;}
        else if(a=='l'){
            xendeffector=xendeffector-5;}
        else if(a=='u'){
            yendeffector=yendeffector+5;}
        else if(a=='d'){
            yendeffector=yendeffector-5;}
            
        
        
        char c = pc.getcNb();
        
        // type c to stop the program
        if (c == 'c')
        {
            pc.printf("Program stopped running");
            motorvalue = 0;
            sendtomotor(motorvalue);
            break;
        }
    }
}