Revision 7:84abed2f376c, committed 2015-10-22

Comitter:

Zeekat

Date:

Thu Oct 22 16:24:44 2015 +0000

Parent:

6:bfd24400e9d0

Child:

8:649a5f555b7b

Commit message:

untested. removed unused buttons. did some formatting.

Changed in this revision

--- a/main.cpp	Thu Oct 22 12:21:06 2015 +0000
+++ b/main.cpp	Thu Oct 22 16:24:44 2015 +0000
@@ -4,62 +4,35 @@
 #include "HIDScope.h"
 #include "math.h"
 
-
-///// points..... mooi maken calib
-Serial pc(USBTX,USBRX);
+Serial pc(USBTX,USBRX);     // MODSERIAL output
 HIDScope scope(4);          // definieerd het aantal kanalen van de scope
 
 // Define Tickers and control frequencies
-Ticker          controller1, controller2, main_filter, cartesian;        // definieer de ticker die controler1 doet
-    // Go flag variables
+Ticker          controller1, controller2, main_filter, cartesian;
+    // Go flag variables belonging to Tickers
     volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false;
 
-    // Frequency control
+// Frequency control
     double controlfreq = 50 ;    // controlloops frequentie (Hz)
     double controlstep = 1/controlfreq; // timestep derived from controlfreq
 
 
+//////////////////////// IN-OUTPUT /////////////////////////////////////////////
 //MOTOR OUTPUTPINS
-// motor 1
-    PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
-    DigitalOut motor1_rich(D7); // digitaal signaal voor richting
-// motor 2
-    PwmOut motor2_aan(D5);
-    DigitalOut motor2_rich(D4);
+    PwmOut motor1_aan(D6), motor2_aan(D5);      // PWM signaal motor 2 (uit sheets)
+    DigitalOut motor1_rich(D7), motor2_rich(D4); // digitaal signaal voor richting
 
 // ENCODER INPUTPINS
-Encoder motor1_enc(D12,D11);        // encoder outputpins
-Encoder motor2_enc(D10,D9);
-
-////////
-// 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
-
-// filter to calibration
-double output1;
-double output2;
-
-// filter to x-y
-double output1_amp;
-double output2_amp;
-
-// x-y to motor control
-double phi_one;
-double phi_two;
-
+    Encoder motor1_enc(D12,D11), motor2_enc(D10,D9);        // encoder outputpins
 
 // EXTRA INPUTS AND REQUIRED VARIABLES
-//POTMETERS
+//POTMETERS (used for debuggin purposes, not reliable anymore)
     AnalogIn potright(A4);      // define the potmeter outputpins
     AnalogIn potleft(A5);
     
 // Analoge input signalen defineren
-AnalogIn    EMG_in(A0);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
-AnalogIn    EMG_int(A2);                // deze leest A3 uit
+    AnalogIn    EMG_in(A0);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
+    AnalogIn    EMG_int(A2);                // deze leest A3 uit
 
 // BUTTONS 
     // control flow             
@@ -69,42 +42,62 @@
         bool loop_start = false;
         bool calib_start = false;
         
-    // direction control
-    DigitalIn   reverse_button_links(D0);
-    DigitalIn   reverse_button_rechts(D1); 
+    // axis control
+    DigitalIn   switch_xy_button(D0);
         // init values
-        bool reverse_links = false;
-        bool reverse_rechts = false;
-        
-        
-// LED
+        bool switch_xy = false;
+
+// LED outputs on dev-board
     DigitalOut ledred(LED1);
     DigitalOut ledgreen(LED2);
     DigitalOut ledblue(LED3);
