File content as of revision 2:afa5a01ad84b:

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

// in gebruik: D(0(TX),1(RX),4(motor2dir),5(motor2pwm),6(motor1pwm),7(motor1dir),
//8(pushbutton),9(servoPWM),10(encoder),11(encoder),12(encoder),13(encoder)) A(0,1,2)(emg)

MODSERIAL pc(USBTX, USBRX);
HIDScope scope(6); // the amount of scopes to send to the pc

//Define objects

//Define the EMG inputs
AnalogIn    emg1( A0 );
AnalogIn    emg2( A1 );
AnalogIn    emg3( A2 );

//Button used to calibrate the threshold values
DigitalIn button(PTC6);

//Define motor outputs
DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
FastPWM motor1(D6);     // speed of motor 1
FastPWM motor2(D5);     //speed of motor 2
DigitalOut motor2dir(D4);   //direction of motor 2, attach at m2, set to 0: ccw
FastPWM servo(D9);   //servo pwm
DigitalIn servo_button(PTC12); // servo flip

QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder

//Define the Tickers
Ticker      pos_timer;                      // the timer which is used to print the position every second
Ticker      sample_timer;                   // the timer which is used to decide when a sample needs to be taken
Ticker      control;                        // Ticker for processing encoder input to motor output
Ticker      servo_control;                          // Ticker for calling servo_control

//Initialize all variables
volatile bool sample_go = false;          // go flag sample()
volatile bool controller_go=false;          // go flag controller()
volatile bool servo_go=false;               // go flag servo_controller()


double highest_emg1;                        // the highest emg signal of emg input 1
double highest_emg2;                        // the highest emg signal of emg input 2
double highest_emg3;                        // the highest emg signal of emg input 3
double threshold1;                          // the threshold which the first emg signal needs to surpass to do something
double threshold2;                          // the threshold which the second emg signal needs to surpass to do something
double threshold3;                          // the threshold which the third emg signal needs to surpass to do something
double samplefreq=0.002;                    // every 0.002 sec a sample will be taken this is a frequency of 500 Hz
double emg11;                               // the first emg signal
double emg21;                               // the second emg signal
double emg31;                               // the third emg signal
double ref_x=0.0000;                        // the x reference position
double ref_y=0.0000;                        // the y reference position
double old_ref_x;                           // the old x reference
double old_ref_y;                           // the old y reference
double speed=0.00008;                       // the variable with which a speed is reached of 1cm/s
double theta=0.0;                           // angle of the arm
double radius=0.0;                          // radius of the arm

const double minRadius=0.43;                 // minimum radius of arm
const double maxRadius=0.62;                 // maximum radius of arm
const double min_y=-0.26;                   // minimum height which the spatula can reach
char key;                                   // variable to place the keyboard input

double m1_pwm=0;    //variable for PWM control motor 1
double m2_pwm=0;    //variable for PWM control motor 2

const double m1_Kp = 35.16, m1_Ki = 108.8, m1_Kd = 2.84, m1_N = 100; // controller constants motor 1
double m1_v1 = 0, m1_v2 = 0; // Memory variables
const double m1_Ts = 0.01; // Controller sample time

const double m2_Kp = 36.24, m2_Ki = 108.41, m2_Kd = 3.03, m2_N = 100; // controller constants motor 2
double m2_v1 = 0, m2_v2 = 0; // Memory variables
const double m2_Ts = 0.01; // Controller sample time

const double pi=3.14159265359;
const double res = 64.0/(1.0/131.25*2.0*pi);     // resolution on gearbox shaft per pulse
const double V_max=9.0;                    // maximum voltage supplied by trafo
const double pulleyRadius=0.0398/2.0;        // pulley radius

double servo_pwm=0.0023; // duty cycle 1.5 ms 7.5%, min 0.5 ms 2.5%, max 2.5 ms 12.5%
const double minTheta=-38.0/180.0*pi;        //minimum angle robot
const double maxTheta=-24.0/180.0*pi;        // maximum angle to which the spatula can stabilise
const double diffTheta=maxTheta-minTheta;  //difference between max and min angle of theta for stabilisation
const double min_servo_pwm=0.00217;   // corresponds to angle of theta -32 degrees
const double max_servo_pwm=0.0023;    // corresponds to angle of theta -43 degrees
const double res_servo=max_servo_pwm-min_servo_pwm;  //resolution of servo pwm signal between min and max angle
const double servo_Ts=0.02;
bool z_push=false;

//Define the needed Biquad chains
BiQuadChain bqc11;
BiQuadChain bqc13;
BiQuadChain bqc21;
BiQuadChain bqc23;
BiQuadChain bqc31;
BiQuadChain bqc33;

