File content as of revision 14:6ecf2b986a4b:

#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);
DigitalIn button(D2);
Serial pc(USBTX,USBRX);
QEI encoder(D12,D13,NC,32);
//HIDScope scope(2);
DigitalOut led(D0);
//*******************Setting tickers and printers*******************
Ticker callMotor;
Ticker angPos;
Ticker t1;
Ticker t2;
Ticker t3;
Ticker myControllerTicker;
Ticker scopeTimer;
//**************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

//***************Global Variables***********************************
const double pi = 3.14159265359;
double m1_ts = 0.01;
const int velocityChannel = 0;
const double transmissionShoulder =94.4/40.2;
const double transmissionElbow = 1.0;
// controller constants 
const double m1_Kp = 0.000048765659063912, m1_Ki = 0.0000228295351674407, m1_Kd = 0.0000255784613247063, m1_N = 54.5397025421619;
double m1_v1 = 0, m1_v2 = 0; // Memory variables
const double m1_Ts = 0.001; // Controller sample time
double v1 = 0;
double v2 = 0;
// position variable
volatile double radians;
//plant variable
volatile double motor1;


//*****************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 PID( 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;
 }


void getAngPosition() 
{   
    volatile int pulses = encoder.getPulses();
    radians = (pulses / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
//     scope.set(velocityChannel,pulses);
//     scope.send();

  
}
  
 // Next task, measure the error and apply the output to the plant
 void motor1_Controller(double radians)
 {
 double reference = 2.0*pi;
 volatile double error1 = reference - radians;
 motor1 = PID( error1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, v1, v2 );
//pc.printf("%d\n\r",motor1);
//scope.set(velocityChannel,motor1);
//scope.send();
pc.printf("error1= %d\n\r",error1);
 }
  
  
void control(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;
        }
}

//****************MAIN FUNCTION*********************************
int main()
{motor1MagnitudePin.period(1.0/1000.0);
t1.attach(&fn1_activate, 0.0001f);
t2.attach(&fn2_activate, 0.0001f);
t3.attach(&fn3_activate, 0.0001f);
pc.baud(115200);
while(true)
{
        if(fn1_go)
        {
        fn1_go = false;
        control(motor1);
        }
        if(fn2_go)
        {
        fn2_go = false;
        motor1_Controller(radians);
        }
        if(fn3_go)
        {
        fn3_go = false;
        getAngPosition();
        }
}
}