+    
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+
+// physical constants
+    const double L = 36;           // lenght of arms
+    const double pi = 3.1415926535897;
+
+    // angle limitations (in radians)
+    // motor1
+    const double limlow1 = 1;             // min height in rad
+    const double limhigh1 = 6.3;            // max height in rad
+    // motor 2
+    const double limlow2 = -4.0;            // maximum height, motor has been inverted due to transmission
+    const double limhigh2 = 2.5;              // minimum height in rad
+
+///////////////////////////
+// Main control loop transfer variables
+// here all variables that transfer bewtween the primary control functions
+
+// filter to calibration
+    double output1;
+    double output2;
+// filter to x-y
+    double output1_amp;
+    double output2_amp;
+// x-y to motor control
+    double phi_one;
+    double phi_two;
+
+// define start up variables
+    double start_up_time = 2;
+    double start_loops = start_up_time*controlfreq;
+    int rc1 = 0;            // counters in function to enable slow start up
+    int rc2 = 0;
+    
 
 // REFERENCE SIGNAL SETTINGS
     double input_threshold = 0.25;   // the minimum value the signal must have to change the reference.
  // Define storage variables for reference values
     double c_reference1 = 0;
     double c_reference2 = 0;
-    
-// define start up variables
-    double start_up_time = 2;
-    double start_loops = start_up_time*controlfreq;
-    int rc1 = 0;
-    int rc2 = 0;
-    
-// limit  angles (in radians)
-    // motor1
-    const double limlow1 = 1;             // min height in rad
-    const double limhigh1 = 6.3;            // max height in rad
-    // motor 2
-    const double limlow2 = -4.0;            // maximum height, motor has been inverted due to transmission
-    const double limhigh2 = 2.5;              // minimum height in rad
-    
+        
 // Define the maximum rate of change for the reference (velocity)
     double Vmax = 3;            // rad/sec
 
+
 // CONTROLLER SETTINGS
     // motor 1
     const double m1_Kp = 5;         // Proportional constant     
@@ -114,7 +107,7 @@
     const double m2_Kp = 2;
     const double m2_Ki = 0.5;
     const double m2_Kd = 0.1;
-// storage variables
+// storage variables.   these variables are used to store data between controller iterations
     // motor 1
     double m1_err_int = 0; 
     double m1_prev_err = 0;
@@ -123,57 +116,40 @@
     double m2_prev_err = 0;
 
 // EMG calibration variables
-double emg_gain1 = 7;       // set to one for unamplified signal
+double emg_gain1 = 7;       
 double emg_gain2 = 7;
 
-double cal_samples = 125;
+double cal_time = 2.5;       // amount of time calibration should take
+double cal_samples = controlfreq*cal_time;
 double normalize_emg_value = 1;      // set te desired value to calibrate the signal to
 
-//// FILTER VARIABLES
-// storage variables
+// FILTER VARIABLES
+    // storage variables
         // differential action filter, same is used for both controllers
-        double m_f_v1 = 0, m_f_v2 = 0;
+            double m_f_v1 = 0, m_f_v2 = 0;
         // input filter to smooth signal