//Define the BiQuads for the filter of the first emg signal
//Notch filter
BiQuad bq111(0.9795,   -1.5849,    0.9795,    1.0000,   -1.5849,    0.9589);
BiQuad bq112(0.9833,   -1.5912,    0.9833,    1.0000,   -1.5793,    0.9787);
BiQuad bq113(0.9957,   -1.6111,    0.9957,    1.0000,   -1.6224,    0.9798);
//High pass filter
//BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
BiQuad bq121( 0.8956,   -1.7911,    0.8956,    1.0000,   -1.7814,    0.7941);
BiQuad bq122( 0.9192,   -1.8385,    0.9192,    1.0000,   -1.8319,    0.8450);
BiQuad bq123( 0.9649,   -1.9298,    0.9649,    1.0000,   -1.9266,    0.9403);
//Low pass filter
BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );

//Define the BiQuads for the filter of the second emg signal
//Notch filter
BiQuad bq211 = bq111;
BiQuad bq212 = bq112;
BiQuad bq213 = bq113;
//High pass filter
BiQuad bq221 = bq121;
BiQuad bq222 = bq122;
BiQuad bq223 = bq123;
//Low pass filter
BiQuad bq231 = bq131;

//Define the BiQuads for the filter of the third emg signal
//notch filter
BiQuad bq311 = bq111;
BiQuad bq312 = bq112;
BiQuad bq313 = bq113;
//High pass filter
BiQuad bq321 = bq121;
BiQuad bq323 = bq122;
BiQuad bq322 = bq123;
//low pass filter
BiQuad bq331 = bq131;

void sampleflag()
{
    if (sample_go==true) {
        // this if statement is used to see if the code takes too long before it is called again
        pc.printf("rate too high error in sampleflag\n\r");
    }
    //This sets the go flag for when the function sample needs to be called
    sample_go=true;
}

void activate_controller()
{
    if (controller_go==true) {
        // this if statement is used to see if the code takes too long before it is called again
        pc.printf("rate too high error in activate_controller()\n\r");
    }
    controller_go=true;   //activate go flag
}

void activate_servo_control()
{
    if (servo_go==true) {
        pc.printf("error servo");
    }
    servo_go=true;   //activate go flag
}

void sample()
{
    //This checks if a key is pressed and changes the variable key in the pressed key
    if (pc.readable()==1) {
        key=pc.getc();
    }
    //Read the emg signals and filter it

    emg11=bqc13.step(fabs(bqc11.step(emg1.read())));    //filtered signal 1
    emg21=bqc23.step(fabs(bqc21.step(emg2.read())));    //filtered signal 2
    emg31=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 3

    //remember what the reference was
    old_ref_x=ref_x;
    old_ref_y=ref_y;
    //look if the emg signals go over the threshold and change the reference accordingly
    if (emg11>threshold1&&emg21>threshold2&&emg31>threshold3 || key=='d') {
        ref_x=ref_x-speed;
        ref_y=ref_y-speed;

    } else if (emg11>threshold1&&emg21>threshold2 || key == 'a' || key == 'z') {
        ref_x=ref_x-speed;

    } else if (emg11>threshold1&&emg31>threshold3 || key == 's') {
        ref_y=ref_y-speed;

    } else if (emg21>threshold2&&emg31>threshold3 || key == 'e' ) {
        ref_x=ref_x+speed;
        ref_y=ref_y+speed;

    } else if (emg21>threshold2 || key == 'q' ) {
        ref_x=ref_x+speed;

    } else if (emg31>threshold3 || key == 'w') {
        ref_y=ref_y+speed;
    }

    if (key != 'z' && z_push) {
        ref_x=0.0;
        ref_y=0.0;
        Encoder1.reset();
        Encoder2.reset();
        z_push=false;
    }

    // convert the x and y reference to the theta and radius reference
    theta=atan(ref_y/(ref_x+minRadius));
    radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));

    //look if the new reference is outside the possible range and revert back to the old reference if it is outside  the range
    if (radius < minRadius) {
        if (key != 'z') {
            ref_x=old_ref_x;
            ref_y=old_ref_y;
        } else if (key == 'z') {
            z_push=true;
        }
    } else if ( radius > maxRadius) {
        ref_x=old_ref_x;
        ref_y=old_ref_y;
    } else if (ref_y<min_y) {
        ref_y=old_ref_y;
    }
    theta=atan(ref_y/(ref_x+minRadius));
    radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
}

