File content as of revision 5:cd1c63ffdc1a:

#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"
#include "math.h"
#include "HIDScope.h"
#define M_PI           3.14159265358979323846

DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
PwmOut motor2speed(D5);
DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield)
PwmOut motor1speed(D6);
AnalogIn potmeter1(A3);         //blauw draaiknopje, nr1
AnalogIn potmeter2(A2);         //nr 2
Encoder encoder2(D13,D12,true);
Encoder encoder1(D11,D10,true);
MODSERIAL pc(USBTX,USBRX);

Ticker myControllerTicker1, myControllerTicker2, countStepTickerX, countStepTickerY, buttonStateCheckX, buttonStateCheckY;
bool motor1_PID_Controller_go = false;
bool countStepX_go = false;
bool countStepY_go = false;
bool checkButtonStateX_go = false;
bool checkButtonStateY_go = false;

DigitalIn telknopX(SW2);
DigitalIn telknopY(SW3);
DigitalOut ledX(D2);
DigitalOut ledY(D3);
HIDScope scope(2);

double m1_Kp = 0.0005, m1_Ki = 0.0, m1_Kd = 0.0;
const double m1_Ts = 0.01;
double m1_err_int = 0, m1_prev_err = 0;
double m1_u_prev = 0;

double m2_Kp = 0.0005, m2_Ki = 0.0, m2_Kd = 0.0;
const double m2_Ts = 0.01;
double m2_err_int = 0, m2_prev_err = 0;
double m2_u_prev = 0;

/*
Gain = 0.002356
B = [ 1.00000000000,  2.00000000000,  1.00000000000]
A = [ 1.00000000000, -1.85808607200,  0.86750940236]
*/

const double m1_f_a1 = -1.94042975320, m1_f_a2 = 0.94215346226, m1_f_b0 = 1.0*0.000431, m1_f_b1 = 2.0*0.000431, m1_f_b2 = 1.0*0.000431; 
                                    //dit zijn de constanten die bij de filter horen (de a0 t/m b2) a0 is altijd 1
double m1_f_v1 = 0, m1_f_v2 = 0;    //de variabelen van de filter

const double m2_f_a1 = -1.94042975320, m2_f_a2 = 0.94215346226, m2_f_b0 = 1.0*0.000431, m2_f_b1 = 2.0*0.000431, m2_f_b2 = 1.0*0.000431; 
                                    //dit zijn de constanten die bij de filter horen (de a0 t/m b2) a0 is altijd 1
double m2_f_v1 = 0, m2_f_v2 = 0;    //de variabelen van de filter 

int reference1;
int reference2;
int difference1;
int difference2;
int buttonStateX = 1;
int buttonStateY = 1;
int motorDirectionX = 1;
int motorDirectionY = 1;
int n;
int countsX=0;
int countsY=0;
int countStateX = 1;
int countStateY = 1;

float x = 4.0;      // voorbeeld, moet eigenlijk bepaald worden door EMG signaal
float y = 4.0;      // is het 4de vakje van rechtsonderin de hoek geteld
float xdist_from_board = 4.0;   //er moet 2 van afgetrokken worden. Als de as 6cm van het bord staat moet er 4 worden ingevuld. Telt namelijk
float ydist_from_board = 4.0;   //vanaf het midden van een vakje.
double constante1;
double constante2;
double constante3;

bool buttonReleasedX(DigitalIn& button)
{
    if (button.read() == 1 && buttonStateX == 0) {
        buttonStateX = 1;
        return true;
    }
    buttonStateX = button.read();
    return false;
}

bool buttonReleasedY(DigitalIn& button)
{
    if (button.read() == 1 && buttonStateY == 0) {
        buttonStateY = 1;
        return true;
    }
    buttonStateY = button.read();
    return false;
}

bool buttonHold(DigitalIn& button)
{
    return (button.read() == 0);
}

float bereken_hoek1(float x, float y)
{
    float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2));    //pythagoras
    float alpha = acos((langste_zijde/2)/30);
    return (atan((4*y)/(4*x)) + alpha)*4200/(2*M_PI);       // omrekenen van radialen naar counts = *4200/(2*M_PI), dit moet nog omgerekend worden met de gear ratio van de extra tandwielen.
}

float bereken_hoek2(float x, float y)
{
    float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2));    //pythagoras
    float alpha = acos((langste_zijde/2)/30);
    float beta = 0.5*M_PI - alpha;
    return (2*beta)*4200/(2*M_PI);                          // x wordt hoek voor eerste arm t.o.v. x-as (CW), y hoek voor tweede arm t.o.v. eerste arm (CW)
}

//-------------------------- MOTORCONTROLLERS --------------------------//

void constanteBepalen()
{
    constante1 = potmeter1.read();
    constante2 = potmeter2.read();
    m1_Kd = constante2*0.001;                     //loopt nu van 0 tot 1
    m1_Ki = constante1*0.001;
}

double biquad(double u, double &v1, double &v2, const double a1, const double a2,
    const double b0, const double b1, const double b2){
        double v = u-a1*v1-a2*v2;       //formule uit slides van biquad filter
        double y = b0*v+b1*v1+b2*v2;    //zie ook slides
        v2 = v1; v1 =v;
        return y;
}

