File content as of revision 26:8fc7e88603f8:

#include "mbed.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include <math.h>

//ATTENTION:    set mBed to version 151
//              set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
//              set MODSERIAL to version 44
//              set HIDScope to version 7
//              set biquadFilter to version 7

InterruptIn encoderA        (D9);
InterruptIn encoderB        (D8);

DigitalOut directionpin1    (D7);
DigitalOut directionpin2    (D4);

DigitalIn  button2          (D10);

PwmOut pwmpin1              (D6);
PwmOut pwmpin2              (D5);


MODSERIAL pc(USBTX, USBRX);
//HIDScope    scope( 3 );                             //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered


//Global variables
int encoder = 1;                //Start value encoder

double T = 0.01f;                 //Ticker period

double Kp = 17.5;              //Motor 1
double Ki = 1.02;
double Kd = 23.2;
double PI = 3.14159;

//double err = 0;

volatile double reference_rotation = PI;   //pi , in radians
double encoder_radians;

//double u_total = 0;

double i=0;

//Tickers
Ticker encoder_tick;
Ticker engine_tick;
   

//Functions
    
/*
void HIDScope_sample()
{
    scope.set(0,sinout);
    scope.set(1,encoder);                   //alles in array?
    scope.set(2,err);
    
    scope.send();
}
*/
    

void encoderA_rise()       
{
    if(encoderB==false)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}

void encoderA_fall()      
{
    if(encoderB==true)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}

void encoderB_rise()       
{
    if(encoderA==true)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}

void encoderB_fall()      
{
    if(encoderA==false)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}

void encoder_count()
{
    encoderA.rise(&encoderA_rise);
    encoderA.fall(&encoderA_fall);
    encoderB.rise(&encoderB_rise);
    encoderB.fall(&encoderB_fall);
}

double PID_controller(double err)
{
  static double err_integral = 0;
  static double err_prev = err; // initialization with this value only done once!
  
  static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

  // Proportional part:
  double u_k = Kp * err;

  // Integral part
  err_integral = err_integral + err * T;
  double u_i = Ki * err_integral;

  // Derivative part
  double err_derivative = (err - err_prev)/T;
  double filtered_err_derivative = LowPassFilter.step(err_derivative);
  double u_d = Kd * filtered_err_derivative;
  err_prev = err;

  // Sum all parts and return it
  return u_k + u_i + u_d;  
}

void start_your_engines(double u)
{
    if(encoder<5000 && encoder>-5000)              //limieten van de translatie 11760 encoder counts
    {
         pwmpin1 = fabs(u);               //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
         directionpin1.write(u < 0.0f);
    }
    else
    {
        pwmpin1 = 0;
    }
}  

void engine_control()
{
    encoder_radians = encoder*(2*PI)/8400;
    double err = reference_rotation - encoder_radians;
    double u = PID_controller(err);                             //PID controller function call
    start_your_engines(u);                                      //Call start_your_engines function
}

// Main function start.

int main()
{ 
        pc.baud(115200);
        pc.printf("hello\n\r");
        pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz 

        encoder_tick.attach(&encoder_count,T);
        engine_tick.attach(&engine_control,T);
        //HIDScope ticker toevoegen
        
// DEFININING VARIABLES

//float reference;
    float foo[17];
    float length = 9.0f;
    int a;
    int q;

while (true) 
{

    // EXECTURION  IN MAIN
        // SIN ALS INPUT GEVEN
        if (button2 == false)
        {
            wait (0.2f);
            
            for (q=0; q<4; q++)
            {
                float b = 8.0f;
                    
                    for (a=0; a<10; a++)
                    {
                        foo[a] = 3.14f/length*(a);
                        //printf("%f\n\r", foo[a]);
                        reference_rotation = foo[a];
                        wait(0.2f);
                    }
            
                    for(a=10; a<18; a++)
                    {
                        foo[a] = 3.14f/length*b;
                        //printf("%f\n\r", foo[a]);
                        reference_rotation = foo[a];
                        b = b-1.0f;
                        wait(0.2f);
                    }
            }

            reference_rotation = 0;
        
        }   

}
}