Diff: main.cpp

Revision:

1:917c07a4f3ec

Parent:

0:dd66fff537d7

Child:

2:c7707856d137

--- a/main.cpp	Fri Oct 09 12:46:10 2015 +0000
+++ b/main.cpp	Mon Oct 12 14:27:42 2015 +0000
@@ -1,13 +1,181 @@
 #include "mbed.h"
+#include "QEI.h"
+#include "HIDScope.h"
+#include "Biquad.h"
+#include "controlandadjust.h"
+
+//info out
+HIDScope scope(4);
+Ticker scope_ticker;
+const float scope_frequency=500;
+Serial pc(USBTX,USBRX);
+
+////////////////ENCODERS
+QEI encoder1(D12,D13,NC,32);
+QEI encoder2(D10,D11,NC,32);
+
+//////////////////////////////////CONTROLLER
+controlandadjust mycontroller; // make a controller
+//controller constants
+float Kp=0.5;
+float Ki=0.01;
+float Kd=0.001;
+Ticker control_ticker;
+const float control_frequency=25;
+
+double error1_int=0;// storage variables for the errors
+double error2_int=0;
+double error1_prev=0;
+double error2_prev=0;
+
+InterruptIn valuechangebutton(PTC6);//button to change controller constants
+
+//safetyandthreshold
+AnalogIn safety(A3);//pot 2, used for the safety cutoff value for the pwm
+AnalogIn threshold(A2);//pot1, used to adjust threshold if signal differs per person
+
+Ticker safetyandthreshold_ticker; // ticker to read potmeters
+const float safetyandthreshold_frequency=1; // frequency for the ticker
+
+////////////////////////////////FILTER
+#include "filtervalues.h"
+Ticker filter_ticker;
+const float filter_frequency=500;
+Biquad myfilter1;
+Biquad myfilter2;
+
+AnalogIn emg1_input(A0);
+AnalogIn emg2_input(A1);
+
+double filteredsignal1=0;
+double filteredsignal2=0;
+
+//////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
+volatile bool scopedata_go=false,
+              control_go=false,
+              filter_go=false,
+              safetyandthreshold_go=false;
+
+void scopedata_activate()
+{
+    scopedata_go=true;
+}
+void control_activate()
+{
+    control_go=true;
+}
+void filter_activate()
+{
+    filter_go=true;
+}
+void safetyandthreshold_activate()
+{
+    safetyandthreshold_go=true;
+}
 
-DigitalOut gpo(D0);
-DigitalOut led(LED_RED);
+
+////////////////////////FUNCTIONS
+//gather data and send to scope
+void scopedata()
+{
+    scope.set(0,encoder1.getPulses());
+    scope.set(1,encoder2.getPulses());
+}
+//read potmeters and adjust the safetyfactor and threshold
+void safetyandthreshold()
+{
+    mycontroller.cutoff((ceil (10*safety.read()) )/10); // adjust the safetyfactor rounded to 1 decimal
+    ///////////////////////NEED AN IDEA FOR THE THRESHOLD!!!!!!!!!!!!!!!!!!!!
+}
+/////filter
+void filtereverything()
+{
+    //filter_timer.reset();
+    // filter_timer.start();
+    //pass1 so f1
+    float pass1_emg1 = myfilter1.filter(emg1_input.read(), v1_f1_emg1 , v2_f1_emg1 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);
+    float pass1_emg2 = myfilter2.filter(emg2_input.read(), v1_f1_emg2 , v2_f1_emg2 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);
+
+    //pass2 so f2
+    float pass2_emg1 = myfilter1.filter(pass1_emg1, v1_f2_emg1 , v2_f2_emg1 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2);
+    float pass2_emg2 = myfilter2.filter(pass1_emg2, v1_f2_emg2 , v2_f2_emg2 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2);
+
+    //pass3 so f3
+    float pass3_emg1 = myfilter1.filter(pass2_emg1, v1_f3_emg1 , v2_f3_emg1 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3);
+    float pass3_emg2 = myfilter2.filter(pass2_emg2, v1_f3_emg2 , v2_f3_emg2 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3);
+
+    //pass4 so f4
+    float pass4_emg1 = myfilter1.filter(pass3_emg1, v1_f4_emg1 , v2_f4_emg1 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4);
+    float pass4_emg2 = myfilter2.filter(pass3_emg2, v1_f4_emg2 , v2_f4_emg2 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4);
+
+    //pass5 so f5
+    float pass5_emg1 = myfilter1.filter(pass4_emg1, v1_f5_emg1 , v2_f5_emg1 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5);
+    float pass5_emg2 = myfilter2.filter(pass4_emg2, v1_f5_emg2 , v2_f5_emg2 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5);
+
+    ///// take absolute value
+    float pass5_emg1_abs=(fabs(pass5_emg1));
+    float pass5_emg2_abs=(fabs(pass5_emg2));
+
+    //pass6 so f6
+    float pass6_emg1 = myfilter1.filter(pass5_emg1_abs, v1_f6_emg1 , v2_f6_emg1 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6);
+    float pass6_emg2 = myfilter2.filter(pass5_emg2_abs, v1_f6_emg2 , v2_f6_emg2 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6);
+
+
+    //pass7 so f7
+    float pass7_emg1 = myfilter1.filter(pass6_emg1, v1_f7_emg1 , v2_f7_emg1 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
+    float pass7_emg2 = myfilter2.filter(pass6_emg2, v1_f7_emg2 , v2_f7_emg2 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
+
+    filteredsignal1=(pass7_emg1*10000000);
+    filteredsignal2=(pass7_emg2*10000000);
+
+    //filter_timer.stop();
+}
+
+void control(){
+    ///call desired controller here
+    }
+
+//adjust controller values when sw2 is pressed
+void valuechange()
+{
+    mycontroller.STOP();
+    pc.printf("KP is now %f, enter new value\n",Kp);
+    pc.scanf("%f", &Kp);
+
+    pc.printf("KI is now %f, enter new value\n",Ki);
+    pc.scanf("%f", &Ki);
+
+    pc.printf("KD is now %f, enter new value\n",Kd);
+    pc.scanf("%f", &Kd);
+}
 
 int main()
 {
-    while (true) {
-        gpo = !gpo; // toggle pin
-        led = !led; // toggle led
-        wait(0.2f);
+    //tickers
+    safetyandthreshold_ticker.attach(&safetyandthreshold_activate,1.0/safetyandthreshold_frequency);
+    filter_ticker.attach(&filter_activate,1.0/filter_frequency);
+    control_ticker.attach(&control_activate,1.0/control_frequency);
+    scope_ticker.attach(&scopedata_activate,1.0/scope_frequency);
+
+    while(1) {
+        if (scopedata_go==true) {
+            scopedata();
+            scopedata_go=false;
+        }
+        if (filter_go==true) {
+            filtereverything();
+            filter_go=false;
+        }
+        if (safetyandthreshold_go==true) {
+            safetyandthreshold();
+            safetyandthreshold_go=false;
+        }
+        if (control_go==true) {
+            control();
+            control_go=false;
+        }
+        if (valuechangebutton==0) {
+            valuechange();
+        }
     }
 }
\ No newline at end of file