File content as of revision 0:ef7fd98bf091:

#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "math.h"

#define     M_Pi 3.141592653589793238462643383279502884L

Serial      pc(USBTX, USBRX);

DigitalOut  led_red(LED_RED);
DigitalOut  led_blue(LED_BLUE);
InterruptIn button1(D2);
InterruptIn button2(D3);
AnalogIn    EMG_A0(A0);
AnalogIn    EMG_A1(A1);

DigitalOut  motor1DirectionPin(D4);
PwmOut      motor1MagnitudePin(D5);
DigitalOut  motor2DirectionPin(D7); // Sequence? Lines 181-183
PwmOut      motor2MagnitudePin(D6);

Ticker      measureTick;

Encoder     motor1(D13,D12); //On the shield actually M2
Encoder     motor2(D11,D10); //On the shield actually M1 (Production mistake?)

int          Count1 = 0;
int          Count2 = 0;
int          MaxEMGcount = 1800; // Split up for motor 1 and 2 AND create a MinEMGCount
int          MinEMGcount = -1800;
bool         switch1 = 0; // manual switch for when to start calculations (later removed for a state machine
bool         direction1 = 1;
bool         direction2 = 1;
const double RAD_PER_PULSE = 0.002991; // Value for RAD_PER_PULSE given through the slides (wrong?)
const double DEG_PER_RAD = 180 / M_Pi; // Basic knowledge of how many degrees are in 1 radian.
int          MaxAveragerValues = 500;
double       Av_A0 = 0;
double       Av_A1 = 0;
double       q1 = 0;                // Angle of arm 1 (upper) in starting position is 0 degrees
double       q2 = 179/DEG_PER_RAD;  // Angle of arm 2 (lower) in starting position is 180 degrees (but can't be 0 or 180 because of determinant = 0)
int          L1 = 47;               // Length of arm 1 (upper) in cm
int          L2 = 29;               // Length of arm 2 (lower) in cm
int          xdes = L1-L2           // Desired x coordinate, arm is located at x = L1-L2 in starting position
int          ydes = 0;              // Disired y coordinate, arm is located at y = 0 in starting position

// Sample time (motor1-timestep)
const double M1_Ts = 0.01; //100 Hz systems
const double M2_Ts = 0.01;

// Controller gains (motor1-Kp,-Ki,...)
const double M1_Kp = 0.1, M1_Ki = 0.02, M1_Kd = 0.00125,  M1_N = 100; // THESE VALUES ARE ARBITRARY AT THIS POINT
const double M2_Kp = 0.1, M2_Ki = 0.02, M2_Kd = 0.00125,  M2_N = 100; // Inspired by the Ziegler-Nichols Method

// Filter variables (motor1-filter-v1,-v2)
double M1_f_v1 = 0.0, M1_f_v2 = 0.0;
double M2_f_v1 = 0.0, M2_f_v2 = 0.0;

// PROGRAM THAT CALCULATES ANGLE CHANGES


// PROGRAM THAT ANALYSES EMG SIGNAL AND FILTERS IT
// Define the HIDScope and Ticker object
//HIDScope    scope(2);
//Ticker      scopeTimer;

BiQuadChain bqchain1, bqchain2; //BiQuad Chain. Makes a combination of various BiQuads.

BiQuad bq1(0.960582636413903, -1.921165272827806, 0.960582636413903, -1.916303032724640, 0.926027512930974);//Highpass 8Hz 
BiQuad bq2(0.912835456766912, -1.825670913533825, 0.912835456766912, -1.821050358261190, 0.830291468806461);//
BiQuad bq3(0.991153589776121, -1.603846829332603, 0.991153589776121, -1.593257429417890, 0.982171881701343);//Notch 49-51Hz
BiQuad bq4(0.991153589776121, -1.603846829332603, 0.991153589776121,  -1.614311809680850, 0.982599066293185);//
BiQuad bq5(0.064591432448980, 0.129182864897960, 0.064591432448980,  -1.401313635492332, 0.659679365288253);//Lowpasss 45Hz
BiQuad bq6(0.052062866562661, 0.104125733125322, 0.052062866562661,  -1.129505912021716, 0.337757378272360);//

