File content as of revision 4:b83460036800:

#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 servo input
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      sample_timer;                                               // Ticker for when a sample needs to be taken
Ticker      control_timer;                                              // Ticker for processing encoder input to motor output
Ticker      servo_timer;                                                // Ticker for calling servo_control

// Define the Time constants
const double freq     = 0.002;                                          // EMG sample time
const double m1_Ts    = 0.002;                                          // Controller sample time
const double m2_Ts    = 0.002;                                          // Controller sample time
const double servo_Ts =  0.02;                                          // Servo controller sample time

// Define the go flags
volatile bool change_ref_go     =   false;                                  // Go flag sample()
volatile bool controller_go =   false;                                  // Go flag controller()
volatile bool servo_go      =   false;                                  // Go flag servo_controller()

// Define the EMG variables
double emg1;                                                            // The first EMG signal
double emg2;                                                            // The second EMG signal
double emg3;                                                            // The third EMG signal
double highest_emg1;                                                    // The highest EMG signal of emg_in1
double highest_emg2;                                                    // The highest EMG signal of emg_in2
double highest_emg3;                                                    // The highest EMG signal of emg_in3
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

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

// Define the reference variables
double ref_x = 0.0;                                                     // The x reference position
double ref_y = 0.0;                                                     // The y reference position
double old_ref_x;                                                       // The old x reference
double old_ref_y;                                                       // The old y reference
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 arm
const double max_radius=0.62;                                           // The maximum radius of 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_sample() // Go flag for the function sample()
{
    if (change_ref_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_sample\n\r");
    }
    change_ref_go=true;   // Activate go flag
}

void activate_controller()  // Go flag for the function 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()   // 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 change_ref()   // 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 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,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
    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);
    }
}

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 the highest emg value
        threshold1=0.30*highest_emg1;
        threshold2=0.30*highest_emg2;
        threshold3=0.30*highest_emg3;
    }

    // Attach the function sample() to the ticker sample_timer
    sample_timer.attach(&activate_sample, freq);

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

    // Attach the function activate_controller() to the ticker control_timer
    control_timer.attach(&activate_controller,m1_Ts);

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

    while(1) {
        // Only take a sample when the go flag is true.
        if (change_ref_go==true) {
            change_ref();
            change_ref_go = false;
        }

        // Only control the motor when the go flag is true
        if(controller_go) {
            controller();
            controller_go=false;
        }

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