File content as of revision 21:9e6461109547:

#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
#include "biquadFilter.h"
 
Serial pc(USBTX, USBRX); // tx, rx
DigitalIn Button(SW2);
DigitalOut LedR(LED_RED);
DigitalOut LedG(LED_GREEN);
DigitalOut LedB(LED_BLUE);
DigitalOut motor2direction(D7); //D6 en D7 zijn motor 2 (op het motorshield)
PwmOut motor2speed(D6);
AnalogIn potmeter2(A1);
QEI Encoder(D12, D13, NC, 64, QEI::X4_ENCODING);
HIDScope scope(3);
 
Ticker ScopeTime;
Ticker myControllerTicker;
 
int q = 0 ;
double P2 ;
double m_ref = 0 ;
double reference = 0 ;
double position ;
double h_m = 0 ;
int count = 0 ;
int f_motor = 100 ;
int t =0 ;
int x1 = -1 ;
int x2 = -1 ;
 
void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
{
    scope.set(0, motor2direction.read());
    scope.set(1, motor2speed.read());
    scope.set(2, position);
 
    scope.send();
    
}
// Sample time (motor2-step)
const double m2_Ts = 0.01;
 
// Controller gain
const double m2_Kp = 0.5,m2_Ki = 0.005, m2_Kd = 0.5;
double m2_err_int = 0, m2_prev_err = 0;
 
//Derivative filter coeffs
const double BiGain = 0.016955;
const double m2_f_a1 = -0.96608908283*BiGain, m2_f_a2 = 0.0*BiGain, m2_f_b0 = 1.0*BiGain, m2_f_b1 = 1.0*BiGain, m2_f_b2 = 0.0*BiGain;
 
// Filter variables
double m2_f_v1 = 0, m2_f_v2 = 0;
 
// Biquad filter
double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1; v1 = v;
    return y;
}    
 
// Reusable PID controller
double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2,
const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
{
    // Derivative
    double e_der = (e-e_prev)/Ts;
    e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
    e_prev = e;
    // Integral
    e_int = e_int + Ts*e;
    // PID
    return Kp * e + Ki*e_int + Kd*e_der;
}
 
void m2_ref()
{
    double pi = 3.14 ;
    double t_m = 1 ;
    double start1 ;
    double start2 ; 
        
    switch (q)
    {
    case 1 :                                // clockwise
                   
    start1 = x1*h_m - (x2+1)*h_m ;                  
    if (t >= 0 & t <= f_motor) {
        reference = start1 + (h_m* t_m *t)*1/f_motor - h_m/(2*pi)*sin(2*pi/t_m*t/f_motor) ;
        t++ ; }
    else if (t > f_motor){
        motor2speed.write(0.0) ; }
        
    pc.printf("\r\n t = %i reference1 = %f pos1 = %f ",t,reference,position) ;
    break ;
    
    case 2  :                               // counterclockwise
    
    start2 = (x1+1)*h_m - x2*h_m ;    
    if (t >= 0 & t <= f_motor) {
        reference = start2 - ((h_m* t_m *t)*1/f_motor - h_m/(2*pi)*sin(2*pi/t_m*t/f_motor)) ;
        t++ ; }
    else if (t > f_motor){
        motor2speed.write(0.0) ; }
        
    pc.printf("\r\n start1 = %i reference2 = %f pos2 = %f ",start1,reference,position) ; 
    break ;    
    }  
}    

// Motor2 control
void motor2_Controller() 
{
    m2_ref() ;
    // Setpoint
    double pulses = Encoder.getPulses();
    position = pulses*360/(64*131.25); // Aantal Degs
    P2 = PID( reference - position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
    m2_f_b0, m2_f_b1, m2_f_b2);
 
    if (P2 > 0.6)
    {   P2 = 0.6 ; }
    else if (P2 < 0.6)
    {   P2 = -0.6 ; }
    
    const int frequency_pwm = 20000 ;
    motor2speed.period(1/frequency_pwm) ;
    motor2speed = abs(P2); // Speed control   
    
    if (P2 > 0) // Direction control
    {  
        motor2direction = 0;
    }
    else if (P2 < 0)
    {
        motor2direction = 1;
    }
}
 
int main()
{   
    pc.baud(115200);
        
    ScopeTime.attach_us(&ScopeSend, 10e4);
    myControllerTicker.attach( &motor2_Controller, 0.01); // 100 Hz
    
    while(true)
    {
       char c = pc.getc();
       if(c == 'r')
        {
           q = 1 ;              // case 1
           t = 0 ;
           h_m = 3 ;
           x1++ ;
        }
        if(c == 'f')
        {
           q = 2 ;              // case 2
           t = 0 ;
           h_m = 3 ;
           x2++ ;
        }        
    }
}