Den iz
Final version used at presentation. Working parts: idle, enccali, emgcali, brushingmode, demomode(potmeter).
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL Encoder FastPWM
Diff: main.cpp
- Revision:
- 3:8eb1b9e0d676
- Parent:
- 2:6ca30ccec353
--- a/main.cpp Mon Oct 28 16:31:37 2019 +0000
+++ b/main.cpp Mon Nov 04 10:32:10 2019 +0000
@@ -1,163 +1,662 @@
+#include <math.h>
#include "mbed.h"
-#include "math.h"
-#include "encoder.h"
+#include "MODSERIAL.h"
#include "QEI.h"
-//#include "HIDScope.h"
-// ---- Alles met een 1(of geen getal) gaat over motor 1----
-// ---- Alles met een 2 gaat over motor 2----
-// ------ Hardware Interfaces -------
+#include "FastPWM.h"
+#include "HIDScope.h"
+#include <stdio.h> /* printf */
+#include <stdlib.h> /* abs */
+#include <complex>
+#include "BiQuad.h"
-const PinName motor1dir = D7; //direction1 (cw,ccw)
-const PinName motor1PWM = D6; //speed1
-const PinName motor2PWM = D5; //speed2
-const PinName motor2dir = D4; //direction2 (cw,ccw)
+
+const double PI = 3.1415926535897;
-DigitalOut motor1direction(motor1dir);
-PwmOut motor1control(motor1PWM);
-PwmOut motor2control(motor2PWM);
-DigitalOut motor2direction(motor2dir);
+InterruptIn but1(D1);
+InterruptIn but2(D0);
+DigitalIn butsw2(SW3);
+DigitalIn butsw3(SW2);
+AnalogIn potMeter1(A5);
+AnalogIn potMeter2(A4);
-const PinName button1name = D10;
-const PinName pot1name = A0;
-const PinName button2name = D11;
-const PinName pot2name = A1;
-InterruptIn button1(button1name); //had ook direct gekund maar he
-AnalogIn potMeterIn1(pot1name);
-InterruptIn button2(button2name);
-AnalogIn potMeterIn2(pot2name);
-
-QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //64 = ..., x4 encoding because ...
-QEI Encoder2(D9,D8,NC,64,QEI::X4_ENCODING);
-// ------- Objects used -------
+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 which controls the mother every 1/100 of a second.
-Ticker controlTicker;
-
-Ticker debugTicker;
+Ticker loop_ticker;
+// SERIAL COMMUNICATION WITH PC.................................................
Serial pc(USBTX, USBRX);
-//HIDScope scope(2); //fuck you hidscope we're not using you!
+enum States {s_idle, s_calibratie_encoder, s_demonstratie, s_EMGcalibratie, 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;
-// ------- Constants
-const float motorGain = 8f; //just to be sure, niet te veel input geven tegen windup (old motor, suboptimal perfomance)
-const float maxVelocity = 8f; //radians per second
-const bool clockwise = true;
-const float rad_count = 0.0007479; // 2pi/8400;
-const float pi = 3.1415926;
+const bool clockwise1 = true;
+const bool clockwise2 = true;
+volatile bool direction1 = clockwise1;
+volatile bool direction2 = clockwise2;
+
+// 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
+//const double gear1=1.0;
+const double gear2=1.25;
+
+
+// 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
-// ------ variables
+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.01; // Soort van geschaald --> meer onderzoek nodig
+const double ki=0.0001;
+const double kp2=0.01;
+const double ki2=0.0001;
+
+
+double maxPWM1 = 0.2;
+double maxPWM2 = 0.2;
+
+//FILTERS EMG...................................................................
-volatile bool direction1 = clockwise;
-volatile bool direction2 = clockwise;
-
-// ------ functions
+//Filters EMG
+//Filters Linker Biceps
+//In de volgorde High pass, notch, low pass
+//BiQuad LBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
+//BiQuad LBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
+BiQuad LBHP1(0.995566972017647, -1.991133944035294, 0.995566972017647, 1.000000000000000, -1.991114292201654, 0.991153595868935);
+//BiQuad LBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
+BiQuad LBN1( 0.5, 0, 0.5, 0, 0);
+BiQuad LBLP1(0000.087655548754006, 0000.175311097508013, 0000.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
+//BiQuad LBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuadChain LeftBicepHP;
+BiQuadChain LeftBicepN;
+BiQuadChain LeftBicepLP;
+
+//Filters Rechter Biceps
+//In de volgorde High pass, notch, low pass
+//BiQuad RBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
+//BiQuad RBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
+BiQuad RBHP1(0.995566972017647, -1.991133944035294, 0.995566972017647, 1.000000000000000, -1.991114292201654, 0.991153595868935);
+//BiQuad RBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
+BiQuad RBN1( 0.5, 0, 0.5, 0, 0);
+BiQuad RBLP1(0000.087655548754006, 0000.175311097508013, 0000.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
+//BiQuad RBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuadChain RightBicepHP;
+BiQuadChain RightBicepN;
+BiQuadChain RightBicepLP;
+
+
+//Filters Rechter Quadriceps
+//In de volgorde High pass, notch, low pass
+//BiQuad RTHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
+//BiQuad RTHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
+BiQuad RTHP1(0.995566972017647, -1.991133944035294 , 0.995566972017647 ,1.000000000000000, -1.991114292201654 , 0.991153595868935);
+//BiQuad RTHP2(1, 1.999999966458334, 0.999999966458334, -1.988418014198592, 0.988451549797368);
+BiQuad RTN1( 0.5, 0, 0.5, 0, 0);
+BiQuad RTLP1(0000.087655548754006 , 0.175311097508013 , 0.087655548754006, 1.000000000000000 , -1.973344249781299 , 0.973694871976315);
+//BiQuad RTLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuadChain RightLegHP;
+BiQuadChain RightLegN;
+BiQuadChain RightLegLP;
-float getReferenceVelocity() {
- // Returns reference velocity in rad/s.
- return maxVelocity * potMeterIn1 ;
+double emgLBHP;
+double emgLBN;
+double emgLBA;
+//double emgLBLP;
+//double emgAbsLBNotch;
+
+double emgRBHP;
+double emgRBN;
+double emgRBA;
+double emgRBLP;
+double emgAbsRBNotch;
+
+double emgRTN;
+double emgRTHP;
+double emgRTA;
+//double emgRTLP;
+//double emgAbsRTNotch;
+
+double emgLBfiltered;
+double emgRBfiltered;
+double emgRTfiltered;
+double emgLBraw;
+double emgRBraw;
+double emgRTraw;
+
+
+double threshold_emgLB;
+double threshold_emgRB;
+double threshold_emgRT;
+double threshold = 0.5;
+double emgLB_max = 0.000;
+double emgRB_max = 0.000;
+double emgRT_max = 0.000;
+double emgLB_maxrust = 0.000;
+double emgRB_maxrust = 0.000;
+double emgRT_maxrust = 0.000;
+volatile int tijd = 0;
+double timecalibration;
+
+double emgsumLB;
+double emgsumRB;
+double emgsumRT;
+double restmeanLB = 0.000;
+double restmeanRB = 0.000;
+double restmeanRT = 0.000;
+//double emgmeansubLB;
+//double emgmeansubRB;
+//double emgmeansubRT;
+double LBvalue;
+double RBvalue;
+double RTvalue;
+double emgLBrust;
+double emgRBrust;
+double emgRTrust;
+double RestmeanLB;
+double RestmeanRB;
+double RestmeanRT;
+
+
+DigitalOut ledr(LED_RED);
+DigitalOut ledg(LED_GREEN);
+DigitalOut ledb(LED_BLUE);
+
+
+AnalogIn emgLB(A0); //read EMG left bicep
+AnalogIn emgRB(A1); //read EMG right bicep
+AnalogIn emgRT(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;
}
-float getReferenceVelocity2() {
- // Returns reference velocity in rad/s.
- return maxVelocity * potMeterIn2;
}
-void setMotor1(float motorValue1) {
- // Given motorValue1<=1, writes the velocity to the pwm control.
- // MotorValues outside range are truncated to within range.
- motor1control.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
-}
+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;
+ }
+ else if (but1_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; // First arm rotated fully ccw
+ theta2 = (float)(enc2_value)/(float)(4200)*2*PI; // second arm fully clockwise --> looks like a Z
+ enc1_zero = enc1_value;
+ enc2_zero = enc2_value;
+
+
+ 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);
+ but1_pressed = false;
+ state_changed = true;
+ state = s_demonstratie;
+ }
-void setMotor2(float motorValue2) {
- // Given motorValue2<=1, writes the velocity to the pwm control.
- // MotorValues outside range are truncated to within range.
- motor2control.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
+ else 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;
+
+
+ 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);
+
+ but2_pressed = false;
+ state_changed = true;
+ state = s_EMGcalibratie;
+
+ }
}
-float feedForwardControl(float referenceVelocity) {
- // very simple linear feed-forward control
- // returns motorValue
- return referenceVelocity / motorGain;
-}
+void filteren()
+{
+ //linkerbicep
+ emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
+ emgLBHP=LeftBicepHP.step(emgLBraw);
+ emgLBN=LeftBicepN.step(emgLBHP);
+ //emgmeansubLB = emgLBN - RestmeanLB;
+ emgLBA= fabs(emgLBN);
+ emgLBfiltered=LeftBicepLP.step(emgLBA);
+ //LBvalue = emgLBfiltered/emgLB_max;
+
+ /* //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, emgRTraw);
+ scope.set(5, emgRTfiltered);
+ scope.send();
+*/
+ //rechterbicep
+ emgRBraw= emgRB.read();
+ emgRBHP= RightBicepHP.step(emgRBraw);
+ emgRBN=RightBicepN.step(emgRBHP);
+ //emgmeansubRB = emgRBN - RestmeanRB;
+ emgRBA= fabs(emgRBN);
+ emgRBfiltered=RightBicepLP.step(emgRBA);
+ //RBvalue = emgRBfiltered/emgRB_max;
+
+ //Right Quadriceps
+ emgRTraw= emgRT.read();
+ emgRTHP= RightLegHP.step(emgRTraw);
+ emgRTN= RightLegN.step(emgRTHP);
+ // emgmeansubRT = emgRTHP - RestmeanRT;
+ emgRTA= fabs(emgRTN);
+ emgRTfiltered=RightLegLP.step(emgRTA);
+ //RTvalue = emgRTfiltered/emgRT_max;
+
-float feedForwardControl2(float referenceVelocity2) {
- // very simple linear feed-forward control
- // returns motorValue
- return referenceVelocity2 / motorGain;
}
-void measureAndControl(void) {
- // This function measures the potmeter position, extracts a
- // reference velocity from it, and controls the motor with
- // a simple FeedForward controller. Call this from a Ticker.
- float referenceVelocity = getReferenceVelocity();
- float motorValue1 = feedForwardControl(referenceVelocity);
- setMotor1(motorValue1);
+void EMGcalibration()
+ {
+
+ 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);
+
+
+ emgRTraw= emgRT.read();
+ emgRTHP= RightLegHP.step(emgRTraw);
+ emgRTN= RightLegN.step(emgRTHP);
+ emgRTA= fabs(emgRTN);
+ emgRTfiltered=RightLegLP.step(emgRTA);
+
+ if (tijd > 1000 && tijd < 6000) {
+ emgLBraw= emgLB.read();
+ emgLBHP=LeftBicepHP.step(emgLBraw);
+ emgLBN=LeftBicepN.step(emgLBHP);
+ //emgmeansubLB = emgLBN - restmeanLB;
+ 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
+ ledr=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);
+ //emgmeansubRB = emgRBN - RestmeanRB;
+ emgRBA= fabs(emgRBN);
+ emgRBfiltered=RightBicepLP.step(emgRBA);
+ if (emgRBfiltered > emgRB_max) {
+ emgRB_max = emgRBfiltered;
+ }
+ //pc.printf("emgRB_max = %f \r\n", emgRB_max);
+ ledr=0; //led gaat uit zodra je rechterbicep moet aanspannen
+ }
+
+
+ else if (tijd > 13000 && tijd < 18000) {
+ emgRTraw= emgRT.read();
+ emgRTHP= RightLegHP.step(emgRTraw);
+ emgRTN= RightLegN.step(emgRTHP);
+ //emgmeansubRT = emgRTHP - RestmeanRT;
+ emgRTA= fabs(emgRTN);
+ emgRTfiltered=RightLegLP.step(emgRTA);
+ if (emgRTfiltered > emgRT_max) {
+ emgRT_max = emgRTfiltered;
+ }
+ //pc.printf("emgRT_max = %f \r\n", emgRT_max);
+ ledr=0; //led gaat aan zodra je rechterbeenspier moet aanspannen
+ } else if (tijd > 18000) {
+ ledr=1; //led gaat uit zodra kalibratie voltooid
+ state=s_EMG;
+ state_changed=true;
+
+ //pc.printf("Calibration finished!");
+ } else {
+ ledr=1;
+ }
+
+ pc.printf("%i", tijd);
+ tijd++;
+ threshold_emgLB = threshold*emgLB_max; //bepaal threshold, nu op 0.15*maximale waarde.
+ threshold_emgRB = threshold*emgRB_max;
+ threshold_emgRT = threshold*emgRT_max;
+
+
+ // if threshold_emgx is reached, brushingmodes activated
+}
- float referenceVelocity2 = getReferenceVelocity2();
- float motorValue2 = feedForwardControl2(referenceVelocity2);
- setMotor2(motorValue2);
-}
+
-void onButtonPress() {
- // reverses the direction
- motor1direction.write(direction1 = !direction1);
- pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
+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 onButtonPress2() {
- // reverses the direction
- motor2direction.write(direction2 = !direction2);
- pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
-}
-
-float Encoding()
+void brushingmode()
+{
+
+ //boven draait arm 1 aan, dus motor 1
+ //onder draait arm 2 aan, dus motor 2
+ if (emgLBfiltered > threshold_emgLB){ //tandenborstel naar links (cw/ccw) //direction1/2 = cw/ccw
+
+ motor2_direction.write(direction1); //motor 2 gaat cw
+ //motor2_direction.write(direction1 = !direction1); //is counterclockwise
+ motor2_pwm.write(maxPWM2);
+ ledr=0;
+
+
+ }else if (emgRBfiltered > threshold_emgRB){
+ motor2_pwm.write(maxPWM2);
+ motor2_direction.write(direction1 = !direction1); //is counterclockwise
+ //tandenborstel naar rechts
+ ledg=0;
+
+
+ }else if (emgRTfiltered > threshold_emgRT){
+ motor1_direction.write(direction1 = !direction1);
+ motor1_pwm.write(maxPWM1);
+ //tandenborstel naar achter
+ ledb=0;
+
+ }else if (emgRBfiltered > threshold_emgRB && emgLBfiltered > threshold_emgLB){
+ motor1_direction.write(direction1); //motor 1 gaat cw
+ motor1_pwm.write(maxPWM1);
+ //tandenborstel naar voren
+ ledg=0;
+ ledr=0;
+}
+}
+
+void demonstratie()
+{
+ if (state_changed) { //
+ pc.printf("Demonstratie gestart\r\n") ; //
+ state_changed = false;
+ }
+}
+
+
+void RKI()
{
- // Encoding is necessary for the computations of the joint angles and
- // velocities of the arm given certain linear velocities.
+ Lq1 = q1*r_trans;
+ Cq2 = q2/gear2; //1.25 is gear ratio voor q2
+
+ q1_dot = (v_x + (v_y*(L2*sin(q2/gear2)))/(L0 + q1*r_trans + L2*cos(q2/gear2)))/r_trans;
+ q2_dot = (gear2*v_y)/(L0 + q1*r_trans + L2*cos(q2/gear2));
+
+ 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 counts1 = Encoder1.getPulses();
- double rad_m1 = (rad_count * (double)counts1) + (1.0f*pi/3.0f);
- return rad_m1;
-}
-
-float Encoding2()
+double GetReferencePosition2()
+{
+ double potmeterwaarde2 = potMeter1.read();
+ double refP2 = potmeterwaarde2*maxwaarde;
+ return refP2;
+}
+
+double FeedBackControl(double error, double &e_int)
{
- // Encoding is necessary for the computations of the joint angles and
- // velocities of the arm given certain linear velocities.
+ //double Proportional= kp*error1;
+ double motorValue1= 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;
+ double motorValue2= kp2*error2;
+ //e_int2 = e_int2+dt*error2;
+ //double Integrator2 = ki2*e_int2;
+
+ //double motorValue2 = Proportional2 + Integrator2 ;
+ return motorValue2;
+}
- double counts2 = Encoder2.getPulses();
- double rad_m2 = (rad_count * (double)counts2) + (7.0f*pi/4.0f);
- return rad_m2;
-}
-
- void onDebugTick() {
-
- pc.printf("pot input1: %f\r\n", potMeterIn1.read());
- pc.printf("motorValue1: %f\r\n", feedForwardControl(getReferenceVelocity()));
- pc.printf("radialen motor 1: %f\r\n",Encoding());
+void MeasureAndControl()
+{
+ // RKI aanroepen
+ //RKI();
+ // hier the control of the 1st control system
+ refP = GetReferencePosition(); //moet eigenlijk nog met RKI
+ Huidigepositie1 = enc1.getPulses()*4200;
+ //error1 = (refP - Huidigepositie1);// make an error
+ error1 = refP ;// ffwd
+ 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
+ error2 = refP2;// ffwd
+ 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 but1_interrupt()
+{
+ /*if(but2.read()) {//both buttons are pressed
+ failure_occurred = true;
+ }*/
+ but1_pressed = true;
+ pc.printf("Button 1 interrupt active \n\r");
+}
+
+void but2_interrupt()
+{
+ /*if(but1.read()) {//both buttons are pressed
+ failure_occurred = true;
+ }*/
+ but2_pressed = true;
+ pc.printf("Button 2 interrupt active \n\r");
+}
- pc.printf("\n\n\n");
-
- /*
- scope.set(0, potMeterIn.read());
- scope.set(1, feedForwardControl(getReferenceVelocity()));
- scope.send();
- */
- pc.printf("pot input2: %f\r\n", potMeterIn2.read());
- pc.printf("motorValue2: %f\r\n", feedForwardControl2(getReferenceVelocity2()));
- pc.printf("radialen motor 2: %f\r\n",Encoding2());
- pc.printf("\n\n\n");
- }
-
+ 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
+ demonstratie();
+ 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_EMGcalibratie:
+ EMGcalibration();
+ break;
+
+ case s_EMG:
+ ledg=1;
+ ledr=1;
+ ledb=1;
+ state_changed=false;
+ filteren();
+ brushingmode();
+ if (but2_pressed) {
+ pc.printf("fail. \r\n");
+ but2_pressed = false;
+ state_changed = true;
+ state = s_idle;
+ }
+ break;
+
+ }
+}
+
int main()
{
pc.baud(115200);
-
- button2.fall(&onButtonPress2);
- controlTicker.attach(&measureAndControl, 1.0f/100.0f);
- debugTicker.attach(&onDebugTick, 1);
-
- button1.fall(&onButtonPress);
-
-}
\ No newline at end of file
+ pc.printf("Executing main()... \r\n");
+ state = s_idle;
+ ledg=1;
+ ledr=1;
+ ledb=1;
+
+ LeftBicepHP.add( &LBHP1 ); //BiQuadChain bqc; //bqc.add( &bq1 ).add( &bq2 );
+ LeftBicepN.add( &LBN1 );
+ LeftBicepLP.add( &LBLP1 );
+
+
+ RightBicepHP.add( &RBHP1 );
+ RightBicepN.add( &RBN1 );
+ RightBicepLP.add( &RBLP1 );
+
+ RightLegHP.add( &RTHP1 );
+ RightLegN.add( &RTN1 );
+ RightLegLP.add( &RTLP1 );
+
+ but1.fall(&but1_interrupt);
+ but2.fall(&but2_interrupt);
+ loop_ticker.attach(&state_machine, dt);
+ pc.printf("state_machine active!\n\r");
+
+}
+
+
+
\ No newline at end of file