Diff: main.cpp

Revision:

8:00c3992af9f4

Parent:

7:4df9f6ebe744

--- a/main.cpp	Mon Oct 12 08:44:50 2015 +0000
+++ b/main.cpp	Wed Oct 14 13:19:32 2015 +0000
@@ -4,7 +4,7 @@
 #include "HIDScope.h"
 
 Serial pc(USBTX,USBRX);
-HIDScope scope(2);          // definieerd het aantal kanalen van de scope
+HIDScope scope(3);          // definieerd het aantal kanalen van de scope
 
 // Define Tickers and control frequencies
 Ticker          controller1, controller2;        // definieer de ticker die controler1 doet
@@ -65,17 +65,25 @@
  // Define storage variables for reference values
     double c_reference1 = 0;
     double c_reference2 = 0;
+// limit  angles
+    // motor1
+    const double limlow1 = 0.5;             // min height
+    const double limhigh1 = 4.5;            // max height
+    // motor 2
+    const double limlow2 = -4.5;            // maximum height, motor has been inversed due to transmission
+    const double limhigh2 = 2;              // minimum height
+    
 // Define the maximum rate of change for the reference (velocity)
-    double Vmax = 5;            // rad/sec
+    double Vmax = 3;            // rad/sec
 
 // CONTROLLER SETTINGS
     // motor 1
-    const double m1_Kp = 1;         // Proportional constant     
-    const double m1_Ki = 1;         // integration constant
+    const double m1_Kp = 2;         // Proportional constant     
+    const double m1_Ki = 0.05;         // integration constant
     const double m1_Kd = 0;         // differentiation constant
     // motor 2
-    const double m2_Kp = 1;
-    const double m2_Ki = 0;
+    const double m2_Kp = 0.25;
+    const double m2_Ki = 0.01;
     const double m2_Kd = 0;
 // storage variables
     // motor 1
@@ -94,7 +102,7 @@
 
 // Filter coefficients
 // differential action filter
-    const double m_f_a1 = 1.0, m_f_a2 = 2.0, m_f_b0 = 1.0, m_f_b1 = 3.0, m_f_b2 = 4.0;      // coefficients from sheets are used as first test.
+    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.
     
 // tweede orde notch filter 50 Hz
 // biquad 1 coefficienten
@@ -153,9 +161,11 @@
 
 // This functions takes a 0->1 input, uses passing by reference (&c_reference)
 // to create a reference that moves with a variable speed. It is now optimised for 0->1 values
-double reference_f(double input, double &c_reference)
+double reference_f(double input, double &c_reference, double limlow, double limhigh)
 {
     double reference = c_reference + input * controlstep * Vmax ;
+            if(reference < limlow){reference = limlow;}
+            if(reference > limhigh){reference = limhigh;}
            c_reference = reference; // change the global variable to the latest location.
     return reference;
 }
@@ -214,21 +224,22 @@
 {
 // Proportional
 double P = Kp * e;
-pc.printf("P = %f, ",P);
+// pc.printf("P = %f, ",P);
 // Integral
 e_int = e_int + Ts * e;
 double I = e_int * Ki;
-pc.printf("I = %f, ",I);
+// pc.printf("I = %f, ",I);
 // Derivative   (Disabled for now because of NaN problem from filter
 double e_derr = (e - e_prev)/Ts;
-pc.printf("e_derr %f, ",e_derr);
+// pc.printf("e_derr %f, ",e_derr);
 e_derr = biquadfilter(e_derr, m_f_v1, m_f_v2, m_f_a1, m_f_a2, m_f_b0, m_f_b1, m_f_b2);
+// 
 pc.printf("fil_e_derr %f, ",e_derr);    // check for NaN                ??
 e_prev = e;
-// double D = Kd* e_derr;
+double D = Kd* e_derr;
 
 // PID
-double output = P + I ;// + D;
+double output = P + I + D;
 return output;
 }
 
@@ -252,16 +263,23 @@
 {
     
     double input1 = potright.read()*signalamp1; // this line must be seperated for emg usage
+    //pc.printf("input = %f ",input1);
     // first data limiter
     if(input1 < input_threshold) {input1 = 0;}
     if(input1 > 1) {input1 = 1;}
     // reverse direction
     if(reverse_rechts == true) {input1 = -input1;}
-    double reference1 = reference_f(input1, c_reference1);      // determine the reference that has been set by the inputsignal
+
+    double reference1 = reference_f(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);
     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);
-    pc.printf("output 1 %f \n",output1);
+    //pc.printf("output 1 %f \n",output1);
+    scope.set(2,output1);
+    scope.send();
+    output1 = outputlimiter(output1,1);
     if(output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
         motor1_rich.write(0);
         motor1_aan.write(output1);
@@ -280,10 +298,13 @@
     if(input2 > 1) {input2 = 1;}
     // reverse direction
     if(reverse_links == true) {input2 = -input2;}
-    double reference2 = reference_f(input2, c_reference2);      // determine the reference that has been set by the clamped inputsignal
+    double reference2 = reference_f(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)
+    //scope.set(1,error2);
+    //scope.send();
     double 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
         motor2_rich.write(0);
         motor2_aan.write(output2);
@@ -312,7 +333,7 @@
 {
     pc.baud(115200);
     controller1.attach(&motor1_activate, controlstep);      // call a go-flag
-    // controller2.attach(&motor2_activate, controlstep);   Disabled while debugging PID-controller ??
+    controller2.attach(&motor2_activate, controlstep);   // Disabled while debugging PID-controller ??
     while(true) 
     {
         // button press functions
@@ -350,7 +371,7 @@
             if(motor2_go) { motor2_go = false; motor2_control();}
         }
         // turn green led on // start calibration procedures
-        if(loop_start == false && calib_start == true) { LED(1,0,1);}
+        if(loop_start == false && calib_start == true) { LED(1,0,1); motor1_aan.write(0); motor2_aan.write(0);}
         // turn red led on
         if(loop_start == true && calib_start == true) { LED(0,1,1);}
         // turn leds off (both buttons false)