-        double r1_f_v1 = 0, r1_f_v2 = 0;
-        double r2_f_v1 = 0, r2_f_v2 = 0;
-
-// Filter coefficients
-// differential action filter (lowpass 5Hz at 50samples)
-    const double m_f_a1 = -1.1430, m_f_a2 = 0.4128, m_f_b0 = 0.0675, m_f_b1 = 0.1349, m_f_b2 = 0.0675;      // coefficients from sheets are used as first test.
-// input filter (lowpass 1Hz at 50samples)
-    const double r1_f_a1 = -1.6475, r1_f_a2 = 0.7009, r1_f_b0 = 0.0134, r1_f_b1 = 0.0267, r1_f_b2 = 0.0134;
-    
-// tweede orde notch filter 50 Hz
-// biquad 1 coefficienten
-    const double numnotch50biq1_1 = 1;
-    const double numnotch50biq1_2 = -1.61816178466632;
-    const double numnotch50biq1_3 = 1.00000006127058;
-    const double dennotch50biq1_2 = -1.59325742941798;
-    const double dennotch50biq1_3 = 0.982171881701431;
-// biquad 2 coefficienten
-    const double numnotch50biq2_1 = 1;
-    const double numnotch50biq2_2 = -1.61816171933244;
-    const double numnotch50biq2_3 = 0.999999938729428;
-    const double dennotch50biq2_2 = -1.61431180968071;
-    const double dennotch50biq2_3 = 0.982599066293075;
-// highpass filter 20 Hz coefficienten
-    const double numhigh20_1 = 0.837089190566345;
-    const double numhigh20_2 = -1.67417838113269;
-    const double numhigh20_3 = 0.837089190566345;
-    const double denhigh20_2 = -1.64745998107698;
-    const double denhigh20_3 = 0.700896781188403;
-// lowpass 5 Hz coefficienten
-    const double numlow5_1 =0.000944691843840162;
-    const double numlow5_2 =0.00188938368768032;
-    const double numlow5_3 =0.000944691843840162;
-    const double denlow5_2 =-1.91119706742607;
-    const double denlow5_3 =0.914975834801434;
-
-// 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;
+            double r1_f_v1 = 0, r1_f_v2 = 0, r2_f_v1 = 0, r2_f_v2 = 0;
+        // Define the storage variables and filter coeficients for eight filters
+        // EMG 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;
+        // EMG 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;
+        
+    // Filter coefficients
+        // differential action filter (lowpass 5Hz at 50samples)
+            const double m_f_a1 = -1.1430, m_f_a2 = 0.4128, m_f_b0 = 0.0675, m_f_b1 = 0.1349, m_f_b2 = 0.0675;
+        // input filter (lowpass 1Hz at 50samples) (used to make the x-y angle signal as smooth as possible
+            const double r1_f_a1 = -1.6475, r1_f_a2 = 0.7009, r1_f_b0 = 0.0134, r1_f_b1 = 0.0267, r1_f_b2 = 0.0134;    
+    // EMG-Filter
+        // tweede orde notch filter 50 Hz
+        // biquad 1 coefficienten
+            const double numnotch50biq1_1 = 1, numnotch50biq1_2 = -1.61816178466632, numnotch50biq1_3 = 1.00000006127058, dennotch50biq1_2 = -1.59325742941798, dennotch50biq1_3 = 0.982171881701431;
+        // biquad 2 coefficienten
+            const double numnotch50biq2_1 = 1, numnotch50biq2_2 = -1.61816171933244, numnotch50biq2_3 = 0.999999938729428, dennotch50biq2_2 = -1.61431180968071, dennotch50biq2_3 = 0.982599066293075;
+        // highpass filter 20 Hz coefficienten
+            const double numhigh20_1 = 0.837089190566345, numhigh20_2 = -1.67417838113269, numhigh20_3 = 0.837089190566345, denhigh20_2 = -1.64745998107698, denhigh20_3 = 0.700896781188403;
+        // lowpass 5 Hz coefficienten
+            const double numlow5_1 =0.000944691843840162, numlow5_2 =0.00188938368768032, numlow5_3 =0.000944691843840162, denlow5_2 =-1.91119706742607, denlow5_3 =0.914975834801434;
 
 ////////////////////////////////////////////////////////////////
 /////////////////// START OF SIDE FUNCTIONS ////////////////////
@@ -276,6 +252,8 @@
 return output;
 }
 
+
+// function that limits the angles that can be used as reference
  double angle_limits(double phi, double limlow, double limhigh)
 {
     if(phi < limlow)
@@ -319,10 +297,10 @@
     output2 = y5t;
     output2_amp = y5t*emg_gain2; 
     
-    scope.set(0,output1);
-    scope.set(1,output2);
-    scope.set(2,output1_amp);
-    scope.set(3,output2_amp);
+    //scope.set(0,output1);
+    //scope.set(1,output2);
+    scope.set(0,output1_amp);
+    scope.set(1,output2_amp);
     scope.send();
  }
  
