File content as of revision 24:f9dccbc1fc7e:

#include "mbed.h"
#include "FastPWM.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include <math.h>
MODSERIAL pc(USBTX, USBRX);
DigitalOut motor1DirectionPin(D7);
DigitalOut motor2DirectionPin(D4);
DigitalOut Led(LED_GREEN);
FastPWM motor1MagnitudePin(D6);
FastPWM motor2MagnitudePin(D5);
AnalogIn potMeter1(A4);
AnalogIn potMeter2(A5);
InterruptIn button2(D3);
QEI Encoder1 (D12, D13, NC, 64, QEI::X4_ENCODING);
QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING);

//Tickers
Ticker MeasureControl1;
Ticker MeasureControl2;
Ticker print;

//Global variables
volatile double measuredPosition1 = 0.0;
volatile double measuredPosition2 = 0.0;
volatile double referencePosition1 = 0.0;
volatile double referencePosition2 = 0.0;
volatile double motorValue1 = 0.01;
volatile double motorValue2 = 0.01;
volatile double errorM1 = 0.0;
volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
volatile double Kd = 0.0;
volatile double Ts = 0.01;

//------------------------------------------------------------------------------

double GetReferencePosition1()
{
    double potMeter1In = potMeter1.read();
    referencePosition1 = 8.0*3.14*potMeter1In - 4.0*3.14 ; // Reference value y, scaled to -4 to 4 revolutions
    return referencePosition1;
}

double GetReferencePosition2()
{
    double potMeter2In = potMeter2.read();
    referencePosition2 = 8.0*3.14*potMeter2In - 4.0*3.14 ;
    return referencePosition2;
}

double GetMeasuredPosition1()
{
    int counts1 = Encoder1.getPulses();
    double counts1d = counts1*1.0f;
    measuredPosition1 = ( counts1d / (8400)) * 6.28; // Rotational position in radians
    return measuredPosition1;
}

double GetMeasuredPosition2()
{
    int counts2 = Encoder2.getPulses();
    double counts2d = counts2*1.0f;
    measuredPosition2 = ( counts2d / (8400)) * 6.28;
    return measuredPosition2;
}

double FeedbackControl1(double Error1)
{
    static double Error_integral1 = 0;
    static double Error_prev1 = Error1;
    //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
    // Proportional part:
    //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
    double u_k1 = Kp * Error1;
    // Integral part:
    Error_integral1 = Error_integral1 + Error1 * Ts;
    double u_i1 = Ki * Error_integral1;
    // Derivative part
    double Error_derivative1 = (Error1 - Error_prev1)/Ts;
    double u_d1 = Kd * Error_derivative1;
    Error_prev1 = Error1;
    // Sum all parts and return it
    return u_k1 + u_i1 + u_d1; //motorValue
}

double FeedbackControl2(double Error2)
{
    static double Error_integral2 = 0;
    static double Error_prev2 = Error2;
    //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
    // Proportional part:
    //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
    double u_k2 = Kp * Error2;
    // Integral part:
    Error_integral2 = Error_integral2 + Error2 * Ts;
    double u_i2 = Ki * Error_integral2;
    // Derivative part
    double Error_derivative2 = (Error2 - Error_prev2)/Ts;
    double u_d2 = Kd * Error_derivative2;
    Error_prev2 = Error2;
    // Sum all parts and return it
    return u_k2 + u_i2 + u_d2; //motorValue
}

void SetMotor1(double motorValue1)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    // 0 = clockwise motor direction
    // 1 = counterclockwise motor direction
    if (motorValue1 >=0) {
        motor1DirectionPin=0;
    } else {
        motor1DirectionPin=1;
    }
    if (fabs(motorValue1)>1) {
        motor1MagnitudePin = 1;
    } else {
        motor1MagnitudePin = fabs(motorValue1);
    }
}

void SetMotor2(double motorValue2)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    // 0 = counterclockwise motor direction
    // 1 = clockwise motor direction
    if (motorValue2 >=0) {
        motor2DirectionPin=1;
    } else {
        motor2DirectionPin=0;
    }
    if (fabs(motorValue2)>1) {
        motor2MagnitudePin = 1;
    } else {
        motor2MagnitudePin = fabs(motorValue2);
    }
}
//-----------------------------------------------------------------------------
// Tickers
void MeasureAndControl1(void)
{
    // This function determines the desired velocity, measures the
    // actual velocity, and controls the motor with
    // a simple Feedback controller. Call this from a Ticker.
    referencePosition1 = GetReferencePosition1();
    measuredPosition1 = GetMeasuredPosition1();
    errorM1 = referencePosition1 - measuredPosition1;
    motorValue1 = FeedbackControl1(referencePosition1 - measuredPosition1);
    SetMotor1(motorValue1);
}

void MeasureAndControl2(void)
{
    // This function determines the desired velocity, measures the
    // actual velocity, and controls the motor with
    // a simple Feedback controller. Call this from a Ticker.
    referencePosition2 = GetReferencePosition2();
    measuredPosition2 = GetMeasuredPosition2();
    motorValue2 = FeedbackControl2(referencePosition2 - measuredPosition2);
    SetMotor2(motorValue2);
}
void printen()
{
    pc.baud (115200);
    pc.printf("Referenceposition POT1 =  %f \t Referenceposition POT2 = %f \r\n", referencePosition1, referencePosition2);
    pc.printf("Measured position 1 = %f \t Measured position 2 = %f \r\n", measuredPosition1, measuredPosition2);
    pc.printf("Error M1 = %f \r\n", errorM1);
    pc.printf("Motorvalue M1 = %f \r\n", motorValue1);
    //pc.printf("Proportional gain %f \r\n", Kp);
    //pc.printf("Integral gain %f \r\n", Ki);
    //pc.printf("Derivative gain %f \r\n", Kd);
}
//-----------------------------------------------------------------------------
int main()
{
    //Initialize once
    pc.baud(115200);
    motor1MagnitudePin.period_us(60.0); // 60 microseconds PWM period: 16.7 kHz.
    motor2MagnitudePin.period_us(60.0); // 60 microseconds PWM period: 16.7 kHz.
    
    MeasureControl1.attach(MeasureAndControl1, 0.01);
    MeasureControl2.attach(MeasureAndControl2, 0.01);
    print.attach(printen, 1.0);

    //Other initializations

    while(true) {
        Led = !Led;
        wait(2);
    }
}