File content as of revision 4:dfed3b98ffb1:

/**
* Demo program for BiQuad and BiQuadChain classes
* author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion
*/
#include "mbed.h"
//#include "HIDScope.h"
#include <stdlib.h>
#include <iostream>
#include <iomanip>
#include <complex>
#include <vector>
#include "BiQuad.h"
#include "Matrix.h"
#include "MatrixMath.h"
#include "MODSERIAL.h"
#include "encoder.h"

AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );
DigitalIn   button1(D11);
Encoder     encoder1(D13,D12);
Encoder     encoder2(D13,D12); //moet nog even aangepast worden

AnalogIn    potMeterIn1(A1);


DigitalOut motorDirection(D4);
PwmOut motorSpeed(D5);

MODSERIAL pc(USBTX, USBRX);

Ticker      sample_timer;
Ticker      cal_timer;
Ticker      setpoint_timer;
Ticker      m1_Ticker;
Ticker      SP_m1_Ticker;
//HIDScope    scope( 3 );
DigitalOut  led(LED1);
DigitalOut  led2(LED_GREEN);
const int size = 100;
vector<double> S(size,0);
double meanSum = 0;

double maxsignal = 0;

double emgX = 0;
double emgY = 1;

double L0 = 0.123;
double L1 = 0.37;
double L2 = 0.41;
double q1 = encoder1.getPosition()/131; //Calibreren nog toevoegen
double q2 = encoder2.getPosition()/131; //Calibreren mist nog
double Periode = 0.002;   //1000 is het aantal Hz


float M1_KP = 2.5;
float M1_KI = 1.0;
const float M1_TS = 0.01;
const float SP_TS = 0.01;
const float RAD_PER_PULSE = 0.000119;
float m1_err_int = 0;
int motorD = 0;
float motor1 = 0;
float reference = 0;
float position = 0;
int outOfEncod = 0;
int KP_changer = 1;
float RotSpeed = 0;


//Making matrices globaly
Matrix      JAPPAPP(2,2);
Matrix      qdot(2,1);
Matrix      Vdes(2,1);
Matrix      qsetpoint(2,1);

//Filter toevoegen, bestaande uit notch, high- en lowpass filter
BiQuadChain Notch;
BiQuadChain Notch50;
BiQuadChain bqcLP;
BiQuadChain bqc;
//50 Hz Notch filter, for radiation from surroundings

//Notch filter chain for 100 Hz
BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 );
BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 );
BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 );


//Notch filter chain for 50 Hz
BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 );
BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 );
BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 );

//2Hz Highpass
BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01);

//80 Hz Lowpass
BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 );
BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 );



//450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...)
//BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01);

float PI( float e, const float Kp, const float Ki, float Ts, float &e_int )
{
    e_int += Ts * e;                                                                // e_int is changed globally because it’s ’by reference’ (&)
    if (fabs(Kp * e + Ki * e_int) < 1) {
        return fabs(Kp * e + Ki * e_int);
    } else {
        return 1;
    }
}
// Next task, measure the error and apply the output to the plant
void m1_Controller()
{
    reference = 5 * potMeterIn1.read();
    //reference = qsetpoint(1,1);
    outOfEncod = encoder1.getPosition();
    position = RAD_PER_PULSE * outOfEncod;
    float ref_pos = reference - position;                                          // Don’t use magic numbers!
    //motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );

    if (ref_pos <= 0) {                                                             // Don’t use magic numbers!
        motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
    } else {
        motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
    }
    if (ref_pos <= 0) {
        //float motor1DirectionPin1 = 1;
        motorD=1;
    } else {
        //float motor1DirectionPin1 = 0;
        motorD=0;
    }
    motorDirection.write(motorD);
    motorSpeed.write(motor1);
}


double findRMS(vector<double> array)
{
    int i;
    double sumsquared = 0.000;
    double RMS;
    //sumsquared = 0;
    for (i = 0; i < size; i++) {
        sumsquared = sumsquared + array.at(i)*array.at(i);
    }
    RMS = sqrt((double(1)/size)*(sumsquared));
    return RMS;
}



void sample()
{
    S.erase(S.begin());
    double b = bqc.step(emg0.read()-0.4542);
    S.push_back(b);
    emgX = findRMS(S);

    led = !led;

    //motorSpeed.write(0.38080*(emgX/maxsignal));
    //motorDirection.write(1);
}


void qSetpointSet()
{
    // Fill Matrix with data.
    JAPPAPP(1,1) =  -(L0 - L0*sin(q1) + L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
    JAPPAPP(1,2) = -(L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
    JAPPAPP(2,1) = (L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
    JAPPAPP(2,2) = (L1*cos(q1) + L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));

    // Fill Matrix with data.
    Vdes(1,1) = emgX;        //Goede code nog toevoegen, Vx
    Vdes(2,1) = emgY;        //goede code nog toevoegen, Vy

    qdot = JAPPAPP*Vdes;

    qsetpoint(1,1) = q1 + qdot(1,1)*Periode;
    qsetpoint(2,1) = q2 + qdot(2,1)*Periode;

    //motorSpeed.write(qsetpoint(1,1));
    //motorDirection.write(1);
}


int main()
{
    pc.baud(115200);

    //Constructing the notch filter chain
    Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 );
    Notch50.add( &bq1 ).add( &bq2 ).add( &bq3 );
    bqcLP.add( &bqLP1 ).add( &bqLP2 );
    bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 );
    /**Attach the 'sample' function to the timer 'sample_timer'.
    * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz
    */
    sample_timer.attach(&sample, 0.002);
    setpoint_timer.attach(&qSetpointSet, 0.002);
    led2 = 1;
    /*empty loop, sample() is executed periodically*/
    m1_Ticker.attach( &m1_Controller, M1_TS );

    m1_Controller();

    while(true) {
        pc.printf("%f \t",emgX);
        pc.printf("%f \t",qdot(1,1));
        pc.printf("%0.8f \t",qsetpoint(1,1));
        pc.printf("%f \t",encoder1.getPosition());
        pc.printf("%f \t",motorD);
        pc.printf("%f \t",emgX);
        pc.printf("%f \r\n",reference);

    }

}