double PID(double e, const double Kp, const double Ki, const double Kd, double Ts, 
    double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,
    const double f_a2, const double f_b0, const double f_b1, const double f_b2){    //In de slides staat f_b3, maar volgens mij moet het f_b2 zijn
    //Bereken eerst afgeleide van de error
    double e_der = (e-e_prev)/Ts;                                       
    double e_der2 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);    //de y die de biquad returnt is de gefilterde e_der
    e_prev = e;
    //Bereken nu de geintegreerde versie van de error
    e_int = e_int + Ts*e;
    //PID
    
    return Kp*e+Ki*e_int+Kd*e_der2;  //formule uit slides 
} 

void motor1_PID_Controller()
{
    //reference1 = potmeter1.read()*4200;                     //draaiknop uitlezen, tussen 0 en 1
    float hoek1 = bereken_hoek1(countsX,countsY);
    reference1=hoek1;
    double position =(encoder1.getPosition());              // waarde tussen 0 en 4200
    
    scope.set(0,reference1);
    scope.set(1,position);
    scope.send();
    
    difference1 = reference1 - position;
    constanteBepalen();
    double u = PID( reference1 - position, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int,m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
    if (u < 0) {
        motor1direction = 1;                            //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
    } else if (u>=0) {
        motor1direction = 0;
    }
    motor1speed = fabs(u);
    pc.printf("u = %.2f \r\n", u); 
}

void motor2_PID_Controller()
{
    //reference2 = potmeter2.read()*4200;                     //draaiknop uitlezen, tussen 0 en 1
    float hoek2 = bereken_hoek2(countsX,countsY);
    reference2=hoek2;
    if (reference2 <500) {
        reference2=500;
    }
    if (reference2 > 2100) {
        reference2 = 2100;
    }
    double position =(encoder2.getPosition());              // waarde tussen 0 en 4200
    
    scope.set(2,reference2);
    scope.set(3,position);
    
    difference2 = reference2 - position;
    constanteBepalen();
    double u = PID( reference2 - position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int,m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2 );
    if (u < 0) {
        motor2direction = 1;                            //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
    } else if (u>=0) {
        motor2direction = 0;
    }
    motor2speed = fabs(u);
    pc.printf("u = %.2f \r\n", u); 
}

//-------------------------- POSITIEBEPALING --------------------------//

void checkButtonStateX()
{
    if (buttonReleasedX(telknopX)) {
        countStateX = -countStateX;
    }
}

void checkButtonStateY()
{
    if (buttonReleasedY(telknopY)) {
        countStateY = -countStateY;
    }
}

void countStepX()
{
    if (buttonHold(telknopX)) {
        countsX+= countStateX;             //hier moet de richting dus veranderen
        if(countsX >= 10) {
            countsX = 10;
        } else if(countsX<=0) {
            countsX = 0;
        }
    }
}

void countStepY()
{
    if (buttonHold(telknopY)) {          //X en Y kunnen niet tegelijk optellen, alleen als de ander niet ingedrukt is
        countsY+= countStateY;
        if(countsY >= 10) {
            countsY = 10;
        } else if(countsY<=0) {
            countsY = 0;
        }
    }
}

void motor2_PID_Controller_activate(){ 
    motor2_PID_Controller_go = true; 
}

void countStepX_activate(){
    countStepX_go = true; 
}

void checkButtonStateX_activate(){
    checkButtonStateX_go = true; 
}

void countStepY_activate(){
    countStepY_go = true; 
}

void checkButtonStateY_activate(){
    checkButtonStateY_go = true;
}

int main()
{
    pc.baud(9600);
    encoder1.setPosition(0);
    encoder2.setPosition(0);
    /*myControllerTicker1.attach( &motor1_PID_Controller, 0.005f ); //Ticker roept elke 0.01 s de motor2 controller aan. = 100Hz
    myControllerTicker2.attach( &motor2_PI_Controller, 0.01f);
    countStepTickerX.attach( &countStepX,1.0f );
    countStepTickerY.attach( &countStepY,1.0f);
    buttonStateCheckX.attach( &checkButtonStateX,0.001f);
    buttonStateCheckY.attach( &checkButtonStateY,0.001f);
    */
    myControllerTicker1.attach( &motor2_PID_Controller_activate, 0.01f);   // Function@ hz
    countStepTickerX.attach( &countStepX_activate, 1.0f);                 // Function@ hz
    countStepTickerY.attach( &countStepY_activate, 1.04f);                 // Function@ hz
    buttonStateCheckX.attach( &checkButtonStateX_activate, 0.001f);         // Function@ hz
    buttonStateCheckY.attach( &checkButtonStateY_activate, 0.001f);            // Function@ hz


    while (true) {

        pc.printf(" 1: pos: %d, ref: %d, dif: %d, Ki: %.5f, Kd: %.5f, countsX: %d, countsY: %d \r\n", encoder1.getPosition(), reference1,difference1,m1_Kd,m1_Ki,countsX,countsY);
        //pc.printf(" 2: pos: %d, ref: %d, dif: %d \r\n", encoder2.getPosition(), reference2,difference2);
        
        if(motor2_PID_Controller_go) { 
            motor2_PID_Controller_go = false;
            motor2_PID_Controller();
        }
        if(countStepX_go) {
            countStepX_go = false;
            countStepX();
        }
        if(countStepY_go) {
            countStepY_go = false;
            countStepY();
        }
        if(checkButtonStateX_go) {
            checkButtonStateX_go = false;
            checkButtonStateX();
        }
        if(checkButtonStateY_go) {
            checkButtonStateY_go = false;
            checkButtonStateY();
        }
    }
}