File content as of revision 5:0581d843fde2:

#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),11(encoder),12(encoder),13(encoder)) A(0,1,2)(EMG)

MODSERIAL   pc(USBTX, USBRX);

// Define the EMG inputs
AnalogIn    emg_in1( A0 );
AnalogIn    emg_in2( A1 );
AnalogIn    emg_in3( A2 );

// 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);                                                  // Speed of motor 1
FastPWM     motor2(D5);                                                  // Speed of motor 2
FastPWM     servo(D9);                                                   // Servo pwm

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

// Define encoder inputs
QEI         encoder1(D13,D12,NC,64,QEI::X4_ENCODING);
QEI         encoder2(D11,D10,NC,64,QEI::X4_ENCODING);

// Define the Tickers
Ticker      print_timer;                                                // Ticker for printing the position or highest EMG values
Ticker      controller_timer;                                           // Ticker for when a sample needs to be taken and the motor need to be controlled
Ticker      servo_timer;                                                // Ticker for calling servo_control

// Define the Time constants
const double  Ts = 0.002;                                               // Time constant for activate_controller()
const double  servo_Ts = 0.02;                                          // Time constant for activate_servo_controller()

// Define the go flags
volatile bool change_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 which the EMG signals need to surpass to change the reference

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

// Define the reference variables
double ref_x = 0.0, ref_y = 0.0;                                        // The reference position
double old_ref_x, old_ref_y;                                            // The old reference position
double speed = 0.00006;                                                 // The variable with which a speed is reached of 3 cm/s in x and y direction
double theta = 0.0;                                                     // The angle reference of the arm
double radius = 0.0;                                                    // The 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 height which the spatula can reach

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

// Define variables of motor 2
double m2_pwm = 0;                                                      // Variable for PWM control motor 2
const double m2_Kp = 36.24, m2_Ki = 108.41, m2_Kd = 3.03, m2_N = 100;   // PID values of 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;                                        // Duty cycle 1.5 ms 7.5%, min 0.5 ms 2.5%, max 2.5 ms 12.5%
const double min_theta = -37.0 / 180.0 * pi;                            // Minimum angle robot
const double max_theta = -14.0 / 180.0 * pi;                            // Maximum angle to which the spatula can stabilise
const double diff_theta = max_theta - min_theta;                        // Difference between max and min angle of theta for stabilisation
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 the functions sample() and controller()
{
    if (change_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");
    }
    change_controller_go = true; // Activate go flag
}

void activate_servo_control()   // Go flag for the function 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 servo_controller()\n\r");
    }
    servo_go = true;  // Activate go flag
}

void sample()   // Function for sampling of 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 if the emg signals go over the threshold
    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 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 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;

    // Get number of pulses 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 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 then the servo_pwm is adjusted to stabilize the spatula 
    if (theta < max_theta ) { // servo can stabilize until maximum theta
        servo_pwm = min_servo_pwm + (theta - min_theta) / diff_theta * res_servo;
    } else {
        servo_pwm = max_servo_pwm;
    }
    
    // The spatula goes to its maximum position to flip a burger if the 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 to putty
{
    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 to putty
{
    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, So that every user can use it
    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 the go flag is true.
        if (change_controller_go == true) {
            sample();
            motor_controller();
            change_controller_go = false;
        }

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