Revision 1:f3910e46b831, committed 2015-10-18

Comitter:

Zeekat

Date:

Sun Oct 18 21:36:16 2015 +0000

Parent:

0:a643b1b38abe

Child:

2:867a1eb33bbe

Commit message:

marijns zooi toegevoegd weet nog nie of het warkt

Changed in this revision

--- a/main.cpp	Sun Oct 18 17:27:35 2015 +0000
+++ b/main.cpp	Sun Oct 18 21:36:16 2015 +0000
@@ -2,14 +2,16 @@
 #include "MODSERIAL.h"
 #include "encoder.h"
 #include "HIDScope.h"
+#include "math.h"
+// #include "complex.h"
 
 Serial pc(USBTX,USBRX);
 HIDScope scope(3);          // definieerd het aantal kanalen van de scope
 
 // Define Tickers and control frequencies
-Ticker          controller1, controller2;        // definieer de ticker die controler1 doet
+Ticker          controller1, controller2, main_filter, cartesian;        // definieer de ticker die controler1 doet
     // Go flag variables
-    volatile bool motor1_go = false, motor2_go = false;
+    volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false;
 
     // Frequency control
     double controlfreq = 50 ;    // controlloops frequentie (Hz)
@@ -31,11 +33,28 @@
 int reference1 = 0;         // set the reference position of the encoders (not used)
 int reference2 = 0;
 
+////////
+// physical constants
+const double L = 36;           // lenght of arms
+const double pi = 3.1415926535897;
+///////////////////////////
+// Main control loop transfer variables
+// here all variables that transfer bewtween the primary control functions
+double output1;
+double output2;
+
+double theta_one;
+double theta_two;
+
 
 // EXTRA INPUTS AND REQUIRED VARIABLES
 //POTMETERS
     AnalogIn potright(A0);      // define the potmeter outputpins
     AnalogIn potleft(A1);
+    
+// Analoge input signalen defineren
+AnalogIn    EMG_in(A2);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
+AnalogIn    EMG_int(A3);                // deze leest A3 uit
 
 // BUTTONS 
     // control flow             
@@ -134,13 +153,11 @@
     const double denlow5_2 =-1.91119706742607;
     const double denlow5_3 =0.914975834801434;
 
-// Define the storage variables and filter coeficients for four filters
-double f1_v1 = 0, f1_v2 = 0;
-double f2_v1 = 0, f2_v2 = 0;
-double f3_v1 = 0, f3_v2 = 0;
-double f4_v1 = 0, f4_v2 = 0;
-
-
+// Define the storage variables and filter coeficients for eight filters
+//filter 1
+double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0;
+// filter2
+double f1_v1t = 0, f1_v2t = 0, f2_v1t = 0, f2_v2t = 0, f3_v1t = 0, f3_v2t = 0,f4_v1t = 0, f4_v2t = 0;
 
 ////////////////////////////////////////////////////////////////
 /////////////////// START OF SIDE FUNCTIONS ////////////////////
@@ -160,7 +177,6 @@
 // It has been set up for standard 2X DECODING!!!
 double get_radians(double counts)       
 {
-    double pi = 3.14159265359;
     double radians = (counts/4200)*2*pi;        // 2X DECODING!!!!! ((32 counts/rotation, last warning)
     return radians;
 }
@@ -214,15 +230,14 @@
     return y;
     }
 
-double EMG_Filter(double u1)
+// biquadfilters die bij het filteren van signaal 2 horen, copy paste, alle waardes zijn veranderd naar +t (t van two of twee)
+double biquadfiltert(double ut, double &v1t, double &v2t, const double a1t, const double a2t, const double b0t, const double b1t, const double b2t)
 {
-    // double u1 = potright.read();     // legacy test code
-    double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
-    double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
-    double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
-    double y4 = abs(y3);
-    double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
-    return y5;
+    double vt = ut- a1t*v1t-a2t*v2t;
+    double yt = b0t*vt+b1t*v1t+b2t*v2t;
+    v2t = v1t;
+    v1t = vt;
+    return yt;
 }
 
 // PID Controller given in sheets
@@ -247,76 +262,135 @@
 /////////////////////////////////////////////////////////////////////
 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
 ///////////////////////////////////////////////////////////////////
