Diff: main.cpp

Revision:

0:c96cf9760185

Child:

1:e1267e72ade8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 20 11:15:59 2016 +0000
@@ -0,0 +1,346 @@
+#include "mbed.h"
+#include "HIDScope.h"
+#include "BiQuad.h"
+#include "math.h"
+#include "iostream"
+#define SERIAL_BAUD 115200
+
+Serial pc(USBTX,USBRX);
+//EMG aansluitingen
+AnalogIn    emg0( A0 );     //Biceps Rechts
+AnalogIn    emg1( A1 );     //Bicpes Links
+AnalogIn    emg2( A2 );     //Upper leg
+//Leds
+DigitalOut  led(LED1);          // lampje om te zien dat het programma werkt
+DigitalOut  led2(LED2);
+DigitalOut  led3(LED3);
+//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;
+// filtergo uit zetten, zodat deze niet fout gaat.
+volatile bool filtergo = false;
+
+// Variabelen voor MotorHoekBerekenen
+double x = 0.0;                             // beginpositie x en y
+double y = 305.5;
+volatile const double pi = 3.14159265359; 
+double Theta1Gear = 1.0/3.0*pi;             // Beginpositie op 60 graden
+double Theta2Gear = 1.0/3.0*pi;
+double Theta1 = Theta1Gear*42/12;           // Beginpositie van MotorHoek
+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);
+    
+    // send all channels to the PC at once 
+    scope.send();
+    
+    // To indicate that the function is working, the LED is on
+    if (outRechts >= 0.015){
+        led2 = 0;
+        led = 1;
+        BicepsRight = true;
+        }
+    else{
+        led2 = 1;
+        led = 0;
+        BicepsRight = false;
+    }
+    
+    // If Biceps links is actuated then:
+    if (outLinks >= 0.015){
+        led3 = 0;
+        led = 1;
+        BicepsLeft = true;
+        }
+    else{
+        led3 = 1;
+        led = 0;
+        BicepsLeft = false;
+    }
+     // If upper leg is actuated then:
+    if (outBeen >= 0.01){
+        led3 = 0;
+        led2 = 0;
+        led = 1;
+        Leg = true;
+        }
+    else{
+        led3 = 1;
+        led2 = 1;
+        led = 0;
+        Leg = false;
+    }
+    filtergo = false;
+}
+
+// zet de filter aan
+void filter()
+    {
+    filtergo=true;
+    }
+
+///////////////////// functies voor motorhoekberekenen //////////////////////////
+// vorige x berekenen
+double Calc_Prev_x () {
+    double Prev_x = x;
+        //pc.printf("prev x = %f\r\n", Prev_x);
+    return Prev_x;
+}
+
+// vorige y berekenen
+double Calc_Prev_y () {
+    double Prev_y = y;
+        //pc.printf("prev y = %f\r\n", Prev_y);
+    return Prev_y;
+}
+
+// berekenen van de nieuwe x en y waardes
+bool CalcXY (bool BicepsLeft, bool BicepsRight, bool Leg, bool Stepper_State) {
+    double Step = 10/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
+        // flipper functie aanroepen
+    }
+    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);
+    
+    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 Prev_x, double Prev_y) {
+    double a = 50.0; 
+    double Bar = 200.0;    // lengte van de armen 
+    double Prev_Theta1_Gear = Theta1Gear; 
+    double MaxThetaGear = pi - 30.0*pi/180.0;   // de hoek voordat arm het opstakel raakt (max hoek is 24.9 tussen arm en opstakel)
+    
+    // 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);
+    }
+             
+    // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
+    if (Theta1Gear >= MaxThetaGear) {    // is de maximale hoek in radialen
+        Theta1Gear = Prev_Theta1_Gear;
+        x = Prev_x;
+        y = Prev_y;
+    }
+    
+    // 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, Prev_Gear = %f\r\n", Theta1, Theta1Gear, Prev_Theta1_Gear);
+}
+
+void CalcTheta2 (double Dia2, double Prev_x, double Prev_y) {
+    double a = 50.0; 
+    double Bar = 200.0;    // lengte van de armen 
+    double Prev_Theta2_Gear = Theta2Gear; 
+    double MaxThetaGear = pi - 30.0*pi/180.0;   // de hoek voordat arm het opstakel raakt (max hoek is 24.9 tussen arm en opstakel)
+    
+    // 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);
+    }
+    
+        // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
+    if (Theta2Gear >= MaxThetaGear) {
+        Theta2Gear = Prev_Theta2_Gear;
+        x = Prev_x;
+        y = Prev_y;
+    }
+    
+    // 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, Prev_Gear = %f\r\n", Theta2, Theta2Gear, Prev_Theta2_Gear);
+}
+
+void CalculationsForTheta () {
+    double Prev_x = Calc_Prev_x ();
+    double Prev_y = Calc_Prev_y ();
+    bool Stepper_State = CalcXY (BicepsLeft, BicepsRight, Leg, Stepper_State);
+    double Dia1 = CalcDia1 (x, y);
+    double Dia2 = CalcDia2 (x, y);
+    CalcTheta1 (Dia1, Prev_x, Prev_y);
+    CalcTheta2 (Dia2, Prev_x, Prev_y);
+}    
+
+// als de button ingedrukt wordt dan stoppen we met calibreren        
+void EndCalibration () {
+        Calibration = false;
+}        
+
+//////////////// functies voor aansturing motoren /////////////////////////////// 
+
+
+
+////////////////////////// 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(&filter, 0.002);
+    
+    // extra stap, zodat er geen op onthoudt komt.
+    while (filtergo == true)
+        {
+            sample();
+        }
+    
+    // ticker voor hoek berekenen aanzetten 
+    TickerCalculationsForTheta.attach(&CalculationsForTheta,2); ///Fr_Speed_Theta    
+    
+    
+    //empty loop, sample() is executed periodically
+    while(1) {}
+}
\ No newline at end of file