BiQuadChain bqchain3, bqchain4; //BiQuad Chain. Makes a combination of various BiQuads.

BiQuad bq7(0.960582636413903, -1.921165272827806, 0.960582636413903, -1.916303032724640, 0.926027512930974);//Highpass 8Hz 
BiQuad bq8(0.912835456766912, -1.825670913533825, 0.912835456766912, -1.821050358261190, 0.830291468806461);//
BiQuad bq9(0.991153589776121, -1.603846829332603, 0.991153589776121, -1.593257429417890, 0.982171881701343);//Notch 49-51Hz
BiQuad bq10(0.991153589776121, -1.603846829332603, 0.991153589776121,  -1.614311809680850, 0.982599066293185);//
BiQuad bq11(0.064591432448980, 0.129182864897960, 0.064591432448980,  -1.401313635492332, 0.659679365288253);//Lowpasss 45Hz
BiQuad bq12(0.052062866562661, 0.104125733125322, 0.052062866562661,  -1.129505912021716, 0.337757378272360);//

double A0t10=0, A0t9=0, A0t8=0, A0t7=0, A0t6=0, A0t5=0, A0t4=0, A0t3=0, A0t2=0, A0t1=0;
double A1t10=0, A1t9=0, A1t8=0, A1t7=0, A1t6=0, A1t5=0, A1t4=0, A1t3=0, A1t2=0, A1t1=0;

double EMGSample1(double Input, double &t10, double &t9, double &t8, double &t7, double &t6, double &t5, double &t4, double &t3, double &t2, double &t1){ // Sample function: read a value from the Analog in
    //double MaxSignal = 0;
    double Signal = 0.0f;
    double EMGFiltered = fabs(bqchain1.step(Input)); // Signal.Read instantly gets filtered.
    double EMGSignal = bqchain2.step(EMGFiltered);
    double treshhold = 0.6*Av_A0;
    // if (EMGSignal > MaxSignal){ MaxSignal = EMGSignal;} // Save the highest value measured.
    // double discrete = floor((EMGSignal/MaxSignal)*10.0)/10.0;
    if (EMGSignal >= treshhold || t1 >= treshhold || t2 >= treshhold || t3 >= treshhold || t4 >= treshhold || t5 >= treshhold || t6 >= treshhold || t7 >= treshhold || t8 >= treshhold || t9 >= treshhold || t10 >= treshhold ){
        Signal = 1;
    }
    else {
        Signal = 0.0f; 
    }
    t10=t9, t9=t8, t8=t7, t7=t6, t6=t5, t5=t4, t4=t3, t3=t2, t2=t1, t1= EMGSignal;
    
    return Signal;
}

double EMGSample2(double Input, double &t10, double &t9, double &t8, double &t7, double &t6, double &t5, double &t4, double &t3, double &t2, double &t1){ // Sample function: read a value from the Analog in
    //double MaxSignal = 0;
    double Signal = 0.0f;
    double EMGFiltered = fabs(bqchain3.step(Input)); // Signal.Read instantly gets filtered.
    double EMGSignal = bqchain4.step(EMGFiltered);
    double treshhold = 0.6*Av_A1;
    // if (EMGSignal > MaxSignal){ MaxSignal = EMGSignal;} // Save the highest value measured.
    // double discrete = floor((EMGSignal/MaxSignal)*10.0)/10.0;
    if (EMGSignal >= treshhold || t1 >= treshhold || t2 >= treshhold || t3 >= treshhold || t4 >= treshhold || t5 >= treshhold || t6 >= treshhold || t7 >= treshhold || t8 >= treshhold || t9 >= treshhold || t10 >= treshhold ){
        Signal = 1;
    }
    else {
        Signal = 0.0f; 
    }
    t10=t9, t9=t8, t8=t7, t7=t6, t6=t5, t5=t4, t4=t3, t3=t2, t2=t1, t1= EMGSignal;
    
    return Signal;
}

