Diff: main.cpp

Revision:

0:28db0afc950f

Child:

1:49f60db70b5a

diff -r 000000000000 -r 28db0afc950f main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Sep 24 16:04:43 2015 +0000
@@ -0,0 +1,158 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "HIDScope.h"
+
+//////////////////////////////////////////////CONSTANTS AND SHIZZLE////////////////////////////////////////////////////////////////////////////
+//info uit
+HIDScope scope(3);
+//encoders
+QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
+
+//ingangen
+AnalogIn pot1(A0);
+
+//uitgangen
+DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
+PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
+//PwmOut motor2_speed_control(D5);
+//DigitalOut motor2_direction(D4);
+const int CW=1; //clockwise
+const int CCW=0; //counterclockwise
+
+//frequencies
+//const float pwm_frequency=1000;
+const double hidscope_frequency=100;
+const double pid_control_frequency=5;
+const double filter_pot1_frequency=200;
+
+//tickers
+Ticker hidscope_ticker;
+Ticker pid_control_ticker;
+Ticker filter_pot1_ticker;
+
+//constants
+const int cpr=32*131;
+const float PI=3.1415;
+const float counttorad=((2*PI)/cpr);
+
+//////////////////////////////////////////////////////////FILTERS///////////////////////////////////////////////////////
+
+//another more powerful low pass filter
+//Biquad #1
+const double gain_f1 = 0.004756;
+const double B_f1[] = { 1.00000000000, -0.34966580719,  1.00000000000};
+const double A_f1[] = { 1.00000000000, -1.86848759822,  0.87558390333};
+
+//Biquad #2
+const double gain_f2 = 0.036454;
+const double B_f2[] = { 1.00000000000, -1.56455717372,  1.00000000000};
+const double A_f2[] = { 1.00000000000, -1.93013247482,  0.94815823632};
+
+
+//filterwaardes uit asn naar aparte waardes omschrijven
+double v1 = 0, v2 = 0, u = 0, y = 0;
+const double a1_f1 = gain_f1*A_f1[1], a2_f1 = gain_f1*A_f1[2], b0_f1 = gain_f1*B_f1[0], b1_f1 = gain_f1*B_f1[1], b2_f1 = gain_f1*B_f1[2]; //filter coefficients filter 1
+const double a1_f2 = gain_f2*A_f1[1], a2_f2 = gain_f2*A_f2[2], b0_f2 = gain_f2*B_f2[0], b1_f2 = gain_f2*B_f2[1], b2_f2 = gain_f2*B_f2[2]; //filter coefficients filter 1
+
+
+//biquad filter
+double biquad( double u, double &v1, double &v2, const double a1, const double a2,
+               const double b0, const double b1, const double b2 )
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+///////////////////////////////////////////////////////////CONTROLLERS//////////////////////////////////////////////////////////
+const float motor1_pid_kp=0.5;
+
+//re usable P controller
+double p_control(float error,float kp)
+{
+    return (error*kp);
+}
+
+
+///////////////////////////////////////////////////////////GO-FLAGS AND ACTIVATION FUNCTIONS///////////////////////////////
+//go flags
+volatile bool scopedata_go=false, pid_control_go=false, filter_pot1_go=false;
+
+//acvitator functions
+
+void scopedata_activate()
+{
+    scopedata_go=true;
+}
+void pid_control_activate()
+{
+    pid_control_go=true;
+}
+void filter_pot1_activate()
+{
+    filter_pot1_go=true;
+}
+///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
+
+//scopedata
+void scopedata()
+{
+    scope.set(0,2*PI*pot1.read());
+    scope.set(1,counttorad*encoder1.getPulses());
+    scope.set(2,motor1_speed_control.read());
+    scope.send();
+}
+
+//////////////////////////////////////////////////MAIN////////////////////////////////////////////////////////////////////
+int main()
+{
+    //set initial shizzle
+    //motor1_speed_control.period(1.0/pwm_frequency);
+    motor1_speed_control.write(0);
+
+    //tickers
+    hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
+    pid_control_ticker.attach(&pid_control_activate,1.0/p_control_frequency);
+    filter_pot1_ticker.attach(&filter_pot1_activate,1.0/filter_pot1_frequency);
+
+    //make variables
+    volatile float signal_motor1=0;
+    volatile double pot1_filtered=0;
+    while(1) {
+        
+        //filter signal with low pass biquad filter
+        if (filter_pot1_go==true) {
+            double pass1, pass2;
+            pass1=biquad(pot1.read(),v1,v2,a1_f1, a2_f1, b0_f1, b1_f1, b2_f1);
+            pass2=biquad(pass1,v1,v2,a1_f2, a2_f2, b0_f2, b1_f2, b2_f2);
+            pot1_filtered=pass2;
+            filter_pot1_go=false;
+        }
+
+        //control motor 1 with a p controller
+        if (p_control_go==true) {
+            double error=2*PI*pot1_filtered-counttorad*encoder1.getPulses();
+            double signal_motor1=pid_control(error,motor1_pid_kp);//send error to pid controller
+            if (signal_motor1>=0) {//determine CW or CCW rotation
+                motor1_direction.write(CW);
+            } else {
+                motor1_direction.write(CCW);
+            }
+
+            if (fabs(signal_motor1)>=1) { //check if signal is <1
+                signal_motor1=1;//if signal >1 make it 1 to not damage motor
+            } else {
+                signal_motor1=fabs(signal_motor1);// if signal<1 use signal
+            }
+            motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
+            p_control_go=false;
+        }
+        //call scopedata
+        if (scopedata_go==true) {
+            scopedata();
+            scopedata_go=false;
+        }
+        return 0;
+    }
+}
\ No newline at end of file