File content as of revision 11:dcc416dbe3ea:

#include "mbed.h"
#include "math.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "BiQuad.h"

//Tickers
Ticker TickerMeasureAndControl;
Timer   TimerLoop;
//Communication
MODSERIAL pc(USBTX,USBRX);
QEI Encoder(D10,D11,NC,32);
QEI Encoder2(D12,D13,NC,32);
//Global pin variables
PwmOut PwmPin(D5);
PwmOut PwmPin2(D6);
DigitalOut DirectionPin(D4);
DigitalOut DirectionPin2(D7);
DigitalIn BUT1(D8);
DigitalIn BUT2(D9);
DigitalIn button_sw2(SW2);
DigitalIn button_sw3(SW3);

DigitalOut LedGreen(LED_GREEN);
DigitalOut LedRed(LED_RED);
DigitalOut LedBlue(LED_BLUE);

double PositionRef=0;
double PositionRef2=0;


//Define substates Operation Mode

enum States {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming};
States CurrentOperationState;

void OperationStateMachine()
{


    switch (CurrentOperationState) {
        case OPWait:
            LedRed = 0; //red
            LedGreen = 1;
            LedBlue = 1;
            if (button_sw2 == false) {
                CurrentOperationState = OPSet;
                TimerLoop.reset();
            }
            break;
            
        case OPSet:
            LedRed = 1; //turqoise
            LedGreen = 0;
            LedBlue = 0;
            if (PositionRef2 > -0.853*(6.380)) {
                PositionRef2 -= 0.0005*(6.380);
                }
            if (PositionRef2 < -0.853*(6.380) && TimerLoop > 2) {
                CurrentOperationState = OPState1;
                TimerLoop.reset();
                }
            break;
                
        case OPState1:
            LedRed = 0; //yellow
            LedGreen = 0;
            LedBlue = 1;
            if (PositionRef < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4
                PositionRef += 0.0005*(6.380);
            }
            if ((PositionRef > 0.48*(6.380)) && (TimerLoop >= 4.0)) {
                CurrentOperationState = OPState2;
                TimerLoop.reset();
            }
            break;
            
        case OPState2:
            LedRed = 0; //white
            LedGreen = 0;
            LedBlue = 0;
            if (PositionRef2 > -0.40*(6.380)) {
                PositionRef2 -= 0.0005*(6.380);
            }
            if ((PositionRef2 < -0.40*(6.380)) && (TimerLoop >= 4.0)) {
                CurrentOperationState = OPState3;
                TimerLoop.reset();
            }
            break;
            
        case OPState3:
            LedRed = 0; //pink
            LedGreen = 1;
            LedBlue = 0;
            if (PositionRef > -0.15*(6.380)) {
                PositionRef -= 0.0005*(6.380);
            }
            if (PositionRef2 > -0.75*(6.380)) {
                PositionRef2 -= 0.0002*(6.380);
            }
            if ((PositionRef < -0.15*(6.380)) && (PositionRef2 < -0.75*(6.380)) && (TimerLoop >= 4.0)) {
                CurrentOperationState = OPState4;
                TimerLoop.reset();
            }
            break;
            
        case OPState4:
            LedRed = 1; //blue
            LedGreen = 1;
            LedBlue = 0;
            if (PositionRef2 > -1.4*(6.380)) {
                PositionRef2 -= 0.005*(6.380);
            }
            if ((PositionRef2 < -1.4*(6.380)) && (TimerLoop > 4)) {
                TimerLoop.reset();
                CurrentOperationState = OPHoming;
            }
            break;
            
        case OPHoming:
            LedGreen = 0; //Green
            LedBlue = 1;
            LedRed = 1;
            if (PositionRef < 0) {
                PositionRef += 0.003*(6.380);
            }
            if (PositionRef2 < 0) {
                PositionRef2 += 0.001*(6.380);
            }
            if ((PositionRef > 0) && (PositionRef2 > 0) && (TimerLoop > 4)) {
                CurrentOperationState = OPWait;
            }
            break;
    }
}