// PROGRAM THAT MEASURES AN AMOUNT OF VALUES AND EXPORTS THE AVERAGE
void AverageValue_A0(double &Av_A0)
{
  double A[MaxAveragerValues];
  double sum = 0;
  for( int i = 0; i < MaxAveragerValues; i++){
      double EMGFiltered = fabs(bqchain1.step(EMG_A0.read())); // Signal.Read instantly gets filtered.
      double Value = bqchain2.step(EMGFiltered);
      wait(0.01);
      for (int j = 1; j < MaxAveragerValues; j++){
          A[MaxAveragerValues-j] = A[MaxAveragerValues-1-j];
      }
      A[0] = Value;
  }
  for (int i = 0; i < MaxAveragerValues; i++){
      sum = sum + A[i];
  }
  printf("result = %.0f", (sum/MaxAveragerValues));
  Av_A0 = (sum/MaxAveragerValues);
}

void AverageValue_A1(double &Av_A1)
{
  double A[MaxAveragerValues];
  double sum = 0;
  for( int i = 0; i < MaxAveragerValues; i++){
      double EMGFiltered = fabs(bqchain3.step(EMG_A1.read()));
      double Value = bqchain4.step(EMGFiltered); 
      wait(0.01);
      for (int j = 1; j < MaxAveragerValues; j++){
          A[MaxAveragerValues-j] = A[MaxAveragerValues-1-j];
      }
      A[0] = Value;
  }
  for (int i = 0; i < MaxAveragerValues; i++){
      sum = sum + A[i];
  }
  printf("result = %.0f", (sum/MaxAveragerValues));
  Av_A1 = (sum/MaxAveragerValues);
}

// PROGRAM THAT ENABLES THE SYSTEM TO OPERATE (Later to be replaced by State Machine)
void but1_pressed(void)
{
    if (switch1 == 0){
        pc.printf("TAKING AVERAGES...");
        for(int i = 1; i < 12; i++){
            led_blue = !led_blue;
            wait(0.1);
            }     
        AverageValue_A0(Av_A0);   
        pc.printf("Av_A0 = %.3f ", Av_A0);
        led_blue = 1;
        for(int i = 1; i < 12; i++){
            led_blue = !led_blue;
            wait(0.1);
            } 
        AverageValue_A1(Av_A1);
        pc.printf("Av_A1 = %.3f \r\n\n", Av_A1);
        led_blue = 1;
    }
    wait(10.0);
    led_red = !led_red;
    switch1 = !switch1;
}

void but2_pressed(void)
{
    direction1 = !direction1;
    direction2 = !direction2;
}

// PROGRAM THAT CALCULATES THE PID
double PID( double err, const double Kp, const double Ki, const double Kd,
const double Ts, const double N, double &v1, double &v2 ) {

    const double a1 = -4/(N*Ts+2), a2 = -(N*Ts-2)/(N*Ts+2), // a1 and a2 are the nominator of our transferfunction
    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); // b0, b1 and b2 the denominator

    double v = err - a1*v1 - a2*v2; // Memory value are calculated and later on stored. (v is like an input)
    double u = b0*v + b1*v1 + b2*v2;
    v2 = v1; v1 = v;
    return u; // u functions as our output value gained from the transferfunction.
}

