File content as of revision 19:da210f89db18:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"
#include "HIDScope.h"

Serial pc(USBTX,USBRX);
HIDScope scope(2);          // definieerd het aantal kanalen van de scope

Ticker          controller1, controller2;        // definieer de ticker die controler1 doet


//MOTOR INPUTPINS
// motor 1
PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
DigitalOut motor1_rich(D7); // digitaal signaal voor richting

// motor 2
PwmOut motor2_aan(D5);
DigitalOut motor2_rich(D4);
// EINDE MOTOR


// ENCODER
Encoder motor1_enc(D12,D11);
Encoder motor2_enc(D10,D9);

//POTMETERS
AnalogIn potright(A0);      
AnalogIn potleft(A1);
AnalogIn gainpot(A5);

// RESETBUTTON
DigitalIn   button(PTA4);
int button_pressed = 0;


// controller stuff
double controlfreq = 50 ;    // controlloops frequentie (Hz)
double controlstep = 1/controlfreq; // timestep derived from controlfreq
const double K1 = 1 ;       // P constant motorcontrolle 1
const double K2 = 1;        // p constant motorcontroller 2

// define storage variables for setpoint values
double c_setpoint1 = 0;
double c_setpoint2 = 0;

// define the maximum rate of change for the setpoint (velocity)
double Vmax = 5;            // rad/sec

// // // // // // // FILTEREN TOEVOEGEN

// tweede orde notch filter 50 Hz
// biquad 1 coefficienten
const double numnotch50biq1_1 = 1;
const double numnotch50biq1_2 = -1.61816178466632;
const double numnotch50biq1_3 = 1.00000006127058;
const double dennotch50biq1_2 = -1.59325742941798;
const double dennotch50biq1_3 = 0.982171881701431;
// biquad 2 coefficienten
const double numnotch50biq2_1 = 1;
const double numnotch50biq2_2 = -1.61816171933244;
const double numnotch50biq2_3 = 0.999999938729428;
const double dennotch50biq2_2 = -1.61431180968071;
const double dennotch50biq2_3 = 0.982599066293075;

// highpass filter 20 Hz coefficienten
const double numhigh20_1 = 0.837089190566345;
const double numhigh20_2 = -1.67417838113269;
const double numhigh20_3 = 0.837089190566345;
const double denhigh20_2 = -1.64745998107698;
const double denhigh20_3 = 0.700896781188403;

// lowpass 5 Hz coefficienten
const double numlow5_1 =0.000944691843840162;
const double numlow5_2 =0.00188938368768032;
const double numlow5_3 =0.000944691843840162;
const double denlow5_2 =-1.91119706742607;
const double denlow5_3 =0.914975834801434;

// Define the storage variables and filter coeicients for two filters
double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0;

double biquadfilter(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;
    }

// FILTERAAR
double EMG_Filter()
{
// double u1 = ..., u2 = ... ;
double u1 = potright.read();
double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
double y4 = abs(y3);
double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
return y5;
}


// // // // // // EINDE FILTERZOOI

double EMG_gain ()
{
    double frac = gainpot.read();
    double EMG_Gain = frac*30;
    return EMG_Gain;
    scope.set(1,EMG_Gain);
    scope.send();
}

// // // // // // EXtra funties
// counts 2 radians
// this function takes the counts from the encoder and converts it to 
// the amount of radians from the zero position.
double get_radians(double counts)       
{
    double pi = 3.14159265359;
    double radians = (counts/4200)*2*pi;
    return radians;
}


// this function takes a 0->1 input (in this case a potmeter and converts it
// to a -2->2 range
double setpoint_f(double input, double &c_setpoint)
{
    double offset = 0.5;            // offset the inputsignal to -0.5->0.5
    double gain = 2;                // increase the signal
    // double potset = (input-offset)*gain;
    double setpoint = c_setpoint + input * controlstep * Vmax ;
        c_setpoint = setpoint;
    return setpoint;
}

// this function is a simple K control called by the motor function
double K_control(double error,double K)
{
    double output = K*error;                            // controller output K*e
    
    // Limit the output to to a number between -1 and 1 because 
    // the motorcode will not handle anything smaller than -1 or larger than 1
    // should be put in own function to improve readability
    if(output>1)                                
    {
        output = 1;
    }
    else if(output < 1 && output > 0)
    {
        output = output;
    }
    else if(output > -1 && output < 0)
    {
        output = output;
    }
    else if(output < -1)
    {
        (output = -1);
    }
    return output;
}

// this function controls the input for one of the electric motors and is called by a ticker
// MOTOR 1
// // // // // // // // // //
void motor1_control()
{
    double input = EMG_Filter()*EMG_gain() ;
    if(input > 1 )
    {
        input = 1;
    }
    else if(input < 0.2)
    {
        input = 0;
    }
    
    double setpoint1 = setpoint_f(input, c_setpoint1);      // determine the setpoint that has been set by the inputsignal
    double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
    double error1 = (setpoint1 - rads1);                       // determine the error (reference - position)
     scope.set(0,input);
    // scope.set(1,rads1);
     scope.send();
    double output1 = K_control(error1, K1);        // bereken de controller output voor motor 1(zie hierboven)
    if(output1 > 0) {                    // 
        motor1_rich.write(0);
        motor1_aan.write(output1);
    } else if(output1 < 0) {
        motor1_rich.write(1);
        motor1_aan.write(abs(output1));
    }
}

// MOTOR 2
// // // // // // //
void motor2_control()
{
    double setpoint2 = setpoint_f(potleft.read(), c_setpoint2);      // determine the setpoint that has been set by the inputsignal
    double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
    double error2 = (setpoint2 - rads2);                       // determine the error (reference - position)
    // scope.set(3,setpoint2);
    // scope.set(4,rads2);
    // scope.send();
    double output2 = K_control(error2, K2);        // bereken de controller output voor motor 1(zie hierboven)
    if(output2 > 0) {                         // determine the direction
        motor2_rich.write(0);
        motor2_aan.write(output2);
    } else if(output2 < 0) {
        motor2_rich.write(1);
        motor2_aan.write(abs(output2));
    }
}


int main()
{
    controller1.attach(&motor1_control, controlstep);
    controller2.attach(&motor2_control, controlstep);
    while(true) 
    {
        if(button.read() == button_pressed)         // reset the counts
        {
            motor1_enc.setPosition(0);
            motor2_enc.setPosition(0);
        }
    }
}