@@ -330,10 +308,11 @@
  // function that updates the required motor angles
  void det_angles()
 {
-    
+    // limit the output between 0 and 1
     if(output1_amp>1) {output1_amp = 1;}
     if(output2_amp>1) {output2_amp = 1;}
     
+    // use potmeter for debugging purposes
     //output1 = potright.read();
     //output2 = potleft.read();
     
@@ -342,22 +321,24 @@
     double ymin = -16; //- sqrt(4900 - pow(xx,2));
     double ymax = 16; //sqrt(4900 - pow(xx,2));
     double yy = ymin+output2_amp*(ymax-ymin);
+    
+    // x-y to arm-angles math
     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
-    
     double theta_one = (atan2(yy,xx)+beta); 
     double theta_two = (-pi + alfa); 
     
+    // convert arm-angles to motor angles (x transmission) and offset (+ offset) to account for reset position
     double phi1 = 4*(theta_one) + 2.8;
-    double phi2 = 4*(theta_one+theta_two) + 1.85;
-    phi2 = -phi2;
+    double phi2 = 4*(theta_one+theta_two) + 1.85;       // math assumes angle relative to first arm. motor does not change relative orientation, so angle wrt reference position is needed.
+    phi2 = -phi2;   // reverse angle because of double timing belt.
 
     // check the input angles and apply the limits
     phi1 = angle_limits(phi1,limlow1,limhigh1);
     phi2 = angle_limits(phi2,limlow2,limhigh2);
     
-    // smooth the input signal (lowpass 1Hz)    
+    // smooth the input signal (lowpass 1Hz). (to reduce the freq content after reaching limit)   
     phi1 = biquadfilter(phi1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
     phi2 = biquadfilter(phi2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
     
@@ -365,6 +346,7 @@
     phi_one = phi1;
     phi_two = phi2;
     
+    // debugging line
     pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two);
 }
 // MOTOR 1
@@ -417,7 +399,7 @@
     double error2 = (reference2 - rads2);                       // determine the error (reference - position)
     double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
     
-    m_output2 = outputlimiter(m_output2,1);
+    m_output2 = outputlimiter(m_output2,1);     // final output limit (not really needed, is for safety)
     if(m_output2 > 0) {                    // uses the calculated output to determine the direction  of the motor
         motor2_rich.write(0);
         motor2_aan.write(m_output2);
@@ -429,6 +411,7 @@
 
 // calibrate the emg-signal
 // works bij taking a certain amount of samples adding them and then normalize to the desired value
+// STILL BUGGED!!!
 void calibrate_amp()
 {
     double total1 = 0;
@@ -453,22 +436,10 @@
 ////////////////////////////////////////////////////////////////
 
 
-void EMG_activate()
-{ 
-    emg_go = true; 
-}
-void angle_activate()
-{ 
-    cart_go = true; 
-}
-void motor1_activate()
-{ 
-    motor1_go = true; 
-}
-void motor2_activate()
-{ 
-    motor2_go = true; 
-}
+void EMG_activate(){emg_go = true;}
+void angle_activate(){cart_go = true;}
+void motor1_activate(){motor1_go = true;}
+void motor2_activate(){motor2_go = true;}
 
 int main()
 {
@@ -494,19 +465,12 @@
             wait(0.3);             
         }
         // reverse buttons
-        if(reverse_button_links.read() == 0)
+        if(switch_xy_button.read() == 0)
         {
-           reverse_links = !reverse_links;     
-           wait(reverse_button_links.read() == 1);
+           switch_xy = !switch_xy;     
+           wait(switch_xy_button.read() == 1);
            wait(0.3);             
         }
-         if(reverse_button_rechts.read() == 0)
-        {
-            reverse_rechts = !reverse_rechts;     
-            wait(reverse_button_rechts.read() == 1);
-            wait(0.3);             
-        }
-
         //////////////////////////////////////////////////
         // Main Control stuff and options
         if(loop_start == true && calib_start == false)        // check if start button = true then start the main control loops