Tess Groeneveld
Version 1: coming together. With PD controller but without inverse kinematics
Dependencies: Encoder MODSERIAL mbed
Fork of
motoraansturing_met_EMG
by 
Jorick Leferink
Revision 1:1c22ce9f370b, committed 2013-11-01
- Comitter:
- Tess
- Date:
- Fri Nov 01 10:46:19 2013 +0000
- Parent:
- 0:73dcc8fb9900
- Child:
- 2:55bff07c1058
- Commit message:
- Version 1: coming together. Without inverse kinematics but with switch
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Nov 01 08:24:02 2013 +0000
+++ b/main.cpp Fri Nov 01 10:46:19 2013 +0000
@@ -3,44 +3,44 @@
#include "MODSERIAL.h"
//high pass filter constantes 15Hz cutoff 4e orde
- #define NUM0 0.2754 // constante
- #define NUM1 -1.1017 // z^-1
- #define NUM2 1.6525 // z^-2etc.
- #define NUM3 -1.1017
- #define NUM4 0.2754
+#define NUM0 0.2754 // constante
+#define NUM1 -1.1017 // z^-1
+#define NUM2 1.6525 // z^-2etc.
+#define NUM3 -1.1017
+#define NUM4 0.2754
- #define DEN0 1 // constante
- #define DEN1 -1.5704
- #define DEN2 1.2756
- #define DEN3 -0.4844
- #define DEN4 0.0762
+#define DEN0 1 // constante
+#define DEN1 -1.5704
+#define DEN2 1.2756
+#define DEN3 -0.4844
+#define DEN4 0.0762
//lowpass filter constantes 40 Hz 4e orde
- #define NUM0_2 0.4328 // constante
- #define NUM1_2 1.7314 // z^-1
- #define NUM2_2 2.5971 // z^-2etc.
- #define NUM3_2 1.7314
- #define NUM4_2 0.4328
+#define NUM0_2 0.4328 // constante
+#define NUM1_2 1.7314 // z^-1
+#define NUM2_2 2.5971 // z^-2etc.
+#define NUM3_2 1.7314
+#define NUM4_2 0.4328
- #define DEN0_2 1 // constante
- #define DEN1_2 2.3695
- #define DEN2_2 2.3140
- #define DEN3_2 1.0547
- #define DEN4_2 0.1874
+#define DEN0_2 1 // constante
+#define DEN1_2 2.3695
+#define DEN2_2 2.3140
+#define DEN3_2 1.0547
+#define DEN4_2 0.1874
//lowpass filter constantes 3Hz 4e orde
- #define NUM0_3 0.0000624 // constante
- #define NUM1_3 0.0002495 // z^-1
- #define NUM2_3 0.0003743 // z^-2etc.
- #define NUM3_3 0.0002495
- #define NUM4_3 0.0000624
+#define NUM0_3 0.0000624 // constante
+#define NUM1_3 0.0002495 // z^-1
+#define NUM2_3 0.0003743 // z^-2etc.
+#define NUM3_3 0.0002495
+#define NUM4_3 0.0000624
- #define DEN0_3 1 // constante
- #define DEN1_3 -3.5078
- #define DEN2_3 4.6409
- #define DEN3_3 -2.7427
- #define DEN4_3 0.6105
+#define DEN0_3 1 // constante
+#define DEN1_3 -3.5078
+#define DEN2_3 4.6409
+#define DEN3_3 -2.7427
+#define DEN4_3 0.6105
/*******************************************************************************
* *
@@ -48,191 +48,249 @@
* *
********************************************************************************/
-// dit is voor schakelaar die aan en uit wil gaan
-DigitalIn toggle(PTD7);
-
-void toggle_on()
-{
-}
-
-void toggle_off()
-{
- // do nothing
-}
-
/** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
void keep_in_range(float * in, float min, float max);
-/** variable to show when a new loop can be started*/
-/** volatile means that it can be changed in an */
-/** interrupt routine, and that that change is vis-*/
-/** ible in the main loop. */
+/** variable to show when a new loop can be started */
+/** volatile means that it can be changed in an interrupt routine, and that that change is visible in the main loop. */
volatile bool looptimerflag;
/** function called by Ticker "looptimer" */
-/** variable 'looptimerflag' is set to 'true' */
-/** each time the looptimer expires. */
+/** variable 'looptimerflag' is set to 'true' each time the looptimer expires.*/
void setlooptimerflag(void)
{
looptimerflag = true;
}
- //emg variabelen
- float emg_value_biceps, emg_value_triceps, emg_value_flexoren, emg_value_extensoren, dy;
- AnalogIn emg_biceps(PTB0); //Analog input
- AnalogIn emg_triceps(PTB1);
- AnalogIn emg_flexoren(PTB2);
- AnalogIn emg_extensoren(PTB3);
+//emg variabelen -> analoge input
+float emg_value_biceps, emg_value_triceps, emg_value_flexoren, emg_value_extensoren, dy;
+AnalogIn emg_biceps(PTB0);
+AnalogIn emg_triceps(PTB1);
+AnalogIn emg_flexoren(PTB2);
+AnalogIn emg_extensoren(PTB3);
/*
DIT IS DE FILTER FUNCTIE! aanroepen door "filter(signaal nummer)"
filter(1): biceps meten
filter(2): triceps meten
filter(3): flexoren meten
- filter(3): extensoren meten
+ filter(3): extensoren meten
*/
-float filter(int sig_number){
+float filter(int sig_number)
+{
float sig_out;
-
- // eerst variabelen definieren
-
+
+ // eerst variabelen definiëren
+
//biceps
- //filter 1
+ //filter 1
float in0_biceps =0;
static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0;
static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0;
- //filter 2
+ //filter 2
float in0_2_biceps =0;
static float in1_2_biceps =0, in2_2_biceps = 0, in3_2_biceps = 0, in4_2_biceps = 0;
static float out0_2_biceps = 0, out1_2_biceps = 0 , out2_2_biceps = 0, out3_2_biceps = 0, out4_2_biceps = 0;
- //filter 3
+ //filter 3
float in0_3_biceps =0;
static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0;
static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0;
-
+
//triceps
- //filter 1
+ //filter 1
float in0_triceps =0;
static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0;
static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0;
- //filter 2
+ //filter 2
float in0_2_triceps =0;
static float in1_2_triceps =0, in2_2_triceps = 0, in3_2_triceps = 0, in4_2_triceps = 0;
static float out0_2_triceps = 0, out1_2_triceps = 0 , out2_2_triceps = 0, out3_2_triceps = 0, out4_2_triceps = 0;
- //filter 3
+ //filter 3
float in0_3_triceps =0;
static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0;
static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0;
-
+
//flexoren
- //filter 1
+ //filter 1
float in0_flexoren =0;
static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0;
static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0;
- //filter 2
+ //filter 2
float in0_2_flexoren =0;
static float in1_2_flexoren =0, in2_2_flexoren = 0, in3_2_flexoren = 0, in4_2_flexoren = 0;
static float out0_2_flexoren = 0, out1_2_flexoren = 0 , out2_2_flexoren = 0, out3_2_flexoren = 0, out4_2_flexoren = 0;
- //filter 3
+ //filter 3
float in0_3_flexoren =0;
static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0;
static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0;
-
+
//extensoren
- //filter 1
+ //filter 1
float in0_extensoren =0;
static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0;
static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0;
- //filter 2
+ //filter 2
float in0_2_extensoren =0;
static float in1_2_extensoren =0, in2_2_extensoren = 0, in3_2_extensoren = 0, in4_2_extensoren = 0;
static float out0_2_extensoren = 0, out1_2_extensoren = 0 , out2_2_extensoren = 0, out3_2_extensoren = 0, out4_2_extensoren = 0;
- //filter 3
+ //filter 3
float in0_3_extensoren =0;
static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0;
static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0;
-
-
- switch(sig_number){
- case 1:
+
+
+ switch(sig_number) {
+ case 1:
// signaal filteren op 15 Hz HIGHPASS
- in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps;
+ in4_biceps = in3_biceps;
+ in3_biceps = in2_biceps;
+ in2_biceps = in1_biceps;
+ in1_biceps = in0_biceps;
in0_biceps = emg_biceps.read();
- out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps;
- out0_biceps = (NUM0*in0_biceps + NUM1*in1_biceps + NUM2*in2_biceps + NUM3*in3_biceps + NUM4*in4_biceps - DEN1*out1_biceps - DEN2*out2_biceps - DEN3*out3_biceps - DEN4*out4_biceps ) / DEN0;
-
+ out4_biceps = out3_biceps;
+ out3_biceps = out2_biceps;
+ out2_biceps = out1_biceps;
+ out1_biceps = out0_biceps;
+ out0_biceps = (NUM0*in0_biceps + NUM1*in1_biceps + NUM2*in2_biceps + NUM3*in3_biceps + NUM4*in4_biceps - DEN1*out1_biceps - DEN2*out2_biceps - DEN3*out3_biceps - DEN4*out4_biceps ) / DEN0;
+
//signaal filteren op 40 HZ LOWPASS
- in4_2_biceps = in3_2_biceps; in3_2_biceps = in2_2_biceps; in2_2_biceps = in1_2_biceps; in1_2_biceps = in0_2_biceps;
+ in4_2_biceps = in3_2_biceps;
+ in3_2_biceps = in2_2_biceps;
+ in2_2_biceps = in1_2_biceps;
+ in1_2_biceps = in0_2_biceps;
in0_2_biceps = out0_biceps;
- out4_2_biceps = out3_2_biceps; out3_2_biceps = out2_2_biceps; out2_2_biceps = out1_2_biceps; out1_2_biceps = out0_2_biceps;
+ out4_2_biceps = out3_2_biceps;
+ out3_2_biceps = out2_2_biceps;
+ out2_2_biceps = out1_2_biceps;
+ out1_2_biceps = out0_2_biceps;
out0_2_biceps = (NUM0_2*in0_2_biceps + NUM1_2*in1_2_biceps + NUM2_2*in2_2_biceps + NUM3_2*in3_2_biceps + NUM4_2*in4_2_biceps - DEN1_2*out1_2_biceps - DEN2_2*out2_2_biceps - DEN3_2*out3_2_biceps - DEN4_2*out4_2_biceps ) / DEN0_2;
-
+
//signaal filteren op 5Hz LOWPASS
- in4_3_biceps = in3_3_biceps; in3_3_biceps = in2_3_biceps; in2_3_biceps = in1_3_biceps; in1_3_biceps = in0_3_biceps;
+ in4_3_biceps = in3_3_biceps;
+ in3_3_biceps = in2_3_biceps;
+ in2_3_biceps = in1_3_biceps;
+ in1_3_biceps = in0_3_biceps;
in0_3_biceps = abs(out0_2_biceps);
- out4_3_biceps = out3_3_biceps; out3_3_biceps = out2_3_biceps; out2_3_biceps = out1_3_biceps; out1_3_biceps = out0_3_biceps;
- out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3;
+ out4_3_biceps = out3_3_biceps;
+ out3_3_biceps = out2_3_biceps;
+ out2_3_biceps = out1_3_biceps;
+ out1_3_biceps = out0_3_biceps;
+ out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3;
sig_out = out0_3_biceps;
break;
case 2:
// signaal filteren op 15 Hz HIGHPASS
- in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps;
+ in4_triceps = in3_triceps;
+ in3_triceps = in2_triceps;
+ in2_triceps = in1_triceps;
+ in1_triceps = in0_triceps;
in0_triceps = emg_triceps.read();
- out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps;
- out0_triceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0;
-
+ out4_triceps = out3_triceps;
+ out3_triceps = out2_triceps;
+ out2_triceps = out1_triceps;
+ out1_triceps = out0_triceps;
+ out0_triceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0;
+
//signaal filteren op 40 HZ LOWPASS
- in4_2_triceps = in3_2_triceps; in3_2_triceps = in2_2_triceps; in2_2_triceps = in1_2_triceps; in1_2_triceps = in0_2_triceps;
+ in4_2_triceps = in3_2_triceps;
+ in3_2_triceps = in2_2_triceps;
+ in2_2_triceps = in1_2_triceps;
+ in1_2_triceps = in0_2_triceps;
in0_2_triceps = out0_triceps;
- out4_2_triceps = out3_2_triceps; out3_2_triceps = out2_2_triceps; out2_2_triceps = out1_2_triceps; out1_2_triceps = out0_2_triceps;
+ out4_2_triceps = out3_2_triceps;
+ out3_2_triceps = out2_2_triceps;
+ out2_2_triceps = out1_2_triceps;
+ out1_2_triceps = out0_2_triceps;
out0_2_triceps = (NUM0_2*in0_2_triceps + NUM1_2*in1_2_triceps + NUM2_2*in2_2_triceps + NUM3_2*in3_2_triceps + NUM4_2*in4_2_triceps - DEN1_2*out1_2_triceps - DEN2_2*out2_2_triceps - DEN3_2*out3_2_triceps - DEN4_2*out4_2_triceps ) / DEN0_2;
-
+
//signaal filteren op 5Hz LOWPASS
- in4_3_triceps = in3_3_triceps; in3_3_triceps = in2_3_triceps; in2_3_triceps = in1_3_triceps; in1_3_triceps = in0_3_triceps;
+ in4_3_triceps = in3_3_triceps;
+ in3_3_triceps = in2_3_triceps;
+ in2_3_triceps = in1_3_triceps;
+ in1_3_triceps = in0_3_triceps;
in0_3_triceps = abs(out0_2_triceps);
- out4_3_triceps = out3_3_triceps; out3_3_triceps = out2_3_triceps; out2_3_triceps = out1_3_triceps; out1_3_triceps = out0_3_triceps;
- out0_3_triceps = (NUM0_3*in0_3_triceps + NUM1_3*in1_3_triceps + NUM2_3*in2_3_triceps + NUM3_3*in3_3_triceps + NUM4_3*in4_3_triceps - DEN1_3*out1_3_triceps - DEN2_3*out2_3_triceps - DEN3_3*out3_3_triceps - DEN4_3*out4_3_triceps ) / DEN0_3;
+ out4_3_triceps = out3_3_triceps;
+ out3_3_triceps = out2_3_triceps;
+ out2_3_triceps = out1_3_triceps;
+ out1_3_triceps = out0_3_triceps;
+ out0_3_triceps = (NUM0_3*in0_3_triceps + NUM1_3*in1_3_triceps + NUM2_3*in2_3_triceps + NUM3_3*in3_3_triceps + NUM4_3*in4_3_triceps - DEN1_3*out1_3_triceps - DEN2_3*out2_3_triceps - DEN3_3*out3_3_triceps - DEN4_3*out4_3_triceps ) / DEN0_3;
sig_out = out0_3_triceps;
break;
case 3:
// signaal filteren op 15 Hz HIGHPASS
- in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren;
+ in4_flexoren = in3_flexoren;
+ in3_flexoren = in2_flexoren;
+ in2_flexoren = in1_flexoren;
+ in1_flexoren = in0_flexoren;
in0_flexoren = emg_flexoren.read();
- out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren;
- out0_flexoren = (NUM0*in0_flexoren + NUM1*in1_flexoren + NUM2*in2_flexoren + NUM3*in3_flexoren + NUM4*in4_flexoren - DEN1*out1_flexoren - DEN2*out2_flexoren - DEN3*out3_flexoren - DEN4*out4_flexoren ) / DEN0;
-
+ out4_flexoren = out3_flexoren;
+ out3_flexoren = out2_flexoren;
+ out2_flexoren = out1_flexoren;
+ out1_flexoren = out0_flexoren;
+ out0_flexoren = (NUM0*in0_flexoren + NUM1*in1_flexoren + NUM2*in2_flexoren + NUM3*in3_flexoren + NUM4*in4_flexoren - DEN1*out1_flexoren - DEN2*out2_flexoren - DEN3*out3_flexoren - DEN4*out4_flexoren ) / DEN0;
+
//signaal filteren op 40 HZ LOWPASS
- in4_2_flexoren = in3_2_flexoren; in3_2_flexoren = in2_2_flexoren; in2_2_flexoren = in1_2_flexoren; in1_2_flexoren = in0_2_flexoren;
+ in4_2_flexoren = in3_2_flexoren;
+ in3_2_flexoren = in2_2_flexoren;
+ in2_2_flexoren = in1_2_flexoren;
+ in1_2_flexoren = in0_2_flexoren;
in0_2_flexoren = out0_flexoren;
- out4_2_flexoren = out3_2_flexoren; out3_2_flexoren = out2_2_flexoren; out2_2_flexoren = out1_2_flexoren; out1_2_flexoren = out0_2_flexoren;
+ out4_2_flexoren = out3_2_flexoren;
+ out3_2_flexoren = out2_2_flexoren;
+ out2_2_flexoren = out1_2_flexoren;
+ out1_2_flexoren = out0_2_flexoren;
out0_2_flexoren = (NUM0_2*in0_2_flexoren + NUM1_2*in1_2_flexoren + NUM2_2*in2_2_flexoren + NUM3_2*in3_2_flexoren + NUM4_2*in4_2_flexoren - DEN1_2*out1_2_flexoren - DEN2_2*out2_2_flexoren - DEN3_2*out3_2_flexoren - DEN4_2*out4_2_flexoren ) / DEN0_2;
-
+
//signaal filteren op 5Hz LOWPASS
- in4_3_flexoren = in3_3_flexoren; in3_3_flexoren = in2_3_flexoren; in2_3_flexoren = in1_3_flexoren; in1_3_flexoren = in0_3_flexoren;
+ in4_3_flexoren = in3_3_flexoren;
+ in3_3_flexoren = in2_3_flexoren;
+ in2_3_flexoren = in1_3_flexoren;
+ in1_3_flexoren = in0_3_flexoren;
in0_3_flexoren = abs(out0_2_flexoren);
- out4_3_flexoren = out3_3_flexoren; out3_3_flexoren = out2_3_flexoren; out2_3_flexoren = out1_3_flexoren; out1_3_flexoren = out0_3_flexoren;
- out0_3_flexoren = (NUM0_3*in0_3_flexoren + NUM1_3*in1_3_flexoren + NUM2_3*in2_3_flexoren + NUM3_3*in3_3_flexoren + NUM4_3*in4_3_flexoren - DEN1_3*out1_3_flexoren - DEN2_3*out2_3_flexoren - DEN3_3*out3_3_flexoren - DEN4_3*out4_3_flexoren ) / DEN0_3;
+ out4_3_flexoren = out3_3_flexoren;
+ out3_3_flexoren = out2_3_flexoren;
+ out2_3_flexoren = out1_3_flexoren;
+ out1_3_flexoren = out0_3_flexoren;
+ out0_3_flexoren = (NUM0_3*in0_3_flexoren + NUM1_3*in1_3_flexoren + NUM2_3*in2_3_flexoren + NUM3_3*in3_3_flexoren + NUM4_3*in4_3_flexoren - DEN1_3*out1_3_flexoren - DEN2_3*out2_3_flexoren - DEN3_3*out3_3_flexoren - DEN4_3*out4_3_flexoren ) / DEN0_3;
sig_out = out0_3_flexoren;
break;
case 4:
// signaal filteren op 15 Hz HIGHPASS
- in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren;
+ in4_extensoren = in3_extensoren;
+ in3_extensoren = in2_extensoren;
+ in2_extensoren = in1_extensoren;
+ in1_extensoren = in0_extensoren;
in0_extensoren = emg_extensoren.read();
- out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren;
- out0_extensoren = (NUM0*in0_extensoren + NUM1*in1_extensoren + NUM2*in2_extensoren + NUM3*in3_extensoren + NUM4*in4_extensoren - DEN1*out1_extensoren - DEN2*out2_extensoren - DEN3*out3_extensoren - DEN4*out4_extensoren ) / DEN0;
-
+ out4_extensoren = out3_extensoren;
+ out3_extensoren = out2_extensoren;
+ out2_extensoren = out1_extensoren;
+ out1_extensoren = out0_extensoren;
+ out0_extensoren = (NUM0*in0_extensoren + NUM1*in1_extensoren + NUM2*in2_extensoren + NUM3*in3_extensoren + NUM4*in4_extensoren - DEN1*out1_extensoren - DEN2*out2_extensoren - DEN3*out3_extensoren - DEN4*out4_extensoren ) / DEN0;
+
//signaal filteren op 40 HZ LOWPASS
- in4_2_extensoren = in3_2_extensoren; in3_2_extensoren = in2_2_extensoren; in2_2_extensoren = in1_2_extensoren; in1_2_extensoren = in0_2_extensoren;
+ in4_2_extensoren = in3_2_extensoren;
+ in3_2_extensoren = in2_2_extensoren;
+ in2_2_extensoren = in1_2_extensoren;
+ in1_2_extensoren = in0_2_extensoren;
in0_2_extensoren = out0_extensoren;
- out4_2_extensoren = out3_2_extensoren; out3_2_extensoren = out2_2_extensoren; out2_2_extensoren = out1_2_extensoren; out1_2_extensoren = out0_2_extensoren;
+ out4_2_extensoren = out3_2_extensoren;
+ out3_2_extensoren = out2_2_extensoren;
+ out2_2_extensoren = out1_2_extensoren;
+ out1_2_extensoren = out0_2_extensoren;
out0_2_extensoren = (NUM0_2*in0_2_extensoren + NUM1_2*in1_2_extensoren + NUM2_2*in2_2_extensoren + NUM3_2*in3_2_extensoren + NUM4_2*in4_2_extensoren - DEN1_2*out1_2_extensoren - DEN2_2*out2_2_extensoren - DEN3_2*out3_2_extensoren - DEN4_2*out4_2_extensoren ) / DEN0_2;
-
+
//signaal filteren op 5Hz LOWPASS
- in4_3_extensoren = in3_3_extensoren; in3_3_extensoren = in2_3_extensoren; in2_3_extensoren = in1_3_extensoren; in1_3_extensoren = in0_3_extensoren;
+ in4_3_extensoren = in3_3_extensoren;
+ in3_3_extensoren = in2_3_extensoren;
+ in2_3_extensoren = in1_3_extensoren;
+ in1_3_extensoren = in0_3_extensoren;
in0_3_extensoren = abs(out0_2_extensoren);
- out4_3_extensoren = out3_3_extensoren; out3_3_extensoren = out2_3_extensoren; out2_3_extensoren = out1_3_extensoren; out1_3_extensoren = out0_3_extensoren;
- out0_3_extensoren = (NUM0_3*in0_3_extensoren + NUM1_3*in1_3_extensoren + NUM2_3*in2_3_extensoren + NUM3_3*in3_3_extensoren + NUM4_3*in4_3_extensoren - DEN1_3*out1_3_extensoren - DEN2_3*out2_3_extensoren - DEN3_3*out3_3_extensoren - DEN4_3*out4_3_extensoren ) / DEN0_3;
+ out4_3_extensoren = out3_3_extensoren;
+ out3_3_extensoren = out2_3_extensoren;
+ out2_3_extensoren = out1_3_extensoren;
+ out1_3_extensoren = out0_3_extensoren;
+ out0_3_extensoren = (NUM0_3*in0_3_extensoren + NUM1_3*in1_3_extensoren + NUM2_3*in2_3_extensoren + NUM3_3*in3_3_extensoren + NUM4_3*in4_3_extensoren - DEN1_3*out1_3_extensoren - DEN2_3*out2_3_extensoren - DEN3_3*out3_3_extensoren - DEN4_3*out4_3_extensoren ) / DEN0_3;
sig_out = out0_3_extensoren;
break;
}
@@ -242,23 +300,14 @@
int main()
{
//LOCAL VARIABLES
- /*Potmeter input*/
- AnalogIn potmeterA(PTC2);
- AnalogIn potmeterB(PTB2);
- /* Encoder, using my encoder library */
- /* First pin should be PTDx or PTAx */
- /* because those pins can be used as */
- /* InterruptIn */
- Encoder motorA(PTD4,PTC8);
+ Encoder motorA(PTD4,PTC8); // encoder, using my encoder library
Encoder motorB(PTD0,PTD2);
- /* MODSERIAL to get non-blocking Serial*/
- MODSERIAL pc(USBTX,USBRX);
- /* PWM control to motor */
- PwmOut pwm_motorA(PTA12);
- PwmOut pwm_motorB(PTA5);
- /* Direction pin */
- DigitalOut motordirA(PTD3);
- DigitalOut motordirB(PTD1);
+ MODSERIAL pc(USBTX,USBRX); // MODSERIAL to get non-blocking Serial
+ PwmOut pwm_motorA(PTA12); // PWM control to motor
+ PwmOut pwm_motorB(PTA5); // PWM control to motor
+ DigitalOut motordirA(PTD3); // Direction pin
+ DigitalOut motordirB(PTD1); // Direction pin
+
/* variable to store setpoint in */
float setpointA;
float setpointB;
@@ -274,30 +323,23 @@
float pwm_to_motorB;
float pwm_to_rechtsonder_motorA;
float pwm_to_rechtsonder_motorB;
-
+
+ /* variable for PD controller*/
const float dt = 0.002;
- float Kp = 0.001; //0.0113
- float Ki = 0.0759;
- float Kd = 0.0004342;
+ float Kp = 0.001; //0.0208
+ float Kd = 0.00004342; //0.0006897
float error_t0_A = 0;
float error_t0_B = 0;
float error_ti_A;
float error_ti_B;
- float error_t_1_A;
- float error_t_1_B;
float P_regelaar_A;
float P_regelaar_B;
- float I_regelaar_A;
- float I_regelaar_B;
float D_regelaar_A;
float D_regelaar_B;
float output_regelaar_A;
float output_regelaar_B;
- float integral_i_A;
- float integral_i_B;
- float integral_0_A = 0;
- float integral_0_B = 0;
+ /* variable to store positions in*/
int32_t positionmotorA_t0;
int32_t positionmotorB_t0;
int32_t positionmotorA_t_1;
@@ -307,223 +349,193 @@
//START OF CODE
- while(1) {
- while(!toggle);
- { // wait while toggle == 0
- toggle_on();
+ pc.baud(115200); // Set the baudrate (use this number in RealTerm too!)
+
+ // In dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen.
+ // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).
- /*Set the baudrate (use this number in RealTerm too!) */
- pc.baud(115200);
-
- // in dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen.
- // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).
+ motordirB.write(0);
+ pwm_motorB.write(.08);
+ positionmotorB_t0 = motorB.getPosition();
+ do {
+ wait(0.2);
+ positionmotorB_t_1 = positionmotorB_t0 ;
+ positionmotorB_t0 = motorB.getPosition();
+ positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
+ } while(positiondifference_motorB > 10);
+ motorB.setPosition(0);
+ pwm_motorB.write(0);
- motordirB.write(0);
- pwm_motorB.write(.08);
- positionmotorB_t0 = motorB.getPosition();
- do {
- wait(0.2);
- positionmotorB_t_1 = positionmotorB_t0 ;
- positionmotorB_t0 = motorB.getPosition();
- positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
- } while(positiondifference_motorB > 10);
- motorB.setPosition(0);
- pwm_motorB.write(0);
-
- wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+ wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
- motordirA.write(1);
- pwm_motorA.write(.08);
- positionmotorA_t0 = motorA.getPosition();
- do {
- wait(0.2);
- positionmotorA_t_1 = positionmotorA_t0 ;
- positionmotorA_t0 = motorA.getPosition();
- positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
- } while(positiondifference_motorA > 10);
- motorA.setPosition(0);
- pwm_motorA.write(0);
+ motordirA.write(1);
+ pwm_motorA.write(.08);
+ positionmotorA_t0 = motorA.getPosition();
+ do {
+ wait(0.2);
+ positionmotorA_t_1 = positionmotorA_t0 ;
+ positionmotorA_t0 = motorA.getPosition();
+ positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
+ } while(positiondifference_motorA > 10);
+ motorA.setPosition(0);
+ pwm_motorA.write(0);
- wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+ wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
- // Hierna willen we de motor van zijn alleruiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links.
+ // Hierna willen we de motor van zijn alleruiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links.
- motordirA.write(0);
- pwm_motorA.write(.08);
- do {
- setpoint_beginA = -63; // x-as
- pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); // + omdat men met een negatieve hoekverdraaiing werkt.
- wait(0.2);
- keep_in_range(&pwm_to_begin_motorA, -1, 1 );
- motordirA.write(0);
- pwm_motorA.write(pwm_to_begin_motorA);
- } while(pwm_to_begin_motorA <= 0);
- motorA.setPosition(0);
- pwm_motorA.write(0);
-
- wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+ motordirA.write(0);
+ pwm_motorA.write(.08);
+ do {
+ setpoint_beginA = -63; // x-as
+ pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); // + omdat men met een negatieve hoekverdraaiing werkt.
+ wait(0.2);
+ keep_in_range(&pwm_to_begin_motorA, -1, 1 );
+ motordirA.write(0);
+ pwm_motorA.write(pwm_to_begin_motorA);
+ } while(pwm_to_begin_motorA <= 0);
+ motorA.setPosition(0);
+ pwm_motorA.write(0);
- // hierna moet motor A naar de rechtsonder A4. Motor A 532.
+ wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
- motordirA.write(0);
- pwm_motorA.write(0.08);
- do {
- setpoint_beginA = -532; // rechtsonder positie A4
- pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);
- wait(0.2);
- keep_in_range(&pwm_to_begin_motorA, -1, 1 );
- motordirA.write(0);
- pwm_motorA.write(pwm_to_begin_motorA);
- } while(pwm_to_begin_motorA <= 0);
- pwm_motorA.write(0);
-
- wait(1);
+ // hierna moet motor A naar de rechtsonder A4. Motor A 532.
- // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.
+ motordirA.write(0);
+ pwm_motorA.write(0.08);
+ do {
+ setpoint_rechtsonderA = -532; // rechtsonder positie A4
+ pwm_to_rechtsonder_motorA = abs((setpoint_rechtsonderA + motorA.getPosition()) *.001);
+ wait(0.2);
+ keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
+ motordirA.write(0);
+ pwm_motorA.write(pwm_to_rechtsonder_motorA);
+ } while(pwm_to_rechtsonder_motorA <= 0);
+ pwm_motorA.write(0);
- motordirB.write(1);
- pwm_motorB.write(.08);
- do {
- setpoint_beginB = 192; // x-as
- pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
- wait(0.2);
- keep_in_range(&pwm_to_begin_motorB, -1, 1 );
- motordirB.write(1);
- pwm_motorB.write(pwm_to_begin_motorB);
- } while(pwm_to_begin_motorB <= 0);
- motorB.setPosition(0);
- pwm_motorB.write(0);
+ wait(1);
+
+ // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.
- wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
-
- // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
+ motordirB.write(1);
+ pwm_motorB.write(.08);
+ do {
+ setpoint_beginB = 192; // x-as
+ pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
+ wait(0.2);
+ keep_in_range(&pwm_to_begin_motorB, -1, 1 );
+ motordirB.write(1);
+ pwm_motorB.write(pwm_to_begin_motorB);
+ } while(pwm_to_begin_motorB <= 0);
+ motorB.setPosition(0);
+ pwm_motorB.write(0);
- motordirB.write(1);
- pwm_motorB.write(0.08);
- do {
- setpoint_beginB = 460; // rechtsonder positie A4
- pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
- wait(0.2);
- keep_in_range(&pwm_to_begin_motorB, -1, 1 );
- motordirB.write(1);
- pwm_motorB.write(pwm_to_begin_motorB);
- } while(pwm_to_begin_motorB <= 0);
- pwm_motorB.write(0);
+ wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
- wait(1);
+ // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
- // Nu zijn de motoren gekalibreed en staan ze op de startpositie.
- // Hierna het script dat EMG wordt omgezet in een positie verandering
+ motordirB.write(1);
+ pwm_motorB.write(0.08);
+ do {
+ setpoint_rechtsonderB = 460; // rechtsonder positie A4
+ pwm_to_rechtsonder_motorB = abs((setpoint_rechtsonderB - motorB.getPosition()) *.001);
+ wait(0.2);
+ keep_in_range(&pwm_to_rechtsonder_motorB, -1, 1 );
+ motordirB.write(1);
+ pwm_motorB.write(pwm_to_rechtsonder_motorB);
+ } while(pwm_to_rechtsonder_motorB <= 0);
+ pwm_motorB.write(0);
- /*Create a ticker, and let it call the */
- /*function 'setlooptimerflag' every 0.01s */
- Ticker looptimer;
- looptimer.attach(setlooptimerflag,0.01);
+ wait(1);
+
+ // Nu zijn de motoren gekalibreed en staan ze op de startpositie (rechtsonderhoek van A4).
+ // Hierna het script dat EMG wordt omgezet in een positie verandering
- //INFINITE LOOP
- while(1) {
+ /*Create a ticker, and let it call the function 'setlooptimerflag' every 0.01s */
+ Ticker looptimer;
+ looptimer.attach(setlooptimerflag,0.01);
- while(looptimerflag != true);
- looptimerflag = false;
+ //INFINITE LOOP
+ while(1) {
+
+ while(looptimerflag != true);
+ looptimerflag = false;
- // HIER EMG!!
-float emg_value_biceps;
- float emg_value_triceps;
- float emg_value_flexoren;
- float emg_value_extensoren;
- float dy;
- emg_value_biceps = ((100*(filter(1))-0.18)/0.49);
- emg_value_triceps = ((100*(filter(2))-0.18)/0.35);
- //emg_value_flexoren = 100*filter(3);
- //emg_value_extensoren = 100*filter(4);
-
- if(emg_value_biceps < 0.10){
- emg_value_biceps=0;
- }
- else {
- emg_value_biceps = emg_value_biceps;
- }
- if(emg_value_triceps < 0.20){
- emg_value_triceps=0;
- }
- else {
- emg_value_triceps=emg_value_triceps;
- }
+ // HIER EMG!!
+ float emg_value_biceps;
+ float emg_value_triceps;
+ float emg_value_flexoren;
+ float emg_value_extensoren;
+ float dy; // verschil tussen biceps en triceps, daarmee snelheid en richting aangeven.
+
+ emg_value_biceps = ((100*(filter(1))-0.18)/0.49); // dit is om waarde tussen 0 en 1 te krijgen. filter(1) zegt biceps, 0.18 offset aftrekken, 0.49 maximale waarde, 100 omdat procent
+ emg_value_triceps = ((100*(filter(2))-0.18)/0.35); // 0.35 maximale waarde van triceps
+ //emg_value_flexoren = 100*filter(3);
+ //emg_value_extensoren = 100*filter(4);
+ if(emg_value_biceps < 0.10) { // lager dan 10% doe dan niks - threshold
+ emg_value_biceps=0;
+ } else {
+ emg_value_biceps = emg_value_biceps;
+ }
+ if(emg_value_triceps < 0.20) { // lager dan 20% doe dan niks - threshold - triceps fluctueren meer
+ emg_value_triceps=0;
+ } else {
+ emg_value_triceps=emg_value_triceps;
+ }
-
- dy = emg_value_biceps-emg_value_triceps;
- dy=dy*10;
- if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
- pc.printf("%.6f\n",dy);
-
-
-
-
- //setpointA = (potmeterA.read()-0.09027)*(631); // bereik van 71 graden dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen bij EMG wordt 0.5 - 0.09027
- //setpointB = (potmeterB.read())*(415); // bereik van 46.7 graden
- //pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
- //pc.printf("s: %f, %d ", setpointB, motorB.getPosition());
+ dy = emg_value_biceps - emg_value_triceps;
+ dy = dy * 10; // Waarde om beter te zien, officieel zonder *10 -> straks veranderen
+ if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
+ pc.printf("%.6f\n",dy);
+
+ setpointB = (dy) * 415; // 451 staat voor een verdraaiing van 46.7 graden. Dit wil men met de uiteindelijke robot
+ setpointA = (dy) * 631; // 631 staat voor een verdraaiing van 71 graden. Dit wil men met de uiteindelijke robot.
+ // Zelf vraag ik mij af of de verdraaiing wordt genomen vanaf nul of niet.
- setpointB = (dy);
- //setpointB = (potmeterB.read() - 0.5) * (871/2);
- // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
- // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
- keep_in_range(&setpointA, -1105, -474); // voor motor moet bereik zijn -1105 tot -474
- keep_in_range(&setpointB, -147, 269); // voor motor moet bereik zijn -147 tot 269
+ /* motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
+ // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
+ keep_in_range(&setpointA, -1105, -474); // voor motor moet bereik zijn -1105 tot -474
+ keep_in_range(&setpointB, -147, 269); // voor motor moet bereik zijn -147 tot 269 */
+ // Dit moeten we testen!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
- // PID regelaar voor motor A
- //wait(dt);
- //error_ti_A = setpointA - motorA.getPosition();
- //P_regelaar_A = Kp * error_ti_A;
- //D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
- //integral_i_A = integral_0_A + (error_ti_A * dt);
- //I_regelaar_A = Ki * integral_i_A;
- //integral_0_A = integral_i_A;
- //error_t0_A = error_ti_A;
- //output_regelaar_A = P_regelaar_A;
+ //PD regelaar voor motor A
+ wait(dt);
+ error_ti_A = setpointA - motorA.getPosition();
+ P_regelaar_A = Kp * error_ti_A;
+ D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
+ error_t0_A = error_ti_A;
+ output_regelaar_A = P_regelaar_A + D_regelaar_A;
- // PID regelaar voor motor B
- //wait(dt);
- //error_ti_B = setpointB - motorB.getPosition();
- //P_regelaar_B = Kp * error_ti_B;
- //D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
- //integral_i_B = integral_0_B + (error_ti_B * dt);
- //I_regelaar_B = Ki * integral_i_B;
- //integral_0_B = integral_i_B;
- //error_t0_B = error_ti_B;
- //output_regelaar_B = P_regelaar_B;
+ //PD regelaar voor motor B
+ wait(dt);
+ error_ti_B = setpointB - motorB.getPosition();
+ P_regelaar_B = Kp * error_ti_B;
+ D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
+ error_t0_B = error_ti_B;
+ output_regelaar_B = P_regelaar_B + D_regelaar_B;
- /* This is a PID-action! store in pwm_to_motor */
- pwm_to_motorA = (setpointA - motorA.getPosition())*.001; //output_regelaar_A;
- pwm_to_motorB = (setpointB); //- motorB.getPosition())*.001; //output_regelaar_B;
-
- keep_in_range(&pwm_to_motorA, -1,1);
- keep_in_range(&pwm_to_motorB, -1,1);
+ /* This is a PD-action! store in pwm_to_motor */
+ pwm_to_motorA = output_regelaar_A;
+ pwm_to_motorB = output_regelaar_B;
- if(pwm_to_motorA > 0)
- motordirA.write(1);
- else
- motordirA.write(0);
- if(pwm_to_motorB > 0)
- motordirB.write(1);
- else
- motordirB.write(0);
+ keep_in_range(&pwm_to_motorA, -1,1);
+ keep_in_range(&pwm_to_motorB, -1,1);
- pwm_motorA.write(abs(pwm_to_motorA));
- pwm_motorB.write(abs(pwm_to_motorB));
- }
- }
- while(toggle);
- { // wait while toggle == 1
- toggle_off();
- pwm_motorA.write(0);
- pwm_motorB.write(0);
- }
+ if(pwm_to_motorA > 0)
+ motordirA.write(1);
+ else
+ motordirA.write(0);
+ if(pwm_to_motorB > 0)
+ motordirB.write(1);
+ else
+ motordirB.write(0);
+
+ pwm_motorA.write(abs(pwm_to_motorA));
+ pwm_motorB.write(abs(pwm_to_motorB));
}
}
-
void keep_in_range(float * in, float min, float max)
{
*in > min ? *in < max? : *in = max: *in = min;