File content as of revision 15:97311058e012:

#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 q2Ref =0;


//Define substates Operation Mode

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

//State Machine for OperationMode
void OperationStateMachine()
{
    switch (CurrentOperationState) {
        case OPWait:
            if (EMGBoolLeft == true) {    //When Left Biceps are contracted, initiate chain to flip the page
                CurrentOperationState = OPState1;
                TimerLoop.reset();
            }
            if (BUTSW3 == false) {  //When sw3 is pressed, go to homing for Operation Mode
                CurrentOperationState = OPSet;
                TimerLoop.reset();
            }
            break;
            
        case OPSet:                 // Set new homing for Operation Mode and go back to waiting mode
            if (q2Ref  > -0.733*(6.380)) {
                q2Ref  -= 0.0005*(6.380);
                }
            if (q2Ref  < -0.733*(6.380) && TimerLoop > 2) {
                CurrentOperationState = OPWait;
                TimerLoop.reset();
                }
            break;
                
        case OPState1:                      // First step of chain to flip the page
            if (q1Ref < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4
                q1Ref += 0.0005*(6.380);
            }
            if ((q1Ref > 0.48*(6.380)) && (TimerLoop >= 2.0)) {
                CurrentOperationState = OPState2;
                TimerLoop.reset();
            }
            break;
            
        case OPState2:                      //Second step of chain to flip the page
            if (q2Ref  > -1.133*(6.380)) {
                q2Ref  -= 0.0005*(6.380);
            }
            if ((q2Ref  < -1.133*(6.380)) && (TimerLoop >= 2.0)) {
                CurrentOperationState = OPState3;
                TimerLoop.reset();
            }
            break;
            
        case OPState3:                  //Third step of chain to flip the page
            if (q1Ref > -0.15*(6.380)) {
                q1Ref -= 0.0005*(6.380);
            }
            if (q2Ref  > -1.483*(6.380)) {
                q2Ref  -= 0.0003*(6.380);
            }
            if ((q1Ref < -0.15*(6.380)) && (q2Ref  < -1.483*(6.380)) && (TimerLoop >= 3.0)) {
                CurrentOperationState = OPState4;
                TimerLoop.reset();
            }
            break;
            
        case OPState4:                          //Fourth step of chain to flip the page
            if (q2Ref  > -2.133*(6.380)) {
                q2Ref  -= 0.005*(6.380);
            }
            if ((q2Ref  < -2.133*(6.380)) && (TimerLoop > 0.5)) {
                TimerLoop.reset();
                CurrentOperationState = OPHoming;
            }
            break;
            
        case OPHoming:                          //Go back to Home for Operation Mode and back to Waiting
            if (q1Ref < 0) {
                q1Ref += 0.003*(6.380);
            }
            if (q2Ref  < -0.733*(6.380)) {
                q2Ref  += 0.001*(6.380);
            }
            if ((q1Ref > 0) && (q2Ref  > -0.733*(6.380)) && (TimerLoop > 3)) {
                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 q1Ref = GetReferencePosition();
    //double q2Ref  = GetReferencePosition2();

    OperationStateMachine();
    double PositionMotor = GetActualPosition();
    double PositionMotor2 = GetActualPosition2();
    double MotorValue = PID_Controller(q1Ref - PositionMotor); // input is error
    double MotorValue2 = PID_Controller2(q2Ref  - 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) {
    }
}