Peter Knoben
Kinematics inclusief headers
Dependencies: AnglePosition Encoder FastPWM MODSERIAL PIDController Servo SignalNumber2 biquadFilter mbed
Fork of
kinematics_control
by 
Peter Knoben
Revision 3:c768a37620c9, committed 2017-10-30
- Comitter:
- peterknoben
- Date:
- Mon Oct 30 10:43:26 2017 +0000
- Parent:
- 2:2ad485d762f5
- Commit message:
- kaldf
Changed in this revision
--- /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
--- /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
--- 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) {}
}