Biorobotics 2019 groep 24
Calibratie motor, demo, (EMG calibratie en movement werkt niet)
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL Encoder FastPWM
Diff: main.cpp
- Revision:
- 6:91cdad4e00e8
- Parent:
- 5:810892d669f9
--- a/main.cpp Thu Oct 31 15:56:07 2019 +0000
+++ b/main.cpp Thu Oct 31 22:40:32 2019 +0000
@@ -3,8 +3,14 @@
#include "MODSERIAL.h"
#include "QEI.h"
#include "FastPWM.h"
+#include "HIDScope.h"
+#include <stdio.h> /* printf */
+#include <stdlib.h> /* abs */
+#include <complex>
+#include "BiQuad.h"
-const double PI = 3.1415926535897;
+
+const double PI = 3.1415926535897;
InterruptIn but1(D1);
InterruptIn but2(D0);
@@ -18,17 +24,17 @@
DigitalOut motor2_direction(D7); //richting van motor2
FastPWM motor2_pwm(D6); //Motor 2 PWM controle van de snelheid
-Ticker loop_ticker;
+Ticker loop_ticker;
// SERIAL COMMUNICATION WITH PC.................................................
Serial pc(USBTX, USBRX);
-enum States {s_idle, s_calibratie_encoder, s_demonstratie, s_EMGcalibratie};
+enum States {s_idle, s_calibratie_encoder, s_demonstratie, s_EMGcalibration, s_EMG};
States state;
// ENCODER .....................................................................
QEI enc1 (D13, D12, NC, 4200); //encoder 1 gebruiken
-QEI enc2 (D9, D8, NC, 4200); //encoder 2 gebruiken
-
+QEI enc2 (D9, D8, NC, 4200); //encoder 2 gebruiken
+
const float dt = 0.001;
double e_int = 0;
double e_int2 = 0;
@@ -49,7 +55,7 @@
volatile double error1;
volatile double error2;
-bool state_changed = false;
+bool state_changed = false;
volatile bool A=true;
volatile bool B=true;
volatile bool but1_pressed = false;
@@ -62,14 +68,14 @@
volatile bool direction1 = clockwise;
volatile bool direction2 = clockwise;
-// RKI VARIABELEN...............................................................
+// RKI VARIABELEN...............................................................
// Lengtes zijn in meters
// Vaste variabelen:
-const double L0 = 0.30; // lengte arm 1 --> moet nog goed worden opgemeten!
-const double L2 = 0.30; // Lengte arm 2 --> moet ook nog goed opgemeten worden!
-const double r_trans = 0.035; // gebruiken voor translation naar shaft rotation
+const double L0 = 0.25; // lengte arm 1 --> moet nog goed worden opgemeten!
+const double L2 = 0.375; // Lengte arm 2 --> moet ook nog goed opgemeten worden!
+const double r_trans = 0.035; // gebruiken voor translation naar shaft rotation
-// Variërende variabelen:
+// Variërende variabelen:
double q1 = 0; // Motorhoek van joint 1 op beginpositie
double q2 = PI/2; // Motorhoek van joint 2 op beginpositie
double v_x; // Snelheid van end effector in x richting --> Door EMG signals
@@ -78,11 +84,11 @@
double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
double Cq2; // Joint angle van het systeem (gecorrigeerd voor gear ratio 1:1.1)
-double q1_dot; // Benodigde hoeksnelheid van motor 1
-double q2_dot; // Benodigde hoeksnelheid van motor 2
+double q1_dot; // Benodigde hoeksnelheid van motor 1
+double q2_dot; // Benodigde hoeksnelheid van motor 2
-double q1_ii; // Referentie signaal voor motorhoek q1
-double q2_ii; // Referentie signaal voor motorhoek q2
+double q1_ii; // Referentie signaal voor motorhoek q1
+double q2_ii; // Referentie signaal voor motorhoek q2
//PI VARIABELEN................................................................
const double kp=0.002; // Soort van geschaald --> meer onderzoek nodig
@@ -90,9 +96,99 @@
const double kp2=0.002;
const double ki2=0.0001;
+//FILTERS EMG...................................................................
+
+//Filters Linker Biceps
+//In de volgorde High pass, notch, low pass
+BiQuad LBHP1(0.995566972017647, -1.991133944035294, 0.995566972017647, 1.000000000000000, -1.991114292201654, 0.991153595868935);
+BiQuad LBN1( 0.5, 0, 0.5, 0, 0);
+BiQuad LBLP1(0000.087655548754006, 0000.175311097508013, 0000.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
+BiQuadChain LeftBicepHP;
+BiQuadChain LeftBicepN;
+BiQuadChain LeftBicepLP;
+
+//Filters Rechter Biceps
+//In de volgorde High pass, notch, low pass
+BiQuad RBHP1(0.995566972017647, -1.991133944035294, 0.995566972017647, 1.000000000000000, -1.991114292201654, 0.991153595868935);
+BiQuad RBN1( 0.5, 0, 0.5, 0, 0);
+BiQuad RBLP1(0000.087655548754006, 0000.175311097508013, 0000.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
+BiQuadChain RightBicepHP;
+BiQuadChain RightBicepN;
+BiQuadChain RightBicepLP;
+
+
+//Filters Rechter Quadriceps
+//In de volgorde High pass, notch, low pass
+BiQuad RQHP1(0.995566972017647, -1.991133944035294, 0.995566972017647,1.000000000000000, -1.991114292201654, 0.991153595868935);
+BiQuad RQN1( 0.5, 0, 0.5, 0, 0);
+BiQuad RQLP1(0000.087655548754006, 0.175311097508013, 0.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
+BiQuadChain RightLegHP;
+BiQuadChain RightLegN;
+BiQuadChain RightLegLP;
+
+double emgLBHP;
+double emgLBN;
+double emgLBA;
+
+double emgRBHP;
+double emgRBN;
+double emgRBA;
+double emgRBLP;
+double emgAbsRBNotch;
+
+double emgRQN;
+double emgRQHP;
+double emgRQA;
+
+double emgLBfiltered;
+double emgRBfiltered;
+double emgRQfiltered;
+double emgLBraw;
+double emgRBraw;
+double emgRQraw;
+
+double threshold_emgLB;
+double threshold_emgRB;
+double threshold_emgRQ;
+double threshold = 0.15;
+double emgLB_max = 0.000;
+double emgRB_max = 0.000;
+double emgRQ_max = 0.000;
+double emgLB_maxrust = 0.000;
+double emgRB_maxrust = 0.000;
+double emgRQ_maxrust = 0.000;
+int tijd = 0;
+double timecalibration;
+
+double emgsumLB;
+double emgsumRB;
+double emgsumRQ;
+double restmeanLB = 0.000;
+double restmeanRB = 0.000;
+double restmeanRQ = 0.000;
+
+double LBvalue;
+double RBvalue;
+double RQvalue;
+double emgLBrust;
+double emgRBrust;
+double emgRQrust;
+double RestmeanLB;
+double RestmeanRB;
+double RestmeanRQ;
+
+DigitalOut led5(LED_RED);
+
+AnalogIn emgLB(A0); //read EMG left bicep
+AnalogIn emgRB(A1); //read EMG right bicep
+AnalogIn emgRQ(A2); //read EMG right quadriceps
+
+HIDScope scope(6); //aantal kanalen HIDScope (voor test 5, voor echt 6)
+
void rest()
- // Rust. Hier wordt niets uitgevoerd. Wanneer button1
-{ // wordt ingedrukt gaan we naar de volgende state waar
+// Rust. Hier wordt niets uitgevoerd. Wanneer button1
+{
+ // wordt ingedrukt gaan we naar de volgende state waar
if (but1_pressed) { // de encoders worden gekalibreerd.
state_changed = true;
state = s_calibratie_encoder;
@@ -100,15 +196,16 @@
}
}
-void calibratie_encoder() // calibratie encoder. Hier worden de encoders gekalibreerd en op
-{ // 0 gezet. Wanneer op button 1 wordt gedrukt gaan we naar de
- if (state_changed) { // demo modus, wanneer op button 2 wordt gedruk gaan we naar
+void calibratie_encoder() // calibratie encoder. Hier worden de encoders gekalibreerd en op
+{
+ // 0 gezet. Wanneer op button 1 wordt gedrukt gaan we naar de
+ if (state_changed) { // demo modus, wanneer op button 2 wordt gedruk gaan we naar
pc.printf("Started encoder calibration\r\n"); // de EMG calibratie
state_changed = false;
}
if (but1_pressed) {
pc.printf("Encoder has been calibrated \r\n");
- enc1_value = enc1.getPulses();
+ enc1_value = enc1.getPulses(); // wat geeft deze voor soort waarde?
enc2_value = enc2.getPulses();
//enc1_value -= enc1_zero;
//enc2_value -= enc2_zero;
@@ -125,8 +222,8 @@
pc.printf("enc2value: %i\r\n", enc2_value);
pc.printf("hoek 1: %f\r\n",theta1);
pc.printf("hoek 2: %f\r\n",theta2);
- }
-
+ }
+
if (but2_pressed) {
pc.printf("Encoder has been calibrated \r\n");
enc1_value = enc1.getPulses();
@@ -139,7 +236,7 @@
enc2_zero = enc2_value;
but2_pressed = false;
state_changed = true;
- state = s_EMGcalibratie;
+ state = s_EMGcalibration;
pc.printf("enc01: %i\r\n", enc1_zero);
pc.printf("enc1value: %i\r\n", enc1_value);
pc.printf("enc02: %i\r\n",enc2_zero);
@@ -149,129 +246,274 @@
}
}
-void demonstratie()
+void filteren()
{
- if (state_changed) { //
- pc.printf("Demonstratie gestart\r\n") ; //
- state_changed = false;
- }
- }
-void EMGcalibratie()
-{
+ //linkerbicep
+ emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
+ emgLBHP=LeftBicepHP.step(emgLBraw);
+ emgLBN=LeftBicepN.step(emgLBHP);
+ emgLBA= fabs(emgLBN);
+ emgLBfiltered=LeftBicepLP.step(emgLBA);
+
+ //to show if filter is working
+ scope.set(0, emgLBraw);
+ scope.set(1, emgLBfiltered);
+ scope.set(2, emgRBraw);
+ scope.set(3, emgRBfiltered);
+ scope.set(4, emgRQraw);
+ scope.set(5, emgRQfiltered);
+
+ //rechterbicep
+ emgRBraw= emgRB.read();
+ emgRBHP= RightBicepHP.step(emgRBraw);
+ emgRBN=RightBicepN.step(emgRBHP);
+ emgRBA= fabs(emgRBN);
+ emgRBfiltered=RightBicepLP.step(emgRBA);
+
+ //Right Quadriceps
+ emgRQraw= emgRQ.read();
+ emgRQHP= RightLegHP.step(emgRQraw);
+ emgRQN= RightLegN.step(emgRQHP);
+ emgRQA= fabs(emgRQN);
+ emgRQfiltered=RightLegLP.step(emgRQA);
+
+ scope.send();
}
-void RKI()
-{
- Lq1 = q1*r_trans;
- Cq2 = q2/1.1; //1.1 is gear ratio
-
- q1_dot = (v_x + (v_y*(/*L1+*/L2*sin(q2/1.1)))/(L0 + q1*r_trans + L2*cos(q2/1.1)))/r_trans;
- q2_dot = (1.1*v_y)/(L0 + q1*r_trans + L2*cos(q2/1.1));
-
- q1_ii = q1 + q1_dot*dt;
- q2_ii = q2 + q2_dot*dt;
-
- q1 = q1_ii;
- q2 = q2_ii;
-
-}
-
-double GetReferencePosition()
-{
- double Potmeterwaarde = potMeter2.read();
-
- double refP = Potmeterwaarde*maxwaarde;
- return refP;
-}
-
-double GetReferencePosition2()
+void EMGcalibration()
{
- double potmeterwaarde2 = potMeter1.read();
- double refP2 = potmeterwaarde2*maxwaarde;
- return refP2;
-}
-
-double FeedBackControl(double error, double &e_int)
-{
- double Proportional= kp*error1;
-
- e_int = e_int+dt*error1;
- double Integrator = ki*e_int;
-
- motorValue1 = Proportional + Integrator ;
- return motorValue1;
-}
-
-double FeedBackControl2(double error2, double &e_int2)
-{
- double Proportional2= kp2*error2;
-
- e_int2 = e_int2+dt*error2;
- double Integrator2 = ki2*e_int2;
-
- double motorValue2 = Proportional2 + Integrator2 ;
- return motorValue2;
+ if(state_changed) {
+ state_changed = false;
+ }
+ /*if (state_changed) { */
+ //pc.printf("Started EMG calibration\r\n");
+ tijd= tijd + 1; //idealiter op een andere plek haha
+ /*if (but1_pressed) {
+ pc.printf("EMG calibratie \r\n");
+ but1_pressed = false;*/
+ emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
+ emgLBHP=LeftBicepHP.step(emgLBraw);
+ emgLBN=LeftBicepN.step(emgLBHP);
+ emgLBA= fabs(emgLBN);
+ emgLBfiltered=LeftBicepLP.step(emgLBA);
+
+ emgRBraw= emgRB.read();
+ emgRBHP= RightBicepHP.step(emgRBraw);
+ emgRBN=RightBicepN.step(emgRBHP);
+ emgRBA= fabs(emgRBN);
+ emgRBfiltered=RightBicepLP.step(emgRBA);
+
+ emgRQraw= emgRQ.read();
+ emgRQHP= RightLegHP.step(emgRQraw);
+ emgRQN= RightLegN.step(emgRQHP);
+ emgRQA= fabs(emgRQN);
+ emgRQfiltered=RightLegLP.step(emgRQA);
+
+ if (tijd > 1000 && tijd < 6000) {
+ emgLBraw= emgLB.read();
+ emgLBHP=LeftBicepHP.step(emgLBraw);
+ emgLBN=LeftBicepN.step(emgLBHP);
+ emgLBA= fabs(emgLBN);
+ emgLBfiltered=LeftBicepLP.step(emgLBA);
+
+ if (emgLBfiltered > emgLB_max) {
+ emgLB_max = emgLBfiltered;
+ }
+ //pc.baud(115200);
+ //pc.printf("emgLB_max = %f \r\n", emgLB_max);// geen tekst printen in ticker want dat gaat mis xjes
+ led5=0; //led gaat aan zodra je linkerbicep moet aanspannen
+ }
+
+ else if (tijd > 7000 && tijd < 12000) {
+ emgRBraw= emgRB.read();
+ emgRBHP= RightBicepHP.step(emgRBraw);
+ emgRBN=RightBicepN.step(emgRBHP);
+ emgRBA= fabs(emgRBN);
+ emgRBfiltered=RightBicepLP.step(emgRBA);
+ if (emgRBfiltered > emgRB_max) {
+ emgRB_max = emgRBfiltered;
+ }
+ //pc.printf("emgRB_max = %f \r\n", emgRB_max);
+ led5=0; //led gaat uit zodra je rechterbicep moet aanspannen
+ }
+
+
+ else if (tijd > 13000 && tijd < 18000) {
+ emgRQraw= emgRQ.read();
+ emgRQHP= RightLegHP.step(emgRQraw);
+ emgRQN= RightLegN.step(emgRQHP);
+
+ emgRQA= fabs(emgRQN);
+ emgRQfiltered=RightLegLP.step(emgRQA);
+ if (emgRQfiltered > emgRQ_max) {
+ emgRQ_max = emgRQfiltered;
+ }
+ //pc.printf("emgRQ_max = %f \r\n", emgRQ_max);
+ led5=0; //led gaat aan zodra je rechterbeenspier moet aanspannen
+
+ } else {
+ led5=1; //led gaat uit zodra kalibratie voltooid
+ }
+
+
+ threshold_emgLB = threshold*emgLB_max; //bepaal threshold, nu op 0.15*maximale waarde.
+ threshold_emgRB = threshold*emgRB_max;
+ threshold_emgRQ = threshold*emgRQ_max;
+
+ state_changed = true;
+ state = s_EMG;
+
+// wanneer threshold_emgx is bereikt, wordt de brushingmode geactiveerd.
}
-void SetMotor1(float motorValue1) {
+
+
+void SetMotor1(float motorValue1)
+{
// gegeven motorValue1 <=1, schrijft snelheid naar pwm.
// MotorValues buiten range worden afgekapt dat ze binnen de range vallen.
motor1_pwm.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
}
-void SetMotor2(float motorValue2) {
+void SetMotor2(float motorValue2)
+{
motor2_pwm.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
-}
-
+}
+
+void brushingmode()
+{
+ if (state_changed) {
+ state_changed = false;
+ }
+
+
+ if (emgLBfiltered > threshold_emgLB) {
+//tandenborstel naar links (cw/ccw) //direction1/2 = cw/ccw
+ motor2_direction.write(direction1); //1/2 is welke motor
+//motor1_direction.write(direction1 = !direction1); //is counterclockwise
+ SetMotor2(motorValue2); //1/2 welke motor
+
+ } else if (emgRBfiltered > threshold_emgRB) {
+//tandenborstel naar rechts
+
+ } else if (emgRQfiltered > threshold_emgRQ) {
+//tandenborstel naar achter
+
+ } else if (emgRBfiltered > threshold_emgRB && emgLBfiltered > threshold_emgLB) {
+//tandenborstel naar voren
+
+ }
+}
+
+void demonstratie()
+{
+ if (state_changed) { //
+ pc.printf("Demonstratie gestart\r\n") ; //
+ state_changed = false;
+ }
+}
+
+
+void RKI()
+{
+ Lq1 = q1*r_trans;
+ Cq2 = q2/1.1; //1.1 is gear ratio
+
+ q1_dot = (v_x + (v_y*(L2*sin(q2/1.1)))/(L0 + q1*r_trans + L2*cos(q2/1.1)))/r_trans;
+ q2_dot = (1.1*v_y)/(L0 + q1*r_trans + L2*cos(q2/1.1));
+
+ q1_ii = q1 + q1_dot*dt;
+ q2_ii = q2 + q2_dot*dt;
+
+ q1 = q1_ii;
+ q2 = q2_ii;
+
+}
+
+double GetReferencePosition()
+{
+ double Potmeterwaarde = potMeter2.read();
+
+ double refP = Potmeterwaarde*maxwaarde;
+ return refP;
+}
+
+double GetReferencePosition2()
+{
+ double potmeterwaarde2 = potMeter1.read();
+ double refP2 = potmeterwaarde2*maxwaarde;
+ return refP2;
+}
+
+double FeedBackControl(double error, double &e_int)
+{
+ double Proportional= kp*error1;
+
+ e_int = e_int+dt*error1;
+ double Integrator = ki*e_int;
+
+ motorValue1 = Proportional + Integrator ;
+ return motorValue1;
+}
+
+double FeedBackControl2(double error2, double &e_int2)
+{
+ double Proportional2= kp2*error2;
+
+ e_int2 = e_int2+dt*error2;
+ double Integrator2 = ki2*e_int2;
+
+ double motorValue2 = Proportional2 + Integrator2 ;
+ return motorValue2;
+}
+
void MeasureAndControl()
{
// RKI aanroepen
//RKI();
// hier the control of the 1st control system
refP = GetReferencePosition(); //moet eigenlijk nog met RKI
- Huidigepositie1 = enc1.getPulses();
+ Huidigepositie1 = enc1.getPulses();
error1 = (refP - Huidigepositie1);// make an error
motorValue1 = FeedBackControl(error1, e_int);
SetMotor1(motorValue1);
// hier the control of the 2nd control system
- refP2 = GetReferencePosition2();
- Huidigepositie2 = enc2.getPulses();
+ refP2 = GetReferencePosition2();
+ Huidigepositie2 = enc2.getPulses();
error2 = (refP2 - Huidigepositie2);// make an error
motorValue2 = FeedBackControl2(error2, e_int2);
SetMotor2(motorValue2);
pc.printf("refP = %d, huidigepos = %d, motorvalue = %d, refP2 = %d, huidigepos2 = %d, motorvalue2 = %d \r\n", refP, Huidigepositie1, motorValue1, refP2, Huidigepositie2, Huidigepositie2);
-
+
}
void direction()
-{
+{
if (butsw2==0) {
- if (A==true){// zodat het knopje 1 x wordt afgelezen
- // richting veranderen
- motor1_direction.write(direction1 = !direction1);
- pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
- A=false;
+ if (A==true) { // zodat het knopje 1 x wordt afgelezen
+ // richting veranderen
+ motor1_direction.write(direction1 = !direction1);
+ pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
+ A=false;
}
- }
- else{
- A=true;
+ } else {
+ A=true;
}
-
- if (butsw3==0){
- if (B==true){
- motor2_direction.write(direction2 = !direction2);
- pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
- B=false;
+
+ if (butsw3==0) {
+ if (B==true) {
+ motor2_direction.write(direction2 = !direction2);
+ pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
+ B=false;
}
+ } else {
+ B=true;
}
- else{
- B=true;}
-
-
+
}
-
+
void state_machine()
{
//run current state
@@ -286,37 +528,41 @@
MeasureAndControl(); // en switch 2 en 3( pot1,sw2->motor1 / pot2,sw3->motor2
direction(); // als op knop 2 wordt gedrukt komen we in de s_idle state
if (but2_pressed) {
- pc.printf("fail. \r\n");
- but2_pressed = false;
- state_changed = true;
- state = s_idle;
+ pc.printf("fail. \r\n");
+ but2_pressed = false;
+ state_changed = true;
+ state = s_idle;
}
break;
- case s_EMGcalibratie:
- rest();
+ case s_EMGcalibration:
+ EMGcalibration();
break;
-
-}
+ case s_EMG:
+ filteren();
+ brushingmode();
+ break;
+
+ }
}
void but1_interrupt()
{
- if(but2.read()) {//both buttons are pressed
- failure_occurred = true;
- }
+ //if(but2.read()) {//both buttons are pressed
+ // failure_occurred = true;
+ //}
but1_pressed = true;
- pc.printf("Button 1 pressed \n\r");
+ pc.printf("Button 1 pressed \n\r");
}
-
+
void but2_interrupt()
{
- if(but1.read()) {//both buttons are pressed
- failure_occurred = true;
- }
+ //if(but1.read()) {//both buttons are pressed
+ // failure_occurred = true;
+ //}
but2_pressed = true;
pc.printf("Button 2 pressed \n\r");
}
-
+
void main_loop()
{
state_machine();
@@ -327,13 +573,13 @@
pc.baud(115200);
pc.printf("Executing main()... \r\n");
state = s_idle;
-
-
+
+
but1.fall(&but1_interrupt);
but2.fall(&but2_interrupt);
loop_ticker.attach(&main_loop, dt);
pc.printf("main_loop is running\n\r");
-
+
}
-
+