// PROGRAM THAT RECEIVES EMG SIGNALS AND CONVERTS THEM TO REFERENCE POSITIONS
double EMG_Referencer(double sig, bool dir, int &n) {
    //pc.printf("MEASURING");
    if (dir == 1){
        if (sig > 0.99){ //Above 0.9 because of inconsistency in received signals
            if (n < MaxEMGcount){
                n = n++; //Normally in steps of 1 (so n = n++;) but for better visibility we have 36 itterations
            }
        }
    }
    else if (dir == 0){
        if (sig > 0.99){
            if (n > MinEMGcount){ //MinEMGcount
                n = n--; // Normally n = n--;
            }
        }
    }
    double u = 0.1*n;
    return u;
}

//Kinematic model
double Kinematic_referencer(double xdes, bool ydes, int &n) {   //weet niet wat ik met die n moet
    //printf ("x=%f, y=%f\n", x, y);
    double qt = q1 + q2;                               // current total angle
    double xcurrent = L1 * cos (q1) + L2 * cos (qt);   // current x position
    double ycurrent = L1 * sin (q1) + L2 * sin (qt);   // current y position
    
    //Initial twist
    double vx = (xdes - xcurrent)/100;  // Running on 100 Hz
    double vy = (ydes - ycurrent)/100;
    
    //Jacobians
    double J11 = -ycurrent;
    double J12 = -L2 * sin (qt);
    double J21 = xcurrent;
    double J22 = L2 * cos (qt);
    double Determinant = J11 * J22 - J21 * J12;   // calculate determinant

    //Calculate angular velocities
    double q1der = (J22 * vx - J12 * vy) / Determinant;
    double q2der = (-J21 * vx + J11 * vy) / Determinant;
    
    //Calculate new angles
    q1new = q1 + q1der/100;    //nog fixen met die tijdstappen?
    q2new = q2 + q2der/100;      //hier ook
    //printf ("q1=%f, q2=%f\n", q1 * c, q2 * c);
    qtnew = q1new + q2new;
    
    //Calculate new positions
    xnew = L1 * cos (q1new) + L2 * cos (qtnew);
    ynew = L1 * sin (q1new) + L2 * sin (qtnew);
    //printf ("x=%f, y=%f\n", x, y);
    
    // Now check whether the calculated position is desired, determinants close to zero may cause the robot to move weirdly
    // New y may not be negative, this means the arm is located in on the plate
    // New q1 may not be less than -45 degrees, less means the arm will crash into the base plate
    // New q2 may not be more than 185 degrees, more means the lower arm will crash into the upper arm
    if (ynew > -1 && q1new > -45 / DEG_PER_RAD && q2new < 185 / DEG_PER_RAD)
    {
        // If desired, change the angles
        q1 = q1new;
        q2 = q2new;
    }
    else
    {
        // If not desired, don't change the angles, but define current position as desired so the robot ignores the input
        xdes = xcurrent;
        ydes = ycurrent;
    }
    return q1,q2;
}

// PROGRAMS THAT CONTROL THE VALUE OUTPUT
double M1_Controller(double Angle1) {
    double EMG_Input_A0 = EMG_A0.read();
    double sig1 = EMGSample1(EMG_Input_A0, A0t10, A0t9, A0t8, A0t7, A0t6, A0t5, A0t4, A0t3, A0t2, A0t1);
    
    double referencePosition = EMG_Referencer(sig1, direction1, Count1);
    pc.printf("D1 = %i, S1 = %.0f  ", Count1, sig1);
    double motor1Value = PID( referencePosition - Angle1 , M1_Kp, M1_Ki, M1_Kd, M1_Ts, M1_N, M1_f_v1, M1_f_v2); //Find the motorvalue by going through the PID
    
    return motor1Value; // According to the PID its calculations, we get a motor value.
}

double M2_Controller(double Angle2) {
    double EMG_Input_A1 = EMG_A1.read();; //EMG_A1.read();
    double sig2 = EMGSample2(EMG_Input_A1, A1t10, A1t9, A1t8, A1t7, A1t6, A1t5, A1t4, A1t3, A1t2, A1t1);
    
    double referencePosition = EMG_Referencer(sig2, direction2, Count2);; // Since we have 360 degrees, our potmeter should reach these values as well.
    pc.printf("D2 = %i, S2 = %.0f \r\n", Count2, sig2);
    double motor2Value = PID( referencePosition - Angle2 , M2_Kp, M2_Ki, M2_Kd, M2_Ts, M2_N, M2_f_v1, M2_f_v2); //Find the motorvalue by going through the PID
    
    return motor2Value; // According to the PID its calculations, we get a motor value.
}

