File content as of revision 5:b4a0301ec8cc:

#include "mbed.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "math.h"
#include "MODSERIAL.h"
#define SERIAL_BAUD 115200

MODSERIAL pc(USBTX,USBRX);
//EMG aansluitingen
AnalogIn    emg0( A0 );     //Biceps Rechts
AnalogIn    emg1( A1 );     //Bicpes Links
AnalogIn    emg2( A2 );     //Upper leg
//HIDScope
//HIDScope    scope( 6 );         // aantal channels op de HIDscope
//Button om Calibratie te beëindigen
InterruptIn Button1(PTC6);          // DIT WORDT EEN ANDERE BUTTON

// Tickers
Ticker      sample_timer;
Ticker      TickerCalculationsForTheta;

//BiQuad
// making the biquads and chains; imported from matlab
BiQuadChain bqc;        //Chain for right biceps
BiQuadChain bqc2;       //Chain for left biceps
BiQuadChain bqc3;       //Chain for Upper leg
// 1 to 3 are for right biceps
BiQuad bq1( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
BiQuad bq2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
BiQuad bq3( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
// 4 to 6 are for left biceps
BiQuad bq4( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
BiQuad bq5( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
BiQuad bq6( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
// 7 to 9 are for upper leg
BiQuad bq7( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
BiQuad bq8( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
BiQuad bq9( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );


// Variabelen voor filter
// spier EMG niet boven treshhold
volatile bool BicepsLeft = false;
volatile bool BicepsRight = false;
volatile bool Leg = false;

// Variabelen voor MotorHoekBerekenen
volatile double x = 0.0;                             // beginpositie x en y
volatile double y = 305.5;
volatile const double pi = 3.14159265359; 
volatile double Theta1Gear = 60.0;             // Beginpositie op 60 graden
volatile double Theta2Gear = 60.0;
volatile double Theta1 = Theta1Gear*42/12;           // Beginpositie van MotorHoek
volatile double Theta2 = Theta2Gear*42/12;
double Fr_Speed_Theta = 100.0;              // frequentie in Hz waarmee theta wordt uigerekend
bool Calibration = true;                    // beginnen met calibreren
volatile bool Stepper_State = false;        // false = we zijn niet bezig met flippen

/////////////////////// functies voor filters /////////////////////////////////////////////
void sample()
{
    /*  Sample function
    samples the emg0 (of the rightBiceps)
    samples the emg1 (of the leftBiceps)
    samples the emg2 (of the Upper leg)
    filter the singals
    sends it to HIDScope
    */
    
    // Rechts Biceps
    double inRechts  = emg0.read();                 // EMG signal
    double FilterRechts = bqc.step(inRechts);       // High pass + Notch (50 HZ)
    double RectifyRechts = fabs(FilterRechts);      // Rectify
    double outRechts =  bq3.step(RectifyRechts);    // Low pass
   
    // Links Biceps
    double inLinks  = emg1.read();                  // EMG signal
    double FilterLinks = bqc2.step(inLinks);        // High pass + Notch (50 HZ)
    double RectifyLinks = fabs(FilterLinks);        // Rectify
    double outLinks =  bq6.step(RectifyLinks);      // Low pass
    
    // Upper leg
    double inBeen  = emg2.read();                   // EMG signal
    double FilterBeen = bqc3.step(inBeen);          // High pass + Notch (50 HZ)
    double RectifyBeen = fabs(FilterBeen);          // Rectify
    double outBeen =  bq9.step(RectifyBeen);        // Low pass 
    
    
    /* EMG signal in channel 0 (the first channel) 
    and the filtered EMG signal in channel 1 (the second channel)
    of the HIDscope */
    /*scope.set(0,inRechts);
    scope.set(1,outRechts);
    scope.set(2,inLinks);
    scope.set(3,outLinks);
    scope.set(4,inBeen);
    scope.set(5,outBeen);
    */
    // To indicate that the function is working, the LED is on
    if (outRechts >= 0.04){
        BicepsRight = true;
        }
    else{
        BicepsRight = false;
    }

    // If Biceps links is actuated then:
    if (outLinks >= 0.04){
        BicepsLeft = true;
        }
    else{
        BicepsLeft = false;
    }
     // If upper leg is actuated then:
    if (outBeen >= 0.01){
        Leg = true;
        }
    else{
        Leg = false;
    }
}

///////////////////// functies voor motorhoekberekenen //////////////////////////
// vorige x opslaan
double Calc_Prev_x () {
    double Prev_x = x;
        //pc.printf("prev x = %f\r\n", Prev_x);
    return Prev_x;
}

// vorige y opslaan
double Calc_Prev_y () {
    double Prev_y = y;
        //pc.printf("prev y = %f\r\n", Prev_y);
    return Prev_y;
}

// vorige Theta1Gear opslaan
double Calc_Prev_Theta1_Gear () {
    double Prev_Theta1_Gear = Theta1Gear;
    return Prev_Theta1_Gear;
}

//vorige Theta2Gear opslaan
double Calc_Prev_Theta2_Gear () {
    double Prev_Theta2_Gear = Theta2Gear;
    return Prev_Theta2_Gear;
}

void FunctieFlipper();      // declarate function, function discribed below
// berekenen van de nieuwe x en y waardes
bool CalcXY (bool BicepsLeft, bool BicepsRight, bool Leg, bool Stepper_State) {
    double Step = 5/Fr_Speed_Theta ;  //10 mm per seconde afleggen
    
    if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) {   
        Stepper_State = true;     // we zijn aan het flipper dus geen andere dingen doen
        FunctieFlipper() ;
    }
    else if (BicepsLeft==true && BicepsRight==false && Leg==false && Stepper_State==false) {
        x = x - Step;
    }
    else if (BicepsLeft==false && BicepsRight==true && Leg==false && Stepper_State==false) {
        x = x + Step;       // naar Right bewegen
    }
    else if (BicepsLeft==true && BicepsRight==true && Leg==false && Stepper_State==false) {
        y = y + Step;        // naar voren bewegen
    }
    else if (BicepsLeft==false && BicepsRight==true && Leg==true && Stepper_State==false) {
        y = y - Step;       // naar achter bewegen
    }
    else if (BicepsLeft==false && BicepsRight==false && Leg==false || Stepper_State==true) {
    }
    
    // Grenswaardes     LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
    if (x > 200) {
        x = 200;
    }
    else if (x < -200) {
        x = -200;
    }
    if (y > 306) {
        y = 306;
    }
    else if (y < 50) {
        y = 50;         // GOKJE, UITPROBEREN
    }
    //pc.printf("x = %f, y = %f\r\n", x, y);
      
    //scope.set(2,x);
    //scope.set(3,y);
    
    return Stepper_State;
}    

// diagonaal berekenen voor linker arm
double CalcDia1 (double x, double y) {
    double a = 50.0;                                // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden)   KEERTJE NAMETEN
    double BV1 = sqrt(pow((a+x),2) + pow(y,2));     // diagonaal (afstand van armas tot locatie) berekenen
    double Dia1 = pow(BV1,2)/(2*BV1);               // berekenen van de afstand oorsprong tot diagonaal
    
    //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia1, x, y);
    return Dia1;
}

// diagonaal berekenen voor rechter arm
double CalcDia2 (double x, double y) {
    double a = 50.0;                                
    double BV2 = sqrt(pow((x-a),2) + pow(y,2));     // zelfde nog een keer doen maar nu voor de rechter arm 
    double Dia2 = pow(BV2,2)/(2*BV2); 
    
    //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia2, x, y);
    return Dia2;
}

// calculate Theta1
void CalcTheta1 (double Dia1) {
    double a = 50.0; 
    double Bar = 200.0;    // lengte van de armen 
    
    // Hoek berekenen van het grote tandwiel (gear)
    if (x > -a) {
        Theta1Gear = pi - atan(y/(x+a)) - acos(Dia1/Bar);
    }
    else if (x > -a) {
        Theta1Gear = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
    }
    else {  // als x=-a
        Theta1Gear = 0.5*pi - acos(Dia1/Bar);
    }
    Theta1Gear = Theta1Gear*180.0/pi;       // veranderen van radialen naar graden
             
    // omrekenen van grote tandwiel naar motortandwiel
    Theta1 = Theta1Gear*42.0/12.0;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12. 
        
    pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta1, Theta1Gear);
}

void CalcTheta2 (double Dia2) {
    double a = 50.0; 
    double Bar = 200.0;    // lengte van de armen 
    double Prev_Theta2_Gear = Theta2Gear; 
        
    // Hoek berekenen van het grote tandwiel (gear)
    if (x < a) {
        Theta2Gear = pi + atan(y/(x-a)) - acos(Dia2/Bar);
    }
    else if (x > a) {
        Theta2Gear = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
    }
    else {  // als x=a
        Theta2Gear = 0.5*pi - acos(Dia2/Bar);
    }
    Theta2Gear = Theta2Gear*180/pi;         // veranderen van radialen naar graden
    
    // omrekenen van grote tandwiel naar motortandwiel
    Theta2 = Theta2Gear*42.0/12.0;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.

    pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
}

// als een van de hoeken te groot wordt, zet dan alles terug naar de vorige positie
void AngleLimits (double Prev_Theta1_Gear, double Prev_Theta2_Gear, double Prev_x, double Prev_y) {
    double MaxThetaGear = 100.0;   // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
    
    if (Theta1Gear >= MaxThetaGear || Theta2Gear >= MaxThetaGear) {
        Theta1Gear = Prev_Theta1_Gear;
        Theta2Gear = Prev_Theta2_Gear;
        x = Prev_x;
        y = Prev_y;
        
        Theta1 = Theta1Gear*42.0/12.0;
        Theta2 = Theta2Gear*42.0/12.0; 
    }
    pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
}

void CalculationsForTheta () {
    sample();
    double Prev_x = Calc_Prev_x ();
    double Prev_y = Calc_Prev_y ();
    double Calc_Prev_Theta1_Gear ();
    double Calc_Prev_Theta2_Gear ();
    bool Stepper_State = CalcXY (BicepsLeft, BicepsRight, Leg, Stepper_State);
    double Dia1 = CalcDia1 (x, y);
    double Dia2 = CalcDia2 (x, y);
    CalcTheta1 (Dia1);
    CalcTheta2 (Dia2); 
    AngleLimits (Theta1Gear, Theta2Gear, Prev_x, Prev_y);

    //scope.send();
}    

// als de button ingedrukt wordt dan stoppen we met calibreren        
void EndCalibration () {
        Calibration = false;
}        

//////////////// functies voor aansturing motoren /////////////////////////////// 
void FunctieFlipper () {
    Stepper_State = false;
}

////////////////////////// main loop ////////////////////////////////////////////
int main()
{   
    pc.baud(SERIAL_BAUD);
    pc.printf("\r\n Nieuwe code uitproberen :) \r\n");
    
    /* making the biquad chain for filtering the emg
    notch and high pass */
    bqc.add( &bq1 ).add( &bq2 );
    bqc2.add( &bq4 ).add( &bq5 );
    bqc3.add( &bq7 ).add( &bq8 );
    
    // Calibreren
    while (Calibration == true) {
        //potmeter dingen aanpassen
        
        pc.printf("calibreren is bezig\r\n");
        Button1.fall(&EndCalibration);
    }
    
    /**Attach the 'sample' function to the timer 'sample_timer'.
    * this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
    */
   // sample_timer.attach(&sample, 0.002);

    // ticker voor hoek berekenen aanzetten 
    TickerCalculationsForTheta.attach(&CalculationsForTheta,1); //0.002 
   
    //empty loop, sample() is executed periodically
    while(1) {
        
        }
}