File content as of revision 6:10ac8c7cac26:

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

//Tickers
Ticker TickerMeasureAndControl;
Ticker TickerPrintToScreen;
//Communication
MODSERIAL pc(USBTX,USBRX);
QEI Encoder(D10,D11,NC,32);
QEI Encoder2(D12,D13,NC,32);

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


//Global pin variables Motor 2
PwmOut PwmPin2(D6);
DigitalOut DirectionPin2(D7);
AnalogIn Potmeter2(A0);

//Global variables
volatile bool PrintFlag = false;

//Global variables for printing on screen
volatile float PosRefPrint; // for printing value on screen
volatile float PosMotorPrint; // for printing value on screen
volatile float ErrorPrint;
volatile float PosRefPrint2; // for printing value on screen
volatile float PosMotorPrint2; // for printing value on screen
volatile float ErrorPrint2;

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

//Get reference position 1
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 = 6.2830*ValuePot - 3.1415; // position reference ranging from -1.5 rotations till 1.5 rotations
        
        return PositionRef; //rad
}

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

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

// 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;
}

// actual position of the motor 2
double GetActualPosition2()
{
    //This function determines the actual position of the motor
    //The count:radians relation is 8400:2pi
    double EncoderCounts2 = Encoder2.getPulses();    //number of counts
    double PositionMotor2 = EncoderCounts2/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
    
    return PositionMotor2;
}


//The controller motor 1
double PI_Controller(double Error)
{

   double Ts = 0.01; //Sampling time 100 Hz
   double Kp = 2; // proportional gain
   double Ki = 0.6;
   static double ErrorIntegral = 0;
   
   //Proportional part:
   double u_k = Kp * Error;
   
   //Integral part:
   ErrorIntegral = ErrorIntegral + Error*Ts;
   double u_i = Ki * ErrorIntegral;
   return u_k + u_i; //This will become the MotorValue 
}

//The controller motor 2
double PI_Controller2(double Error)
{

   double Ts = 0.01; //Sampling time 100 Hz
   double Kp = 2; // proportional gain
   double Ki = 0.6;
   static double ErrorIntegral = 0;
   
   //Proportional part:
   double u_k = Kp * Error;
   
   //Integral part:
   ErrorIntegral = ErrorIntegral + Error*Ts;
   double u_i = Ki * ErrorIntegral;
   return u_k + u_i; //This will become the MotorValue2
}

//Ticker function set motorvalues
void SetMotor(double MotorValue, double MotorValue2)
{
    
    // motor 1
    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);
    }
    
    //Motor 2
    
        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)
{
    
    //motor 1
    double PositionRef = GetReferencePosition();
    double PositionMotor = GetActualPosition();
    double MotorValue = PI_Controller(PositionRef - PositionMotor); // input is error
    
    //motor 2
    double PositionRef2 = GetReferencePosition2();
    double PositionMotor2 = GetActualPosition2();
    double MotorValue2 = PI_Controller2(PositionRef2 - PositionMotor2); // input is error
    
    SetMotor(MotorValue,MotorValue2);
    
    //for printing on screen
    PosRefPrint = PositionRef;
    PosMotorPrint = PositionMotor;
    ErrorPrint = PositionRef - PositionMotor;
    PosRefPrint2 = PositionRef2;
    PosMotorPrint2 = PositionMotor2;
    ErrorPrint2 = PositionRef2 - PositionMotor2;
}



void PrintToScreen()
{
    PrintFlag = true;
}


//-----------------------------------------------------------------------------
int main()
{
    pc.baud(115200);
    pc.printf("Hello World\n\r");
    PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
    TickerMeasureAndControl.attach(&MeasureAndControl,0.01); //100 Hz
    TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen
    
    while (true) 
    {
        if(PrintFlag) // if-statement for printing every second four times to screen
        {
            pc.printf("Pos ref1 = %f, Pos motor1 = %f, Error1 = %f, Pos ref2 = %f, Pos motor2 = %f, Error2 = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,PosRefPrint2,PosMotorPrint2,ErrorPrint2);
            PrintFlag = false;
        }
    }
}