File content as of revision 16:2083f634c91c:

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

//*****************Defining ports********************
DigitalOut motor1DirectionPin (D4);
PwmOut motor1MagnitudePin(D5);
DigitalOut motor2DirectionPin (D6);
PwmOut motor2MagnitudePin(D7);
QEI encoder_m1(D12,D13,NC,32);
QEI encoder_m2(D10,D11,NC,32);
HIDScope scope(2);
DigitalIn button(D2);
Serial pc(USBTX,USBRX);
//*******************Setting tickers and printers*******************
Ticker angPos1;
Ticker t1;
Ticker t2;
Ticker t3;
Ticker t4;
Ticker t5;
Ticker t6;
Ticker HIDS;


//**************Go flags********************************************
volatile bool fn1_go = false;
void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
volatile bool fn2_go = false;
void fn2_activate(){ fn2_go = true; }; //Activates the go-flag
volatile bool fn3_go = false;
void fn3_activate(){ fn3_go = true; }; //Activates the go-flag
volatile bool fn4_go = false;
void fn4_activate(){ fn4_go = true; }; //Activates the go−flag
volatile bool fn5_go = false;
void fn5_activate(){ fn5_go = true; }; //Activates the go-flag
volatile bool fn6_go = false;
void fn6_activate(){ fn6_go = true; }; //Activates the go-flag
//***************Global Variables***********************************
const double pi = 3.14159265359;
//const double transmissionShoulder =94.4/40.2;
//const double transmissionElbow = 1.0;
// controller constants 
const double m1_Kp =  33760.9402682728, m1_Ki = 17183475.7181081, m1_Kd = 11.5703086491897, m1_N = 14221.4403045604;
const double m2_Kp = 0.000048765659063912, m2_Ki = 0.0000228295351674407, m2_Kd = 0.0000255784613247063, m2_N = 54.5397025421619;
const double m1_Ts = 0.001; // Controller sample time motor 1 
const double m2_Ts = 0.001; // Controller sample time motor 2
double m1_v1 = 0;
double m1_v2 = 0;
double m2_v1 = 0;
double m2_v2 = 0;
// position variable
volatile double radians_m1;
volatile double radians_m2;
volatile double error_m1;
volatile double error_m2;

//plant variable
volatile double motor1;
volatile double motor2;



//*****************Angles Arms***********************

double O1=1.7633;
double O2=2.0915;
double O3=1.8685;
double O4=1.1363;
double O5=2.3960;
double O6=2.0827;
double B1=1.3551;
double B2=0.5964;
double B3=0.06652;
double B4=0.0669;
double B5=1.7462;
double B6=-0.8994;

//**********functions******************************
 double PID1( double err, const double Kp, const double Ki, const double Kd,
 const double Ts, const double N, double &v1, double &v2 ) {
 // These variables are only calculated once!
 const double a1 = (-4.0/(N*Ts+2));
 const double a2 = -(N*Ts-2)/(N*Ts+2);
 const double b0 = (4.0*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
 const double b1 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
 const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);

 double v = err - a1*v1 - a2*v2;
 double u = b0*v + b1*v1 + b2*v2; 
 v2 = v1; v1 = v;
 return u;
 }

 double PID2( double err, const double Kp, const double Ki, const double Kd,
 const double Ts, const double N, double &v1, double &v2 ) {
 // These variables are only calculated once!
 const double a1_2 = (-4.0/(N*Ts+2));
 const double a2_2 = -(N*Ts-2)/(N*Ts+2);
 const double b0_2 = (4.0*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
 const double b1_2 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
 const double b2_2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);

 double v_2 = err - a1_2*v1_2 - a2_2*v2_2;
 double u_2 = b0_2*v_2 + b1_2*v1_2 + b2_2*v2_2; 
 v2_2 = v1_2; v1_2 = v_2;
 return u_2;
 }

void getAngPosition_m1()  //Get angular position motor 1
{   
    volatile int pulses_m1 = encoder_m1.getPulses();
    radians_m1 = (pulses_m1 / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
}

void getAngPosition_m2() //Get angular position motor 2
{   
    volatile int pulses_m2 = encoder_m2.getPulses();
    radians_m2 = (pulses_m2 / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors  
}
  
 // Next task, measure the error and apply the output to the plant
 void motor1_Controller(double radians_m1)
 {
 double reference_m1 = 2*pi;
 volatile double error_m1 = reference_m1 - radians_m1;
 motor1 = PID1( error_m1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, m1_v1, m1_v2 );
 }
 
  // Next task, measure the error and apply the output to the plant
 void motor2_Controller(double radians_m2)
 {
 double reference_m2 = 2*pi;
 volatile double error_m2 = reference_m2 - radians_m2;
 motor2 = PID2( error_m2,m2_Kp,m2_Ki,m2_Kd,m2_Ts, m2_N, m2_v1, m2_v2 );
 
 }
  
  
  
void control_m1(double motor1)
{
if(abs(motor1)>0.000005)
        {
        motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
        }
else
        {
        motor1MagnitudePin=0.0;    
        }
if(motor1<=0)      
        {
        motor1DirectionPin=0.0;
        }
else    {
        motor1DirectionPin=1.0;
        }
}

void control_m2(double motor2)
{
if(abs(motor2)>0.005)
        {
        motor2MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
        }
else
        {
        motor2MagnitudePin=0.0;    
        }
if(motor2<=0)      
        {
        motor2DirectionPin=0.0;
        }
else    {
        motor2DirectionPin=1.0;
        }
}

void print()
{
scope.set(0,radians_m1);
scope.send();  
scope.set(1,radians_m2);
scope.send();    
}

//****************MAIN FUNCTION*********************************
int main()
{HIDS.attach(print,0.001f);
motor1MagnitudePin.period(1.0/1000.0);
motor2MagnitudePin.period(1.0/1000.0);
t1.attach(&fn1_activate, 0.0001f);
t2.attach(&fn2_activate, 0.0001f);
t3.attach(&fn3_activate, 0.0001f);
t4.attach(&fn4_activate, 0.0001f);
t5.attach(&fn5_activate, 0.0001f);
t6.attach(&fn6_activate, 0.0001f);
pc.baud(115200);
while(true)
{
        if(fn1_go)
        {
        fn1_go = false;
        control_m1(motor1);
        }
        if(fn2_go)
        {
        fn2_go = false;
        motor1_Controller(radians_m1);
        }
        if(fn3_go)
        {
        fn3_go = false;
        getAngPosition_m1();
        }
        if(fn4_go)
        {
        fn4_go = false;
        control_m2(motor2);
        }
        if(fn5_go)
        {
        fn5_go = false;
        motor2_Controller(radians_m2);
        }
        if(fn6_go)
        {
        fn6_go = false;
        getAngPosition_m2();
        }

}
}