File content as of revision 25:039e2b6429ad:

#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 Led1(LED_GREEN);
DigitalOut Led2(LED_GREEN);
FastPWM motor1MagnitudePin(D6);
FastPWM motor2MagnitudePin(D5);
AnalogIn potMeter1(A4);
AnalogIn potMeter2(A5);
InterruptIn button(SW3);
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 Kp2 = 0.34;
volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
volatile double Kd = 0.0;
volatile double Ts = 0.01;

//------------------------------------------------------------------------------
void Stopmotor() {
    // Motor PWM
    motor1MagnitudePin = 0.0f; // Range van 0.0f tot 1.0f
    motor2MagnitudePin = 0.0f;
    
    Led1 = 1;
    Led2 = 0;
}


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;
    // Proportional part:
    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 = Kp2 * 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
    button.fall(Stopmotor);

    while(true) {
        Led2 = 1;
        Led1 = !Led1;
        wait(2);
    }
}