File content as of revision 9:ef6c5feb5623:

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

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

MODSERIAL   pc(USBTX, USBRX);

// Define the EMG inputs
AnalogIn    emg_in1( A0 );  // EMG signal of the thumb
AnalogIn    emg_in2( A1 );  // EMG signal of the right bicep
AnalogIn    emg_in3( A2 );  // EMG signal of the left bicep

// Define motor outputs
DigitalOut  motor1dir(D7);  // Direction of motor 1, attach at m1, set to 0: cw
DigitalOut  motor2dir(D4);  // Direction of motor 2, attach at m2, set to 0: ccw
FastPWM     motor1(D6);     // Duty cycle of pwm signal for motor 1
FastPWM     motor2(D5);     // Duty cycle of pwm signal for motor 2
FastPWM     servo(D9);      // Pulsewidth of pwm signal for servo

// Define button for flipping the spatula
DigitalIn   servo_button(PTC12);

// Define encoder inputs
QEI         encoder1(D13,D12,NC,64,QEI::X4_ENCODING);   //Defining highest encoder accuracy for motor1
QEI         encoder2(D11,D10,NC,64,QEI::X4_ENCODING);   //Defining highest encoder accuracy for motor2

// Define the Tickers
Ticker      print_timer;        // Ticker for printing position or highest EMG values
Ticker      controller_timer;   // Ticker for sampling and motor control
Ticker      servo_timer;        // Ticker for servo control

// Define the Time constants
const double  Ts = 0.002;       // Time constant for sampling and motor control
const double  servo_Ts = 0.02;  // Time constant for servo control

// Define the go flags
volatile bool controller_go = false;    // Go flag for sample() and motor_controller()
volatile bool servo_go = false;         // Go flag servo_controller()

// Define the EMG variables
double emg1, emg2, emg3;                         // The three filtered EMG signals
double highest_emg1, highest_emg2, highest_emg3; // The highest EMG signals of emg_in
double threshold1, threshold2, threshold3;       // The threshold for the EMG signals to change the reference

//Define the keyboard input
char key;                           // Stores last pressed key

// Define the reference variables
double ref_x = 0.0, ref_y = 0.0;    // Reference position
double old_ref_x, old_ref_y;        // Old reference position
double speed = 0.00006;             // Variable with which a speed is reached of 3 cm/s in x and y direction
double theta = 0.0;                 // Angle reference of the arm
double radius = 0.0;                // Radius reference of the arm
bool   z_pushed = false;            // To see if z is pressed

// Define reference limits
const double min_radius = 0.43;     // The minimum radius of the arm
const double max_radius = 0.62;     // The maximum radius of the arm
const double min_y = -0.26;         // The minimum y position of the arm

// Define variables of motor 1
double m1_pwm = 0;                  // Variable for PWM  motor 1
const double m1_Kp = 35.16, m1_Ki = 108.8, m1_Kd = 2.84, m1_N = 100;    // PID values for motor 1
double m1_v1 = 0, m1_v2 = 0;        // Memory variables

// Define variables of motor 2
double m2_pwm = 0;                  // Variable for PWM motor 2
const double m2_Kp = 36.24, m2_Ki = 108.41, m2_Kd = 3.03, m2_N = 100;   // PID values for motor 2
double m2_v1 = 0, m2_v2 = 0;        // Memory variables

// Define machine constants
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 pulley_radius = 0.0398/2.0;                // Pulley radius

// Define variables needed for controlling the servo
double       servo_pwm = 0.0023;                        // Pulsewidth PWM for servo (min 0.0005, max 0.0025)
const double min_theta = -37.0 / 180.0 * pi;            // Minimum angle of the arm
const double max_theta = -14.0 / 180.0 * pi;            // Maximum angle to which the spatula is stabilised
const double diff_theta = max_theta - min_theta;        // Difference between max and min angle
const double min_servo_pwm = 0.0021;                    // Corresponds to angle of theta -38 degrees
const double max_servo_pwm = 0.0024;                    // Corresponds to angle of theta -24 degrees
const double res_servo = max_servo_pwm - min_servo_pwm; // Resolution of servo pwm ... 
                                                        // ... signal between min and max angle

// Define the Biquad chains
BiQuadChain bqc11;
BiQuadChain bqc12;
BiQuadChain bqc21;
BiQuadChain bqc22;
BiQuadChain bqc31;
BiQuadChain bqc32;

// 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( 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 activate_controller() // Go flag for sample() and 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()   // Go flag for servo_controller()
{
    if (servo_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_servo_control()\n\r");
    }
    servo_go = true;  // Activate go flag
}