double PID( double err, const double Kp, const double Ki, const double Kd,
            const double Ts, const double N, double &v1, double &v2 )   //discrete PIDF filter
{
    const double a1 =-4/(N*Ts+2),
                 a2=-(N*Ts-2)/(N*Ts+2),
                 b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
                 b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
                 b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);

    double v=err-a1*v1-a2*v2;
    double u=b0*v+b1*v1+b2*v2;
    v2=v1;
    v1=v;
    return u;
}

void controller()  //function for executing controller action
{

    //converting radius and theta to gearbox angle
    double ref_angle1=16*theta;
    double ref_angle2=(-radius+minRadius)/pulleyRadius;

    double angle1 = Encoder1.getPulses()/res;   //get number of pulses (counterclockwise is positive)
    double angle2 = Encoder2.getPulses()/res;   //get number of pulses
    m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
    //divide by voltage to get pwm duty cycle percentage)
    m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;

    //limit pwm value and change motor direction when pwm becomes either negative or positive
    if (m1_pwm >=0.0f && m1_pwm <=1.0f) {
        motor1dir=0;
        motor1.write(m1_pwm);
    } else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) {
        motor1dir=1;
        motor1.write(-m1_pwm);
    }

    if (m2_pwm >=0.0f && m2_pwm <=1.0f) {
        motor2dir=0;
        motor2.write(m2_pwm);
    } else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) {
        motor2dir=1;
        motor2.write(-m2_pwm);
    }

    //hidsopce to check what the code does exactly
    scope.set(0,ref_angle1-angle1); //error1
    scope.set(1,ref_angle1);
    scope.set(2,m1_pwm);
    scope.set(3,ref_angle2-angle2);
    scope.set(4,ref_angle2);
    scope.set(5,servo_pwm);
    scope.send();
}

void servo_controller()  // this function is used to stabalize the spatula with the servo
{
    if (theta < maxTheta ) { //servo can stabilize until a maximum theta
        servo_pwm=min_servo_pwm+(theta-minTheta)/diffTheta*res_servo;
    } else {
        servo_pwm=max_servo_pwm;
    }
    if (!servo_button){ // flip burger, spatula to max position
        servo_pwm=0.0014;
        }
        
    servo.pulsewidth(servo_pwm);

}

void my_emg()
{
    //This function is attached to a ticker so that the highest emg values are printed every second.
    pc.printf("highest_emg1=%.4f\thighest_emg2=%.4f\thighest_emg3=%.4f\n\r", highest_emg1, highest_emg2, highest_emg3);
}


void my_pos()
{
    //This function is attached to the same ticker as my_emg so that the reference position is printed every second instead of the highest emg values.
    pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta/pi*180.0);
}

int main()
{
    pc.printf("RESET\n\r");
    pc.baud(115200);

    //Attach the Biquads to the Biquad chains
    bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
    bqc13.add( &bq131);
    bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
    bqc23.add( &bq231);
    bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
    bqc33.add( &bq331);

    motor1.period(0.02f);           // period of pwm signal for motor 1
    motor2.period(0.02f);           // period of pwm signal for motor 2
    motor1dir=0;                    // setting direction to ccw
    motor2dir=0;                    // setting direction to ccw
    
    pos_timer.attach(&my_emg, 1);   // ticker used to print the maximum emg values every second
    
    //this while loop is used to determine what the highest possible value of the emg signals are and the threshold values are 1/5 of that.
    //this is done so that every user can use his own threshold value.
    while (button==1) {
        emg11=bqc13.step(fabs(bqc11.step(emg1.read())));    //filtered signal 1
        emg21=bqc23.step(fabs(bqc21.step(emg2.read())));    //filtered signal 2
        emg31=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 3
        if(emg11>highest_emg1) {
            highest_emg1=emg11;
        }
        if(emg21>highest_emg2) {
            highest_emg2=emg21;
        }
        if(emg31>highest_emg3) {
            highest_emg3=emg31;
        }
        threshold1=0.2*highest_emg1;
        threshold2=0.2*highest_emg2;
        threshold3=0.2*highest_emg3;
    }

    //Attach the 'sample' function to the timer 'sample_timer'.
    //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
    sample_timer.attach(&sampleflag, samplefreq);

    //Attach the function my_pos to the timer pos_timer.
    //This ensures that the reference position is printed every second.
    pos_timer.attach(&my_pos, 1); 
    control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
    servo_control.attach(&activate_servo_control,servo_Ts);

    while(1) {
        //Only take a sample when the go flag is true.
        if (sample_go==true) {
            sample();
            sample_go = false;            //change sample_go to false if sample() is finished
        }
        if(controller_go) { // go flag
            controller();
            controller_go=false;
        }
        if(servo_go) {
            servo_controller();
            servo_go=false;
        }
    }
}