File content as of revision 2:c2ae5044ec82:

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

//states
enum States {Waiting, Homing, Emergency,EMGCal,MotorCal,Operation,Demo};

// Global variables
States CurrentState;
Ticker TickerLoop;
Timer TimerLoop;

//Communication
MODSERIAL pc(USBTX,USBRX);
QEI Encoder(D10,D11,NC,32);

//Global pin variables Motor 1
PwmOut PwmPin(D5);
DigitalOut DirectionPin(D4);
AnalogIn Potmeter1(A1);
AnalogIn Potmeter2(A0);

//Output LED
DigitalOut LedRed (LED_RED);
DigitalOut LedGreen (LED_GREEN);
DigitalOut LedBlue (LED_BLUE);


// Buttons
DigitalIn Button1(SW2);
DigitalIn Button2(SW3);
DigitalIn ButtonHome(D8);


//Global variables for printing on screen
volatile bool PrintFlag = false;
volatile float PosRefPrint; // for printing value on screen
volatile float PosMotorPrint; // for printing value on screen
volatile float ErrorPrint;
volatile int PrintCount = 0;
volatile float MotorValuePrint;

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

//Functions
void StateMachine ()
{
    switch (CurrentState)
    {
        case Waiting:
            LedRed = 0;
            LedGreen = 0;
            LedBlue = 1; //Yellow
        break;
        
        case Homing:
            LedRed = 0;
            LedGreen = 0;
            LedBlue = 0; //White
            if (TimerLoop.read() >= 5.0f)
            {CurrentState = Waiting;}
            
        break;  
        
        case Emergency:
            LedRed = 0;
            LedGreen = 1;
            LedBlue = 1; //Red
        break;
              
        case EMGCal:
            LedRed = 0;
            LedGreen = 1;
            LedBlue = 0; //Pink
        break;
        
        case MotorCal:
            LedRed = 1;
            LedGreen = 0;
            LedBlue = 0; //Turqoise
        break;
              
        case Operation:
            LedRed = 1;
            LedGreen = 0;
            LedBlue = 1; //Green
        break;
        
        case Demo:
            LedRed = 1;
            LedGreen = 1;
            LedBlue = 0; //Blue       
        break;
        

    }
}

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

//Get reference position
double GetReferencePosition()
{
// This function set the reference position to determine the position of the signal.
// a positive position is defined as a counterclockwise (CCW) rotation
    

    
    double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1)
    
        
        double PositionRef = 3*6.2830*ValuePot - 3*3.1415; // position reference ranging from -1.5 rotations till 1.5 rotations
        
        return PositionRef; //rad
}

// actual position of the motor
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;
}



///The controller
double PID_Controller(double Error)
{

   double Ts = 0.005; //Sampling time 100 Hz
   double Kp = 5.34; // proportional gain
   double Ki = 2.0;//integral gain
   double Kd = 6.9;  //derivative 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;
   
   return u_k + u_i + u_d; //This will become the MotorValue
}

//Ticker function set motorvalues
void SetMotor(double MotorValue)
{
    if (MotorValue >=0)
    {
        DirectionPin = 1;
    }
    else
    {
        DirectionPin = 0;
    }
    
    if (fabs(MotorValue)>3) // if error more than 1 radian, full duty cycle
    {
        PwmPin = 1; 
    }
    else
    {
        PwmPin = 0; //fabs(MotorValue);
    }
}

void SetMotorOff()
{
    PwmPin = 0; // Turn motor off
}

//-----------------------------------------------------------------------------
void MeasureAndControl(void)
{
    double PositionRef = GetReferencePosition();
    double PositionMotor = GetActualPosition();
    double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
    SetMotor(MotorValue);
    
    //for printing on screen
    PosRefPrint = PositionRef;
    PosMotorPrint = PositionMotor;
    ErrorPrint = PositionRef - PositionMotor;
    MotorValuePrint = MotorValue;
}



void PrintToScreen()
{
    PrintCount++;
    if (PrintCount == 100) // printing with 2 Hz
    {PrintFlag = true; PrintCount = 0;}
}


// Execution function
void LoopFunction()
{
    // buttons
    if (Button1.read() == false) // if pressed
    {CurrentState = Operation; TimerLoop.reset();}
    
    if (Button2.read() == false) // if pressed 
    {CurrentState = Demo; TimerLoop.reset();}
    
    if (ButtonHome.read() == false) // if pressed
    {CurrentState = Homing; TimerLoop.reset();}
    //functions
    StateMachine(); //determine states
    if (CurrentState >= 4)
    {MeasureAndControl(); PrintToScreen();}
    else 
    {SetMotorOff();}
}

int main()
{
    pc.baud(115200);
    pc.printf("Hi I'm PTR, you can call me Peter!\r\n");
    TimerLoop.start();
    CurrentState = Waiting; // set initial state as Waiting
    TickerLoop.attach(&LoopFunction, 0.002f); //ticker for function calls 500 Hz
    
    while (true)
    {
        if(PrintFlag) // if-statement for printing every second four times to screen
        {

            pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, motorvalue = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,MotorValuePrint);
            PrintFlag = false;
        }        
    }
    
}