BMT K9 groep 9 2015-2016
Goed werkende PID, vanaf nu dit script gebruiken.
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 1:d9cfdc904b10
- Parent:
- 0:50d492ea0fd0
- Child:
- 2:d04df4be6cf7
--- a/main.cpp Thu Oct 15 07:57:23 2015 +0000
+++ b/main.cpp Fri Oct 16 13:41:47 2015 +0000
@@ -3,6 +3,8 @@
#include "MODSERIAL.h"
#include "math.h"
#include "HIDScope.h"
+#include "biquadFilter.h"
+#include "iostream"
#define M_PI 3.14159265358979323846
DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
@@ -21,21 +23,18 @@
bool countStepY_go = false;
bool checkButtonStateX_go = false;
bool checkButtonStateY_go = false;
+bool Filter_go = false;
DigitalIn telknopX(SW2);
DigitalIn telknopY(SW3);
DigitalOut ledX(D2);
DigitalOut ledY(D3);
-HIDScope scope(2);
-const double motor1_Kp = 1; //4200=1 rondje
-double m1_Kp = 0.0005, m1_Ki = 0.0, m1_Kd = 0.0;
+double m1_Kp = 0.0003, m1_Ki = 0.0, m1_Kd = 0.0;
const double m1_Ts = 0.01;
double m1_err_int = 0, m1_prev_err = 0;
double m1_u_prev = 0;
-const int grens = 50;
-
/*
Gain = 0.002356
B = [ 1.00000000000, 2.00000000000, 1.00000000000]
@@ -68,6 +67,100 @@
double constante2;
double constante3;
+// --- EMG declaraties --- //
+AnalogIn emg1(A0);
+AnalogIn emg2(A1);
+DigitalIn button(SW2);
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_red(LED_RED);
+DigitalOut led_blue(LED_BLUE);
+
+double filtered_notch1;
+double filtered_notch2;
+double signal1;
+double signal2;
+bool start = false;
+
+HIDScope scope(2); //4 channels
+Ticker scopeTimer;
+Ticker Amplitude_height;
+int counter = 0;
+using namespace std;
+
+double high[2] = {};
+double low[2] = {};
+double notch[2] = {};
+double max1 =0.01;
+double max2=0.01;
+
+// ----- Filters ----- //
+
+//tweede highpass filter, poging met latere cutoff frequency: werkt niet. Nu alleen nog bewegingsartefacten te zien, geen kracht!
+//9 oktober: nu werkt het wel. Een beetje beweging te zien, maar aanspannen geeft hogere amplitude
+biquadFilter High_1_Filter1( -0.69832496500, 0.00000000000, 0.849163, -0.849163, 0.00000000000);
+biquadFilter High_2_Filter1( -1.87472749640, 0.90756583051, 0.945573, 1.891146, 0.945573);
+biquadFilter High_1_Filter2( -0.69832496500, 0.00000000000, 0.849163, -0.849163, 0.00000000000);
+biquadFilter High_2_Filter2( -1.87472749640, 0.90756583051, 0.945573, 1.891146, 0.945573);
+//Notch filter voor 50 Hz. Butter
+biquadFilter Notch_Filter1(-1.65238019196, 0.73741535193, 0.86870768, -1.652380199639, 0.86870768);
+biquadFilter Notch_Filter2(-1.65238019196, 0.73741535193, 0.86870768, -1.652380199639, 0.86870768);
+//Lowpass cheby II. bestaat uit twee biquads. De B coefficienten moeten maal de gain gedaan worden.
+biquadFilter Low_1_Filter1( -0.96240334975 , 0.00000000000 , 0.018798 , 0.018798 , 0.000000 ); // a1, a2, b0, b1, b2.
+biquadFilter Low_2_Filter1( -1.96214371581 , 0.96354072388 , 0.030252 , -0.0591069922, 0.030252 );
+biquadFilter Low_1_Filter2( -0.96240334975 , 0.00000000000 , 0.018798 , 0.018798 , 0.000000 ); // a1, a2, b0, b1, b2.
+biquadFilter Low_2_Filter2( -1.96214371581 , 0.96354072388 , 0.030252 , -0.0591069922, 0.030252 );
+
+double* LowFilter(double n3_emg1, double n3_emg2)
+{
+ double l1_emg1 = Low_1_Filter1.step(n3_emg1);
+ double l1_emg2 = Low_1_Filter2.step(n3_emg2);
+ low[0] = Low_2_Filter1.step(l1_emg1);
+ low[1] = Low_2_Filter2.step(l1_emg2);
+ return low;
+}
+
+double* HighFilter(void)
+{
+ double reademg1 = emg1.read();
+ double reademg2 = emg2.read();
+ double h1_emg1 = High_1_Filter1.step(reademg1);
+ high[0] = High_2_Filter1.step(h1_emg1);
+ double h1_emg2 = High_1_Filter2.step(reademg2);
+ high[1] = High_2_Filter2.step(h1_emg2);
+ return high;
+}
+
+double* NotchFilter(double n_emg1, double n_emg2 )
+{
+ notch[0] = Notch_Filter1.step(n_emg1);
+ notch[1] = Notch_Filter2.step(n_emg2);
+ return notch;
+}
+
+void Filter(void)
+{
+ HighFilter();
+ double h2_emg1_abs = fabs(high[0]);
+ double h2_emg2_abs = fabs(high[1]);
+ LowFilter(h2_emg1_abs, h2_emg2_abs);
+ double filtered_emg1 = low[0];
+ double filtered_emg2 = low[1];
+ NotchFilter(filtered_emg1, filtered_emg2);
+ filtered_notch1 = notch[0];
+ filtered_notch2 = notch[1];
+ signal1 = filtered_notch1/max1;
+ signal2 = filtered_notch2/max2;
+ //scope.set(2, filtered_notch1);
+ //scope.set(3, filtered_notch2);
+ scope.set(0, signal1);
+ scope.set(1, signal2);
+ scope.send();
+}
+
+
+
+// ------ OVERIGE FUNCTIES ------ //
+
bool buttonReleasedX(DigitalIn& button)
{
if (button.read() == 1 && buttonStateX == 0) {
@@ -96,18 +189,114 @@
float bereken_hoek1(float x, float y)
{
float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
- float alpha = acos((langste_zijde/2)/30);
+ float alpha = acos((langste_zijde/2)/35); //4200=1 rondje
return (atan((4*y)/(4*x)) + alpha)*4200/(2*M_PI); // omrekenen van radialen naar counts = *4200/(2*M_PI), dit moet nog omgerekend worden met de gear ratio van de extra tandwielen.
}
float bereken_hoek2(float x, float y)
{
float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
- float alpha = acos((langste_zijde/2)/30);
+ float alpha = acos((langste_zijde/2)/35);
float beta = 0.5*M_PI - alpha;
return (2*beta)*4200/(2*M_PI); // x wordt hoek voor eerste arm t.o.v. x-as (CW), y hoek voor tweede arm t.o.v. eerste arm (CW)
}
+void Amplitude() //true is aangespanen, false is niet aangespannen. met 2 thresholds. Dit moet nog in procent worden omgezet!
+{
+ if(start == false && filtered_notch1< 0.6) { //moeten waarden 70 en 20 worden als drempelwaarden
+ start = true;
+ //printf("start 0 \n\r");
+ }
+ if (start == true && filtered_notch1<0.6) {
+ start = true;
+ //printf("start 1\n\r");
+ }
+ if (start == true && filtered_notch1< 0.2) {
+ start = false;
+ //printf("start 1\n\r");
+ }
+ if (start == false && filtered_notch1 <0.2) {
+ start = false;
+ //printf("start 0\n\r");
+ }
+ if (start == true && filtered_notch1 <0.6 && filtered_notch1>0.2) {
+ start = true;
+ //printf("start 1\n\r");
+ }
+ if (start == false && filtered_notch1 <0.6 && filtered_notch1>0.2) {
+ start = false;
+ //printf("start 0\n\r");
+ }
+}
+
+// ----- KALIBRATIE ------ //
+
+void MaxValue()
+{
+ int length = 500;
+ double meetwaarden1 [500] = {}; //lege array maken voor emg1
+ double meetwaarden2 [500] = {}; //lege array maken voor emg2
+ for(int i = 0; i<500; i++) { //array vullen met meetwaarden van emg1
+ meetwaarden1[i] = filtered_notch1;
+ wait(0.005); // zonder wait zit er maar een dezelfde waarde in de array! eventueel een ticker er van maken
+ }
+ for(int i = 0; i<500; i++) { //array vullen met meetwaarden van emg2
+ meetwaarden2[i] = filtered_notch2;
+ wait(0.005);
+ }
+ //printarray(meetwaarden, 500); //was als controle
+ for(int i = 0; i<length; i++) { //maximale waarde van de array berekenen
+ if(meetwaarden1[i] > max1) {
+ max1= meetwaarden1[i];
+ }
+ }
+ for(int i = 0; i<length; i++) { //maximale waarde van de array berekenen
+ if(meetwaarden2[i] > max2) {
+ max2 = meetwaarden2[i];
+ }
+ }
+ printf("max value of emg1 is %f, and of emg 2 is %f \n\r", max1, max2);
+}
+
+void Calibration()
+{
+ switch(button.read()) {
+ case 0: //knop wel indrukken
+ switch(counter) {
+ case 0: //eerste keer knop indrukken
+ led_red.write(0); //rode led aan
+ MaxValue();
+ led_green.write(0); //groene led aan + rode = geel
+ wait(1); //even wachten, zodat er tijd is om de knop weer los te laten
+ counter++;
+ break;
+ case 1: //tweede keer knop indrukken
+ led_red.write(1); //uit
+ led_green.write(1); //uit
+ led_blue.write(0); //blauw ledje aan
+
+ // move arm to 0 position: motoren aansturen.
+ //emg_signal1 = filtered_notch/max*100 //schalen
+ wait(1);
+ counter++;
+ break;
+ case 2:
+ //encoder....setPosition(0);
+ led_blue.write(1); //uit
+ led_green.write(0); // ledje groen aan
+ wait(1);
+ counter++;
+ break;
+ }
+ break;
+ }
+}
+
+//voor de motoren: als counter =0: motorspeed =0
+//als counter = 1: niet in vakjes rekenen
+// als counter >1: wel in vakjes rekenen
+
+
//-------------------------- MOTORCONTROLLERS --------------------------//
void constanteBepalen()
@@ -118,17 +307,6 @@
m1_Ki = constante1*0.001;
}
-double P(double error, const double Kp) //P regelaar
-{
- return Kp*error; //ingangssignaal voor de motor\plant) = dus voltage
-}
-
-double PI(double e, double Kp, const double Ki, double Ts, double &e_int)
-{
- e_int = e_int + Ts * e; // e_int is changed globally because it’s ’by reference’ (&)
- return Kp * e + Ki * e_int;
-}
-
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; //formule uit slides van biquad filter
@@ -151,89 +329,7 @@
return Kp*e+Ki*e_int+Kd*e_der2; //formule uit slides
}
-// P controllers
-void motor2_P_Controller()
-{
- reference2 = potmeter2.read()*4200; //draaiknop uitlezen, tussen 0 en 1
- //reference2 = y;
- double position =(encoder2.getPosition()); //waarde tussen 0 en 4200
- double u = P(reference2 - position, motor1_Kp); //P controller aanroepen
-
- if (u > 0) { //directie bepalen
- motor2direction=1;
- } else if (u <= 0) {
- motor2direction =0;
- }
- if (u <grens && u>-grens) { //bepalen of de motor aan of uit moet
- motor2speed = 0.0f;
- } else {
- motor2speed = 0.1f;
- }
-}
-
-void motor1_P_Controller()
-{
- reference1 = potmeter1.read()*4200; //draaiknop uitlezen, tussen 0 en 1
- //reference1=x;
- double position =(encoder1.getPosition()); // waarde tussen 0 en 4200
- difference1 = reference1-position;
- double u = P(reference1 - position, motor1_Kp); //P controller aanroepen
- if (u > 0) { //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
- motor1direction=0;
- } else if (u <= 0) {
- motor1direction =1;
- }
- if (u <grens && u >-grens) { //bepalen of de motor aan of uit moet
- motor1speed = 0.0f;
- } else {
- motor1speed = 0.1f;
- }
-}
-
-//PI controllers
-void motor1_PI_Controller()
-{
- //reference1 = potmeter1.read()*4200; //draaiknop uitlezen, tussen 0 en 1
- float hoek1 = bereken_hoek1(countsX,countsY);
- reference1=hoek1;
- double position =(encoder1.getPosition()); // waarde tussen 0 en 4200
-
- difference1 = reference1 - position;
- constanteBepalen();
- double u = PI(reference1 - position, m1_Kp, m1_Ki, m1_Ts, m1_err_int);
- if (u > 0) { //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
- motor1direction=0;
- } else if (u <= 0) {
- motor1direction =1;
- }
- if (u <grens && u >-grens) { //bepalen of de motor aan of uit moet
- motor1speed = 0.0f;
- } else {
- motor1speed = 0.1f;
- }
-}
-
-void motor2_PI_Controller()
-{
- //reference2 = potmeter2.read()*4200; //draaiknop uitlezen, tussen 0 en 1
- float hoek2 = bereken_hoek2(countsX,countsY);
- reference2 = hoek2;
- double position =(encoder2.getPosition()); //waarde tussen 0 en 4200
-
- difference2 = reference2 - position;
- constanteBepalen();
- double u = PI(reference2 - position, m1_Kp, m1_Ki, m1_Ts, m1_err_int);
- if (u > 0) { //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
- motor2direction=1;
- } else if (u <= 0) {
- motor2direction =0;
- }
- if (u <grens && u >-grens) { //bepalen of de motor aan of uit moet
- motor2speed = 0.0f;
- } else {
- motor2speed = 0.1f;
- }
-}
+// ---- PID Controller ----- //
void motor1_PID_Controller()
{
@@ -254,7 +350,9 @@
} else if (u>=0) {
motor1direction = 0;
}
- motor1speed = fabs(u);
+ if(counter != 0) {
+ motor1speed = fabs(u);
+ }
pc.printf("u = %.2f \r\n", u);
}
@@ -277,7 +375,7 @@
void countStepX()
{
- if (buttonHold(telknopX)) {
+ if (start == true) {
countsX+= countStateX; //hier moet de richting dus veranderen
if(countsX >= 10) {
countsX = 10;
@@ -289,7 +387,7 @@
void countStepY()
{
- if (buttonHold(telknopY)) { //X en Y kunnen niet tegelijk optellen, alleen als de ander niet ingedrukt is
+ if (start == true) { //X en Y kunnen niet tegelijk optellen, alleen als de ander niet ingedrukt is // start moet nog start1 en start2 worden
countsY+= countStateY;
if(countsY >= 10) {
countsY = 10;
@@ -299,6 +397,7 @@
}
}
+
void motor1_PID_Controller_activate(){
motor1_PID_Controller_go = true;
}
@@ -319,6 +418,10 @@
checkButtonStateY_go = true;
}
+void Filter_activate(){
+ Filter_go = true;
+}
+
int main()
{
pc.baud(9600);
@@ -338,11 +441,22 @@
buttonStateCheckY.attach( &checkButtonStateY_activate, 0.001f); // Function@ hz
+//EMG-gedeelte
+ led_green.write(1);
+ led_red.write(1);
+ led_blue.write(1);
+ scopeTimer.attach(Filter_activate, 0.001);
+ //scopeTimer.attach(Filter, 0.001);
+ //Amplitude_height.attach(Amplitude, 0.1); //elke 0.05 seconde start is 0 of 1 printen
+
+
while (true) {
- pc.printf(" 1: pos: %d, ref: %d, dif: %d, Ki: %.5f, Kd: %.5f, countsX: %d, countsY: %d \r\n", encoder1.getPosition(), reference1,difference1,m1_Kd,m1_Ki,countsX,countsY);
+ pc.printf(" 1: pos: %d, ref: %d, dif: %d, Ki: %.5f, Kd: %.5f, countsX: %d, countsY: %d \r\n", encoder1.getPosition(), reference1,difference1,m1_Ki,m1_Kd,countsX,countsY);
//pc.printf(" 2: pos: %d, ref: %d, dif: %d \r\n", encoder2.getPosition(), reference2,difference2);
+ Calibration();
+
if(motor1_PID_Controller_go) {
motor1_PID_Controller_go = false;
motor1_PID_Controller();
@@ -363,8 +477,9 @@
checkButtonStateY_go = false;
checkButtonStateY();
}
-
-
-
+ if(Filter_go) {
+ Filter_go = false;
+ Filter();
+ }
}
}
\ No newline at end of file