void sample()   // Function for sampling the EMG signal and changing the reference position
{
    // Change key if the keyboard is pressed
    if (pc.readable() == 1) {
        key=pc.getc();
    }

    // Read the EMG signals and filter it
    emg1 = bqc12.step(fabs(bqc11.step(emg_in1.read())));    //filtered signal 1
    emg2 = bqc22.step(fabs(bqc21.step(emg_in2.read())));    //filtered signal 2
    emg3 = bqc32.step(fabs(bqc31.step(emg_in3.read())));    //filtered signal 3

    // Remember what the old reference was
    old_ref_x = ref_x;
    old_ref_y = ref_y;

    // Change the reference position if the EMG signals exceed the threshold value or if a key is pressed
    if (emg1 > threshold1 && emg2 > threshold2 && emg3 > threshold3 || key == 'd') { // Negative XY direction
        ref_x = ref_x - speed;
        ref_y = ref_y - speed;

    } else if (emg1 > threshold1 && emg2 > threshold2 || key == 'a' || key == 'z') { // Negative X direction
        ref_x = ref_x - speed;

    } else if (emg1 > threshold1 && emg3 > threshold3 || key == 's') {               // Negative Y direction
        ref_y = ref_y - speed;

    } else if (emg2 > threshold2 && emg3 > threshold3 || key == 'e' ) {              // Positive XY direction
        ref_x = ref_x + speed;
        ref_y = ref_y + speed;

    } else if (emg2 > threshold2 || key == 'q' ) {                                   // Positive X direction
        ref_x = ref_x + speed;

    } else if (emg3 > threshold3 || key == 'w') {                                    // Positive Y direction
        ref_y = ref_y + speed;
    }

    // Change z_pushed to true if 'z' is pressed on the keyboard
    if (key == 'z') {
        z_pushed = true;
    }

    // Reset the reference and the encoders if z is no longer pressed
    if (key != 'z' && z_pushed) {
        ref_x = 0.0;
        ref_y = 0.0;
        encoder1.reset();
        encoder2.reset();
        z_pushed = false;
    }

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

    // If the new reference is outside the possible range
    // then revert back to the old reference unless z is pressed
    if (radius < min_radius) {
        if (key != 'z') {
            ref_x = old_ref_x;
            ref_y = old_ref_y;
        }
    } else if ( radius > max_radius) {
        ref_x = old_ref_x;
        ref_y = old_ref_y;
    } else if (ref_y < min_y) {
        ref_y = old_ref_y;
    }

    // Calculate theta and radius again
    theta = atan( ref_y / (ref_x + min_radius));
    radius = sqrt( pow( ref_x + min_radius, 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 controller (tustin approximation)
{
    //calculate the controller coefficients
    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;          //input for motors
    v2 = v1;            //store old value
    v1 = v;             //store old value
    return u;
}

void motor_controller()  // Function for executing controller action
{
    // Convert radius and theta to gearbox angles
    double ref_angle1 = 16 * theta;
    double ref_angle2 = (-radius + min_radius) / pulley_radius;

    // calculate angle of the encoders
    double angle1 = encoder1.getPulses() / res;   //counterclockwise is positive
    double angle2 = encoder2.getPulses() / res;

    // Calculate the motor pwm using the function PID() and the maximum voltage
    m1_pwm = (PID(ref_angle1 - angle1, m1_Kp, m1_Ki, m1_Kd, Ts, m1_N, m1_v1, m1_v2)) / V_max;
    m2_pwm = (PID(ref_angle2 - angle2, m2_Kp, m2_Ki, m2_Kd, 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);
    }
}

void servo_controller()  // Function for stabilizing the spatula with the servo
{
    // If theta is smaller than max_theta, servo_pwm is adjusted to stabilise spatula
    if (theta < max_theta ) {
        servo_pwm = min_servo_pwm + (theta - min_theta) / diff_theta * res_servo;
    } else {
        servo_pwm = max_servo_pwm;
    }

    // Spatula goes to its maximum position to flip a burger if button is pressed
    if (!servo_button) {
        servo_pwm = 0.0014;
    }

    // Send the servo_pwm to the servo
    servo.pulsewidth(servo_pwm);
}

void my_emg()   // This function prints the highest EMG values
{
    pc.printf("highest_emg1=%.4f\thighest_emg2=%.4f\thighest_emg3=%.4f\n\r",
    highest_emg1, highest_emg2, highest_emg3);
}


void my_pos()   // This function prints the reference position
{
    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");

    // Set the baud rate
    pc.baud(115200);

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

    // Define the period of the pwm signals
    motor1.period(0.02f);
    motor2.period(0.02f);

    // Set the direction of the motors to ccw
    motor1dir = 0;
    motor2dir = 0;

    // Attaching the function my_emg() to the ticker print_timer
    print_timer.attach(&my_emg, 1);

    // While loop used for calibrating the EMG thresholds, since EMG values of muscles differ
    while (servo_button == 1) {
        emg1 = bqc12.step(fabs(bqc11.step(emg_in1.read())));    //filtered signal 1
        emg2 = bqc22.step(fabs(bqc21.step(emg_in2.read())));    //filtered signal 2
        emg3 = bqc32.step(fabs(bqc31.step(emg_in3.read())));    //filtered signal 3

        // Check if the new EMG signal is higher than the current highest value
        if(emg1 > highest_emg1) {
            highest_emg1 = emg1;
        }

        if(emg2 > highest_emg2) {
            highest_emg2 = emg2;
        }

        if(emg3 > highest_emg3) {
            highest_emg3 = emg3;
        }

        // Define the thresholds as 0.3 times the highest EMG value
        threshold1 = 0.30 * highest_emg1;
        threshold2 = 0.30 * highest_emg2;
        threshold3 = 0.30 * highest_emg3;
    }

    // Attach the function activate_controller() to the ticker change_pos_timer
    controller_timer.attach(&activate_controller, Ts);

    // Attach the function my_pos() to the ticker print_timer.
    print_timer.attach(&my_pos, 1);

    // Attach the function activate_servo_control() to the ticker servo_timer
    servo_timer.attach(&activate_servo_control,servo_Ts);

    while(1) {
        // Take a sample and control the motor when go flag is true.
        if (controller_go == true) {
            sample();
            motor_controller();
            controller_go = false;
        }

        // Control the servo when go flag is true
        if(servo_go == true) {
            servo_controller();
            servo_go=false;
        }
    }
}