File content as of revision 1:5681fcdc22fe:

#include "mbed.h"
#include "encoder.h"

Ticker timer;
const double PI = 3.141592653589793;
const double RAD_PER_PULSE = 0.000476190;
float CONTROLLER_TS = 0.01;


//Motor1
PwmOut motor1(D5);
DigitalOut motor1DirectionPin(D4);
DigitalIn ENC2A(D12);
DigitalIn ENC2B(D13);
Encoder encoder1(D13,D12);
AnalogIn potmeter1(A3);
const double MOTOR1_KP = 0.5;
const double MOTOR1_KI = 1.5;
double m1_err_int = 0;
const double motor1_gain = 2*PI;


//Motor2
PwmOut motor2(D6);
DigitalOut motor2DirectionPin(D7);
DigitalIn ENC1A(D10);
DigitalIn ENC1B(D11);
Encoder encoder2(D10,D11);
AnalogIn potmeter2(A4);
const double MOTOR2_KP = 0.5;
const double MOTOR2_KI = 1.5;
double m2_err_int = 0;
const double motor2_gain = 2*PI;


float getreferenceangle(const double PI, double potmeter)
{
    float max_angle = 2*PI;
    float  reference_angle = max_angle * potmeter;
    return reference_angle;    
}

double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) 
{
    e_int =+ Ts * error;
    return Kp * error + Ki * e_int ;
}

void motor1_control(const double motor1_gain)
{
    double referenceangle1 = getreferenceangle(PI, potmeter1);
    double position1 = RAD_PER_PULSE * encoder1.getPosition();
    double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain;
    motor1 = fabs(magnitude1);
    
    if (magnitude1 < 0)
    {
        motor1DirectionPin = 1;
    }
    else
    {
        motor1DirectionPin = 0;
    }
}

void motor2_control(const double motor2_gain)
{
    double referenceangle2 = getreferenceangle(PI, potmeter2);
    double position2 = RAD_PER_PULSE * encoder2.getPosition();
    double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain;
    motor2 = fabs(magnitude2);
    
    if (magnitude2 < 0)
    {
        motor2DirectionPin = 1;
    }
    else
    {
        motor2DirectionPin = 0;
    }
}


int main()
{
   while(1) 
   {
     motor1_control(motor1_gain);
     wait(0.005f);
     motor2_control(motor2_gain);
     wait(0.005f);
    }
     
   
           
}