File content as of revision 6:81dda0c8eb71:

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

MODSERIAL pc(USBTX, USBRX);
QEI motor1 (D8, D9, NC, 32);
FastPWM motor1_pwm(D5);
DigitalOut motor1_dir(D4);
AnalogIn pod1(A0);
AnalogIn pod2(A1);

Ticker measure;

int m1_count; 
float m1_angle;
float pi = 3.14;
float pod1_angle;
float motor1_er;
float pod2_tick;
float PWM_m1;

//PID variables
bool q = true;
float error_prev;
float Kp = 8;
float Ki = 1.02;
float Kd = 0.1;
float Ts = 0.01;
float u;
float u_p;
float u_i;
float u_d;

static double error_integral = 0;
static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

//initial parameters time and HIDSCOPE
volatile float theoretical_time = 0;
static int frequency_theoretical_time = 20;
static float duration_theoretical_time = (1/(float)frequency_theoretical_time);
Ticker sample_timer;    //HIDSCOPE ticker, deze is langzamer dan en in periode met measure,
                        //deze frequentie is frequency_time
HIDScope scope( 2 );    //2 ingangen

/** Sample function
 * this function samples the emg and sends it to HIDScope
 **/
void sample()
{
    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    scope.set(0, m1_count );
    scope.set(1,  theoretical_time);
    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
    *  Ensure that enough channels are available (HIDScope scope( 2 ))
    *  Finally, send all channels to the PC at once */
    scope.send();
    /* To indicate that the function is working, the LED is toggled */
    //led = !led; suppressed for now
}

/** Theoretical time function
*This function calculates time based on sample numbers and frequency
**/
void theoretical_time_fun(){
theoretical_time = theoretical_time + duration_theoretical_time; //increment by 1 second
}
void StepResponse(){
theoretical_time_fun();
sample();
}

void getCounts(){
    m1_count = motor1.getPulses();
    m1_angle = (float)m1_count / 4200.0f * (2.0f * pi);
    pod1_angle = (float)pod1 * 2.0f * pi;
    pod2_tick = (float)pod2;
    }
    
void error(void){
    motor1_er = pod1_angle - m1_angle;
    }
    
void PID_control(void){
    //P action
    u_p = Kp * fabs(motor1_er);
    
    //I action 
    error_integral = error_integral + motor1_er * Ts;
    u_i = Ki * error_integral;
    
    //D action
    if(q){
        error_prev = motor1_er;
        q=false;
        }
        
    float error_derivative = (motor1_er - error_prev)/Ts;
    float filtered_error_derivative = LowPassFilter.step(error_derivative);
    u_d = Kd * filtered_error_derivative;
    error_prev = motor1_er;
    
    u = u_p + u_i + u_d;
    
    PWM_m1 = fabs(motor1_er * u);
    }
    
void control(){
    if(fabs(motor1_er) < 0.005f){
        motor1_pwm.write(0);
    }else if(motor1_er < 0.0f){
            motor1_pwm.write(PWM_m1);
            motor1_dir = 1;
    }else if(motor1_er > 0.0f){
            motor1_pwm.write(PWM_m1);
            motor1_dir = 0;   
            }
    }
    
void MeasureandControl(){
    getCounts();
    error();
    control();
    PID_control();
    }

int main()
{
    pc.baud(115200);
    pc.printf("\r\nStarting up...\r\n\r\n");
    motor1_pwm.period(1.0/10000);
    motor1_dir = 0;
    measure.attach(MeasureandControl,0.01);
    sample_timer.attach(StepResponse,duration_theoretical_time);
    while(true){
        pc.printf("\r\nMotor angle: %f Pod angle: %f Error: %f \r\n u = %f , u_d = %f",m1_angle,pod1_angle,motor1_er,u,u_d);
        wait(0.8);
    }
}