File content as of revision 1:e10ae03926e6:

// ------------- INITIALIZING -----------------

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

AnalogIn button2(A4);
PwmOut pwmpin1(D6);
DigitalOut directionpin1(D7);
QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
Ticker ref_rot;
Ticker show_counts; 
Ticker Scope_Data;
MODSERIAL pc(USBTX, USBRX);
HIDScope scope(2);

// INITIALIZING GLOBAl VALUES
double Kp = 50; //200
double Ki = 0.5;   //1
double Kd = 10; //200
double Ts = 0.1; // Sample time in seconds
volatile double reference_rotation = 0; //define as radians
double motor_position;
bool AlwaysTrue;

//----------- FUNCTIONS ----------------------


// PID CONTROLLER FUNCTION
double PID_controller(double error)
{
  static double error_integral = 0;
  static double error_prev = error; // 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 * error;

  // Integral part
  error_integral = error_integral + error * Ts;
  double u_i = Ki * error_integral;

  // Derivative part
  double error_derivative = (error - error_prev)/Ts;
  double filtered_error_derivative = LowPassFilter.step(error_derivative);
  double u_d = Kd * filtered_error_derivative;
  error_prev = error;

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


// DIRECTON AND SPEED CONTROL
void moter_control(double u)
{
    directionpin1= u > 0.0f; //eithertrueor false
    pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value            
}


// CONTROLLING THE MOTOR
void Motor_mover()
{
    double motor_position = encoder1.getPulses(); //output in counts
    double error = reference_rotation - motor_position*(2*PI)/8400;
    double u = PID_controller(error);
    moter_control(u);
}

//PRINT TICKER
void PrintFlag()
{
    AlwaysTrue = true;
    }
    
// HIDSCOPE
void ScopeData()
{
    double y = encoder1.getPulses();
    scope.set(0, y);
    scope.send();
    }

//------------------ EXECUTION ---------------

int main()
{
  // INITIALIZING
  
  pwmpin1.period_us(60);
  pc.printf("test");
  pc.baud(9600);
  //show_counts.attach(PrintFlag, 0.2);
  ref_rot.attach(Motor_mover, 0.01);
  Scope_Data.attach(ScopeData, 0.1);
 
 // DEFININING VARIABLES
            //float reference;
            float foo[17];
            float length = 9.0f;
            float b = 8.0f;
            int a;
 
 
  
  // EXECTURION  IN MAIN
        for (a=0; a<10; a++){
            foo[a] = 3.14f/length*(a);
            printf("%f\n\r", foo[a]);
            }
        for(a=10; a<18; a++){
            foo[a] = 3.14f/length*b;
            printf("%f\n\r", foo[a]);
            reference = foo[a];
            b = b-1.0f;
            }        
  
  
    
    while (true) {
        /*
        PrintFlag();
        while (AlwaysTrue){
            float q = encoder1.getPulses();
            pc.printf("%f \r\n", q);
            AlwaysTrue = false;
            wait(0.2f);
            }
            */
           // sin input 
           /*
           if (button2 == false){
            wait (0.2f);
            for (b=0; b<4; b++){
                  for(a=0; a<18; a++){
                    reference_rotation = foo[a];
                    wait(0.1f);
                     }
                    }
                }
                */
            }
            
            
    }