-// these functions are used to control all aspects of a single electric motor and are called by the main function from tickers
+// these functions are called by go-flags and are used to update main variables and send signals to motor
 
 // MOTOR 1
 void motor1_control()
 {
-    
-    double input1 = potright.read()*signalamp1; // this line must be seperated for emg usage
+    double m_input1 = potright.read(); // used for pot input
     //input1 = 0.4505;
     
     // first input edit (limit signal between threshold and 1, and reverse if wanted
-    if(input1 < input_threshold) {input1 = 0;}
-    if(input1 > 1) {input1 = 1;}
-    if(reverse_rechts == true) {input1 = -input1;}
-    input1 = biquadfilter(input1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);        //biquad with diff-filter  coefficients to smooth input
+    if(m_input1 < input_threshold) {m_input1 = 0;}
+    if(m_input1 > 1) {m_input1 = 1;}
+    if(reverse_rechts == true) {m_input1 = -m_input1;}
+    m_input1 = biquadfilter(m_input1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);        //biquad with diff-filter  coefficients to smooth input
     
     
-    double reference1 = reference_f(input1, c_reference1,limlow1,limhigh1);      // determine the reference that has been set by the inputsignal
+    double reference1 = reference_f(m_input1, c_reference1,limlow1,limhigh1);      // determine the reference that has been set by the inputsignal
     scope.set(0,reference1);
     double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
     scope.set(1,rads1);
     scope.send();
     double error1 = (reference1 - rads1);                       // determine the error (reference - position)
-    double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
+    double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
     
-    output1 = outputlimiter(output1,1); // relimit the output for safety
-    if(output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
+    m_output1 = outputlimiter(m_output1,1); // relimit the output for safety
+    if(m_output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
         motor1_rich.write(0);
-        motor1_aan.write(output1);
-    } else if(output1 < 0) {
+        motor1_aan.write(m_output1);
+    } else if(m_output1 < 0) {
         motor1_rich.write(1);
-        motor1_aan.write(abs(output1));
+        motor1_aan.write(abs(m_output1));
     }
 }
 
 // MOTOR 2
 void motor2_control()
 {
-    double input2 = potleft.read()*signalamp2;      // replace potleft with filter
+    double m_input2 = potleft.read()*signalamp2;      // replace potleft with filter
+    
     // first input limiter
-    if(input2 < input_threshold) {input2 = 0;}
-    if(input2 > 1) {input2 = 1;}
-    if(reverse_links == false) {input2 = -input2;}
-    input2 = biquadfilter(input2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
+    if(m_input2 < input_threshold) {m_input2 = 0;}
+    if(m_input2 > 1) {m_input2 = 1;}
+    if(reverse_links == false) {m_input2 = -m_input2;}
+    m_input2 = biquadfilter(m_input2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
     
     
-    double reference2 = reference_f(input2, c_reference2,limlow2,limhigh2);      // determine the reference that has been set by the clamped inputsignal
+    double reference2 = reference_f(m_input2, c_reference2,limlow2,limhigh2);      // determine the reference that has been set by the clamped inputsignal
     double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
     double error2 = (reference2 - rads2);                       // determine the error (reference - position)
-    double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
+    double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
     
-    output2 = outputlimiter(output2,1);
-    if(output2 > 0) {                    // uses the calculated output to determine the direction  of the motor
+    m_output2 = outputlimiter(m_output2,1);
+    if(m_output2 > 0) {                    // uses the calculated output to determine the direction  of the motor
         motor2_rich.write(0);
-        motor2_aan.write(output2);
-    } else if(output2 < 0) {
+        motor2_aan.write(m_output2);
+    } else if(m_output2 < 0) {
         motor2_rich.write(1);
-        motor2_aan.write(abs(output2));
+        motor2_aan.write(abs(m_output2));
     }
 }
 
 
+// function that updates the inputs
+void EMG_filter()
+{
+// filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
+    double u1 = EMG_in.read();
+    double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
+    double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
+    double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
+    double y4 = abs(y3);
+    double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
+    output1 = y5;    // update global variable
+// versturen van het input signaal u1 en het gefilterde signaal y5 naar HIDScope channel 0 en 1
+    //scope.set(0,u1);
+    //scope.set(1,y5);
+// filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
+    double u1t = EMG_int.read();
+    double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
+    double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
+    double y3t = biquadfiltert( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
+    double y4t = abs(y3t);
+    double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
+    output2 = y5t; // update global variable
+    
+    // versturen van input signaal2; u1t en het gefilterde signaal; y5t naar HIDScope channel 2 en 3
+    //scope.set(2,u1t);
+    //scope.set(3,y5t);
+    // verzenden van de verstuurde signalen
+    //scope.send();
+ }
+ 
+ // function that updates the required motor angles
+ void det_angles()
+{
+    double xx = 7*50*output1;
+    double yy = 7*50*output2;
+    double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
+    double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
+    double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
+    // hoeken berekenen
+    theta_one = atan2(yy,xx)+beta;
+    theta_two = pi + alfa;
+    
+    // testen van de functie
+    scope.set(0,xx);
+    scope.set(1,yy);
+    double xt = L*cos(theta_one)+L*cos(theta_one+theta_two);
+    double yt = L*sin(theta_one)+L*sin(theta_one+theta_two);
+    scope.set(2,xt);
+    scope.set(3,yt);
+    scope.send();
+}
 //////////////////////////////////////////////////////////////////
 //////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
 ////////////////////////////////////////////////////////////////
 
+
+void EMG_activate()
+{ 
+    emg_go = true; 
+}
+void angle_activate()
+{ 
+    cart_go = true; 
+}
 void motor1_activate()
 { 
     motor1_go = true; 
 }
- 
 void motor2_activate()
 { 
     motor2_go = true; 
@@ -325,6 +399,8 @@
 int main()
 {
     pc.baud(115200);
+    main_filter.attach(&EMG_activate, controlstep);
+    cartesian.attach(&angle_activate, controlstep);
     controller1.attach(&motor1_activate, controlstep);      // call a go-flag
     controller2.attach(&motor2_activate, controlstep);   
     while(true) 
@@ -361,6 +437,8 @@
         if(loop_start == true && calib_start == false)        // check if start button = true then start the main control loops
         {
             LED(1,1,0); // turn blue led on
+            if(cart_go)   { cart_go = false; det_angles();}
+            if(emg_go)    { emg_go = false; EMG_filter();}
             if(motor1_go) { motor1_go = false; motor1_control();}
             if(motor2_go) { motor2_go = false; motor2_control();}
         }