Kinematics inclusief headers

Dependencies:   AnglePosition Encoder FastPWM MODSERIAL PIDController Servo SignalNumber2 biquadFilter mbed

Fork of kinematics_control by Peter Knoben

Files at this revision

API Documentation at this revision

Comitter:
peterknoben
Date:
Mon Oct 30 10:43:26 2017 +0000
Parent:
2:2ad485d762f5
Commit message:
kaldf

Changed in this revision

SignalNumber.lib Show annotated file Show diff for this revision Revisions of this file
biquadFilter.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 2ad485d762f5 -r c768a37620c9 SignalNumber.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SignalNumber.lib	Mon Oct 30 10:43:26 2017 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/peterknoben/code/SignalNumber2/#5f8dee4d4b09
diff -r 2ad485d762f5 -r c768a37620c9 biquadFilter.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/biquadFilter.lib	Mon Oct 30 10:43:26 2017 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305
diff -r 2ad485d762f5 -r c768a37620c9 main.cpp
--- a/main.cpp	Thu Oct 26 10:54:56 2017 +0000
+++ b/main.cpp	Mon Oct 30 10:43:26 2017 +0000
@@ -1,23 +1,91 @@
+#include "MODSERIAL.h"
 #include "AnglePosition.h"
 #include "PIDControl.h"
+#include "BiQuad.h"
+#include "signalnumber.h"
 #include "mbed.h"
 #include "encoder.h"
 #include "Servo.h"
-#include "MODSERIAL.h"
 #include "FastPWM.h"
 
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
-MODSERIAL pc(USBTX, USBRX);         //
+MODSERIAL pc(USBTX, USBRX);         //Establish connection
 Ticker MyControllerTicker1;         //Declare Ticker Motor 1
 Ticker MyControllerTicker2;         //Declare Ticker Motor 2
+Ticker MySampleTicker;                //Declare Ticker HIDscope
+Ticker MyTickerMean;                //Declare Ticker Signalprocessing
+
+InterruptIn But2(PTC6);             //Declare button for calibration
+
 AnglePosition Angle;                //Declare Angle calculater
 PIDControl PID;                     //Declare PID Controller
-AnalogIn potmeter1(A3);             //Set Inputpin
+SignalNumber Signal;                //Declare Signal determiner
+
+AnalogIn potmeter1(A5);
+AnalogIn potmeter5(A3);             //Set Inputpin
 AnalogIn potmeter2(A4);             //Set Inputpin
+AnalogIn    emg0( A0 );             //Set Inputpin
+AnalogIn    emg1( A1 );             //Set Inputpin
+
 
 const float CONTROLLER_TS = 0.02;   //Motor frequency
+const float MEAN_TS = 0.1;          //Mean value frequency
+const float SAMPLE_TS = 0.02;        //BiQuad frequency
+
+
+//------------------------------------------------------------------------------
+//-----------------------------EMG Signals--------------------------------------
+//------------------------------------------------------------------------------
+const int n = 10;                   //Window size for the mean value, also adjust in signalnumber.cpp
+const int action =50;               //Number of same mean values to change the signalnumber
+const int m = 10;                   //Number of samples for calibration
+int mode;
+float emg_offset;
+float meanx;
+
+//BiQuad filter
+BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
+BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
+BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
+BiQuadChain BiQuad_filter;
+
+// Calibration function
+void calibration(){
+    emg_offset = Signal.calibrate(m, potmeter5);
+    pc.printf("offset = %f \r\n", emg_offset);
+}
+
+/*
+//Sample function
+void sample()
+{
+    float Signal=((emg0+emg1)/2)-emg_offset;
+    float Signal_filtered= BiQuad_filter.step(Signal);
+    /* scope
+    scope.set(0, emg0.read() );
+    scope.set(1, emg1.read() );
+    scope.set(2, Signal_filtered);
+    scope.send();
+    led = !led;
+    /
+}
+*/
+
+float Filter(float input0, float input1, float offset){
+    float Signal=((input0+input1)/2)-offset;
+    float Signal_filtered= BiQuad_filter.step(Signal);
+    return Signal_filtered;
+}
+
+void signalnumber(){
+    float Signal_filtered = Filter(emg0, emg1, emg_offset);
+    meanx = Signal.getmean(n, Signal_filtered);
+    mode = Signal.getnumber(n, action, Signal_filtered);
+    pc.printf("pot = %d  after filtering = %f   mean = %f  Signalnumber = %i \r\n" , potmeter5,  meanx, mode);
+}
+
 
 //------------------------------------------------------------------------------
 //-------------------------Kinematic Constants----------------------------------
@@ -37,6 +105,7 @@
 //------------------------------------------------------------------------------
 //--------------------------------Servo-----------------------------------------
 //------------------------------------------------------------------------------
+// This will be programmed on a different Mbed
 Servo MyServo(D9);                  //Declare button
 InterruptIn But1(D8);               //Declare used button
 int k=0;                            //Position flag
@@ -76,8 +145,8 @@
     float error_alpha = reference_alpha-position_alpha;
     float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
     motor1 = fabs(magnitude1);
-    pc.printf("err_a = %f  alpha = %f   mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
-    pc.printf("\r\n");
+//    pc.printf("err_a = %f  alpha = %f   mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
+//    pc.printf("\r\n");
     // Determine Motor Direction 
     if (magnitude1  < 0){
         motor1DirectionPin = 1;
@@ -111,8 +180,8 @@
     float error_beta = reference_beta-position_beta;
     float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
     motor2 = fabs(magnitude2);
-    pc.printf("err2 = %f  beta = %f   mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
-    pc.printf("\r\n");
+//    pc.printf("err2 = %f  beta = %f   mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
+//    pc.printf("\r\n");
     //Determine Motor Direction
     if (magnitude2 > 0){
         motor2DirectionPin = 1;
@@ -127,11 +196,15 @@
 
 int main(){
     pc.baud(115200);
+    BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3);
     motor1.period(0.0001f);
     motor2.period(0.0001f);
     MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
     MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
+    MyTickerMean.attach(&signalnumber, MEAN_TS);
+//  MySampleTicker.attach(&sample, SAMPLE_TS);
     But1.rise(&servo_control);      
+    But2.rise(&calibration);
     while(1) {}   
 }