//-----------------------------------------------------------------------------
//The double-functions




// actual position of the motor 1
double GetActualPosition()
{
    //This function determines the actual position of the motor
    //The count:radians relation is 8400:2pi
    double EncoderCounts = Encoder.getPulses();    //number of counts
    double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding

    return PositionMotor;
}

double GetActualPosition2() //motor 2
{
    double Encoder2Counts = Encoder2.getPulses();    //number of counts
    double PositionMotor2 = Encoder2Counts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding

    return PositionMotor2;
}

///The controller
double PID_Controller(double Error)
{
    //Arm drive parameters
    double Ts = 0.002; //Sampling time 100 Hz
    double Kp = 19.8; // proportional gain
    double Ki = 3.98; //Intergral gain
    double Kd = 1.96; //Differential gain.

    static double ErrorIntegral = 0;
    static double ErrorPrevious = Error;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    //Proportional part:
    double u_k = Kp * Error;

    //Integral part:
    ErrorIntegral = ErrorIntegral + Error*Ts;
    double u_i = Ki * ErrorIntegral;

    //Derivative part:
    double ErrorDerivative = (Error - ErrorPrevious)/Ts;
    double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
    double u_d = Kd * FilteredErrorDerivative;
    ErrorPrevious = Error;

    // sum of parts and return it
    return u_k + u_i + u_d; //This will become the MotorValue
}

double PID_Controller2(double Error)
{
    //Belt drive parameters
    double Ts = 0.002; //Sampling time 100 Hz
    double Kp = 11.1; // proportional gain
    double Ki = 2.24; //Integral gain
    double Kd = 1.1; //Differential gain

    static double ErrorIntegral = 0;
    static double ErrorPrevious = Error;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    //Proportional part:
    double u_k = Kp * Error;

    //Integral part:
    ErrorIntegral = ErrorIntegral + Error*Ts;
    double u_i = Ki * ErrorIntegral;

    //Derivative part:
    double ErrorDerivative = (Error - ErrorPrevious)/Ts;
    double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
    double u_d = Kd * FilteredErrorDerivative;
    ErrorPrevious = Error;

    // sum of parts and return it
    return u_k + u_i + u_d; //This will become the MotorValue
}

//Ticker function set motorvalues
void SetMotor(double MotorValue, double MotorValue2)
{
    if (MotorValue >=0) {
        DirectionPin = 1;
    } else {
        DirectionPin = 0;
    }

    if (fabs(MotorValue)>1) { // if error more than 1 radian, full duty cycle
        PwmPin = 1;
    } else {
        PwmPin = fabs(MotorValue);
    }

    if (MotorValue2 >=0) {
        DirectionPin2 = 1;
    } else {
        DirectionPin2 = 0;
    }

    if (fabs(MotorValue2)>1) { // if error more than 1 radian, full duty cycle
        PwmPin2 = 1;
    } else {
        PwmPin2 = fabs(MotorValue2);
    }
}

// ----------------------------------------------------------------------------
//Ticker function
void MeasureAndControl(void)
{

    //double PositionRef = GetReferencePosition();
    //double PositionRef2 = GetReferencePosition2();

    OperationStateMachine();
    double PositionMotor = GetActualPosition();
    double PositionMotor2 = GetActualPosition2();
    double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
    double MotorValue2 = PID_Controller2(PositionRef2 - PositionMotor2);
    SetMotor(MotorValue, MotorValue2);
}



//-----------------------------------------------------------------------------
int main()
{
    LedGreen = 1;
    LedRed = 1;
    LedBlue = 1;
    pc.baud(115200);
    pc.printf("Hello World\n\r");
    TimerLoop.start();
    CurrentOperationState = OPWait;
    PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
    TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz
    while (true) {
    }
}