Calibratie motor +demo (alleen nog snelheid aan/uit) button direction moet nog
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL Encoder FastPWM
main.cpp
- Committer:
- S1933191
- Date:
- 2019-10-31
- Revision:
- 6:91cdad4e00e8
- Parent:
- 5:810892d669f9
File content as of revision 6:91cdad4e00e8:
#include <math.h> #include "mbed.h" #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; InterruptIn but1(D1); InterruptIn but2(D0); DigitalIn butsw2(SW3); DigitalIn butsw3(SW2); AnalogIn potMeter1(A5); AnalogIn potMeter2(A4); DigitalOut motor1_direction(D4); //richting van motor1 FastPWM motor1_pwm(D5); //Motor 1 PWM controle van de snelheid DigitalOut motor2_direction(D7); //richting van motor2 FastPWM motor2_pwm(D6); //Motor 2 PWM controle van de snelheid Ticker loop_ticker; // SERIAL COMMUNICATION WITH PC................................................. Serial pc(USBTX, USBRX); 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 const float dt = 0.001; double e_int = 0; double e_int2 = 0; double theta1; double theta2; int enc1_zero = 0; int enc2_zero = 0; int enc1_value; int enc2_value; const int maxwaarde = 400; volatile double Huidigepositie1=0; volatile double Huidigepositie2=0; volatile double motorValue1; volatile double motorValue2; volatile double refP; volatile double refP2; volatile double error1; volatile double error2; bool state_changed = false; volatile bool A=true; volatile bool B=true; volatile bool but1_pressed = false; volatile bool but2_pressed = false; volatile bool butsw2_pressed = false; volatile bool butsw3_pressed = false; volatile bool failure_occurred = false; const bool clockwise = true; volatile bool direction1 = clockwise; volatile bool direction2 = clockwise; // RKI VARIABELEN............................................................... // Lengtes zijn in meters // Vaste variabelen: 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: 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 double v_y; // Snelheid van end effector in y richting --> Door EMG signals 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_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 const double ki=0.0001; 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 if (but1_pressed) { // de encoders worden gekalibreerd. state_changed = true; state = s_calibratie_encoder; but1_pressed = false; } } 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(); // wat geeft deze voor soort waarde? enc2_value = enc2.getPulses(); //enc1_value -= enc1_zero; //enc2_value -= enc2_zero; theta1 = (float)(enc1_value)/(float)(4200)*2*PI; theta2 = (float)(enc2_value)/(float)(4200)*2*PI; enc1_zero = enc1_value; enc2_zero = enc2_value; but1_pressed = false; state_changed = true; state = s_demonstratie; pc.printf("enc01: %i\r\n", enc1_zero); pc.printf("enc1value: %i\r\n", enc1_value); pc.printf("enc02: %i\r\n",enc2_zero); 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(); enc2_value = enc2.getPulses(); //enc1_value -= enc1_zero; //enc2_value -= enc2_zero; theta1 = (float)(enc1_value)/(float)(4200)*2*PI; theta2 = (float)(enc2_value)/(float)(4200)*2*PI; enc1_zero = enc1_value; enc2_zero = enc2_value; but2_pressed = false; state_changed = true; 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); pc.printf("enc2value: %i\r\n", enc2_value); pc.printf("hoek 1: %f\r\n",theta1); pc.printf("hoek 2: %f\r\n",theta2); } } void filteren() { //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 EMGcalibration() { 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) { // 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) { 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(); 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(); 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; } } 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; } } else { B=true; } } void state_machine() { //run current state switch (state) { case s_idle: // in deze state gebeurd niets. Als op knop 1 wordt gedrukt rest(); // gaan we over naar s_calibratie_encoder break; case s_calibratie_encoder: // in deze state worden de encoders gekalibreerd. knop 1 -> s_demonstratie calibratie_encoder(); // knop 2 -> s_EMGcalibratie break; case s_demonstratie: // in deze state kunnen de motors worden bestuurd met de potmeters 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; } break; case s_EMGcalibration: EMGcalibration(); break; case s_EMG: filteren(); brushingmode(); break; } } void but1_interrupt() { //if(but2.read()) {//both buttons are pressed // failure_occurred = true; //} but1_pressed = true; pc.printf("Button 1 pressed \n\r"); } void but2_interrupt() { //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(); } int main() { 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"); }