Revision 41:a89907bb3f70, committed 2016-10-27

Comitter:

RiP

Date:

Thu Oct 27 14:22:47 2016 +0000

Parent:

40:1ca50c657cc5

Child:

42:37cd882e7f2b

Commit message:

emg filter en motor aansturing samengevoegd en alles werkt, nu moet alleen het servo gedeelte nog toegevoegd worden

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Thu Oct 27 14:22:47 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/FastPWM/#87e38b846651

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Thu Oct 27 14:22:47 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- a/main.cpp	Thu Oct 27 12:41:00 2016 +0000
+++ b/main.cpp	Thu Oct 27 14:22:47 2016 +0000
@@ -2,27 +2,38 @@
 #include "HIDScope.h"
 #include "BiQuad.h"
 #include "MODSERIAL.h"
+#include "QEI.h"
+#include "FastPWM.h"
 
 // in gebruik: D(0,1,4,5,6,7,8,10,11,12,13) A(0,1,2)
 
 MODSERIAL pc(USBTX, USBRX);
-HIDScope scope(3); // aantal scopes in hidscope opzetten
+HIDScope scope(3); // the amount of scopes to send to the pc
 
 //Define objects
-//Define the button interrupt for the calibration
-InterruptIn button_calibrate(PTA4);
 
 //Define the EMG inputs
 AnalogIn    emg1( A0 );
 AnalogIn    emg2( A1 );
 AnalogIn    emg3( A2 );
 
+//Define motor outputs
+DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
+FastPWM motor1(D6);     // speed of motor 1
+FastPWM motor2(D5);     //speed of motor 2
+DigitalOut motor2dir(D4);   //direction of motor 2, attach at m2, set to 0: ccw
+
+QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
+QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
+
 //Define the Tickers
 Ticker      pos_timer;                      // the timer which is used to print the position every second
 Ticker      sample_timer;                   // the timer which is used to decide when a sample needs to be taken
+Ticker      control;                        // Ticker for processing encoder input
 
 //Initialize all variables
 volatile bool sampletimer = false;          // a variable which is changed when a sample needs to be taken
+volatile bool controller_go=false;
 
 double threshold = 0.04;                    // the threshold which the emg signals need to surpass to do something
 double samplefreq=0.002;                    // every 0.002 sec a sample will be taken this is a frequency of 500 Hz
@@ -43,6 +54,22 @@
 const double min_y=-0.28;                   // minimum height which the spatula can reach
 char key;                                   // variable to place the keyboard input
 
+double m1_pwm=0;    //variable for PWM control motor 1
+double m2_pwm=0;    //variable for PWM control motor 2
+
+const double m1_Kp = 9.974, m1_Ki = 16.49, m1_Kd = 1.341, m1_N = 100; // controller constants motor 1
+double m1_v1 = 0, m1_v2 = 0; // Memory variables
+const double m1_Ts = 0.01; // Controller sample time
+
+const double m2_Kp = 9.974, m2_Ki = 16.49, m2_Kd = 1.341, m2_N = 100; // controller constants motor 2
+double m2_v1 = 0, m2_v2 = 0; // Memory variables
+const double m2_Ts = 0.01; // Controller sample time
+
+const double pi=3.14159265359;
+const double res = 64/(1/131.25*2*pi);     // resolution on gearbox shaft per pulse
+const double V_max=9.0;                    // maximum voltage supplied by trafo
+const double pulleyDiameter=0.0398;        // pulley diameter
+
 //Define the needed Biquad chains
 BiQuadChain bqc11;
 BiQuadChain bqc13;
@@ -98,11 +125,13 @@
     sampletimer=true;
 }
 
-void calibrate()
+void activate_controller()
 {
-    //This resets the reference signals so that the robot can be calibrated
-    ref_x=0.0000;
-    ref_y=0.0000;
+    if (controller_go==true) {
+        // this if statement is used to see if the code takes too long before it is called again
+        pc.printf("rate too high error in activate_controller()\n\r");
+    }
+    controller_go=true;   //activate go flag
 }
 
 void sample()
@@ -162,6 +191,63 @@
         ref_x=old_ref_x;
         ref_y=old_ref_y;
     }
+    theta=atan(ref_y/(ref_x+minRadius));
+    radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
+}
+
+double PID( double err, const double Kp, const double Ki, const double Kd,
+            const double Ts, const double N, double &v1, double &v2 )   //discrete PIDF filter
+{
+    const double a1 =-4/(N*Ts+2),
+                 a2=-(N*Ts-2)/(N*Ts+2),
+                 b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
+                 b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
+                 b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
+
+    double v=err-a1*v1-a2*v2;
+    double u=b0*v+b1*v1+b2*v2;
+    v2=v1;
+    v1=v;
+    return u;
+}
+
+void controller()  //function for executing controller action
+{
+
+    //converting radius and theta to gearbox angle
+    double ref_angle1=16*theta;
+    double ref_angle2=(-radius+minRadius)/pi/pulleyDiameter;
+    //pc.printf("%.5f",ref_angle2);
+    //ref_angle2=0.0;
+
+    double angle1 = Encoder1.getPulses()/res;   //get number of pulses (counterclockwise is positive)
+    double angle2 = Encoder2.getPulses()/res;   //get number of pulses
+    m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
+    //divide by voltage to get pwm duty cycle percentage)
+    m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;
+
+    //limit pwm value and change motor direction when pwm becomes either negative or positive
+    if (m1_pwm >=0.0f && m1_pwm <=1.0f) {
+        motor1dir=0;
+        motor1.write(m1_pwm);
+    } else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) {
+        motor1dir=1;
+        motor1.write(-m1_pwm);
+    }
+
+    if (m2_pwm >=0.0f && m2_pwm <=1.0f) {
+        motor2dir=0;
+        motor2.write(m2_pwm);
+    } else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) {
+        motor2dir=1;
+        motor2.write(-m2_pwm);
+    }
+
+    //hidsopce to check what the code does exactly
+    scope.set(0,ref_angle2-angle2); //error
+    scope.set(1,angle2);
+    scope.set(2,m2_pwm);
+    scope.send();
 }
 
 void my_pos()
@@ -184,14 +270,20 @@
     bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
     bqc33.add( &bq331);
 
+    motor1.period(0.02f); //period of pwm signal for motor 1
+    motor2.period(0.02f); // period of pwm signal for motor 2
+    motor1dir=0; // setting direction to ccw
+    motor2dir=0; // setting direction to ccw
+
     //Attach the 'sample' function to the timer 'sample_timer'.
     //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
     sample_timer.attach(&sampleflag, samplefreq);
-    //Attach the function calibrate to the button interrupt
-    button_calibrate.fall(&calibrate);
+
     //Attach the function my_pos to the timer pos_timer.
     //This ensures that the position is printed every second.
     pos_timer.attach(&my_pos, 1);
+    control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
+
 
     while(1) {
         //Only take a sample when the go flag is true.
@@ -199,5 +291,9 @@
             sample();
             sampletimer = false;            //change sampletimer to false if sample() is finished
         }
+        if(controller_go) { // go flag
+            controller();
+            controller_go=false;
+        }
     }
 }
\ No newline at end of file