// PROGRAMS FOR POWERING THE MOTOR ACCORDING TO THE ERROR (P VARIANT)
void SetMotor1(double motor1Value) // function that actually changes the output for the motor
{
    if(motor1Value >= 0 )  //Function sets direction and strength
        motor1DirectionPin = 1; //If the reference value is positive, we will turn clockwise
    else
        motor1DirectionPin = 0; // if not, counterclockwise
        
    if(fabs(motor1Value) > 0.2 ) // Next, check the absolute motor value, which is the magnitude
        motor1MagnitudePin = 0.2; // This is a safety. We never want to exceed 1
    else 
        motor1MagnitudePin = fabs(motor1Value); // if we fall underneath the safety, take the magnitude
}

void SetMotor2(double motor2Value) // function that actually changes the output for the motor
{
    if(motor2Value >= 0 )  //Function sets direction and strength
        motor2DirectionPin = 0; //If the reference value is positive, we will turn clockwise
    else
        motor2DirectionPin = 1; // if not, counterclockwise
        
    if(fabs(motor2Value) > 0.2 ) // Next, check the absolute motor value, which is the magnitude
        motor2MagnitudePin = 0.2; // This is a safety. We never want to exceed 1
    else 
        motor2MagnitudePin = fabs(motor2Value); // if we fall underneath the safety, take the magnitude
}

// PROGRAM THAT MEASURES AND CONTROLES THE MOTOR OUTPUT
void MeasureAndControl() // Pure values being calculated and send to the Mbed.
{   
    double Angle1 = DEG_PER_RAD * RAD_PER_PULSE * motor1.getPosition(); // Angle is equal to the degrees per pulse measured (NOT TRUE AT THIS MOMENT, '360' != 2 * M_Pi
    double Angle2 = DEG_PER_RAD * RAD_PER_PULSE * motor2.getPosition(); // Angle is equal to the degrees per pulse measured (NOT TRUE AT THIS MOMENT, '360' != 2 * M_Pi
    
    if (switch1 == 1){ // If the switch is pressed
        double motor1Value = M1_Controller(Angle1); // create a motorvalue to give to motor 1 according to the control and angle
        double motor2Value = M2_Controller(Angle2); // create a motorvalue to give to motor 1 according to the control and angle
        
        SetMotor1(motor1Value);
        SetMotor2(motor2Value);
    }
    else {
        SetMotor1(0);
        SetMotor2(0); }
}

int main() // Main function
{
    pc.baud(115200); // For post analysis, seeing if the plug works etc.
    pc.printf("STARTING SEQUENCE \r\n"); //Merely checking if there is a serial connection at all
    bqchain1.add(&bq1).add(&bq2).add(&bq3).add(&bq4); // Combine the BiQuads bq1, bq2 and bq3 in the Chain.
    bqchain2.add(&bq5).add(&bq6);
    bqchain3.add(&bq7).add(&bq8).add(&bq9).add(&bq10); // Combine the BiQuads bq1, bq2 and bq3 in the Chain.
    bqchain4.add(&bq11).add(&bq12);
    measureTick.attach(&MeasureAndControl, M1_Ts); // Tick that changes the motor (currently 1Hz)
    led_red = 1; // Set the LED off in the positive direction, on in the negative direction
    led_blue = 1;
    button1.fall(&but1_pressed); // Trigger the InterruptIn of the button.
    button2.fall(&but2_pressed);
    //scopeTimer.attach_us(&scopeSend, 1e4); // Attach in Microseconds
}