Biorobotics 2019 groep 24 / Mbed 2 deprecated Final_code

Calibratie motor, demo, (EMG calibratie en movement werkt niet)

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL Encoder FastPWM

main.cpp@6:91cdad4e00e8, 2019-10-31 (annotated)

Committer:
S1933191
Date:
Thu Oct 31 22:40:32 2019 +0000
Revision:
6:91cdad4e00e8
Parent:
5:810892d669f9 
Uiteindelijke script, demo, emg(werkt niet)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
S1933191 2:5093b66c9b26 1 #include <math.h>
RobertoO 0:67c50348f842 2 #include "mbed.h"
RobertoO 1:b862262a9d14 3 #include "MODSERIAL.h"
S1933191 2:5093b66c9b26 4 #include "QEI.h"
S1933191 2:5093b66c9b26 5 #include "FastPWM.h"
S1933191 6:91cdad4e00e8 6 #include "HIDScope.h"
S1933191 6:91cdad4e00e8 7 #include <stdio.h> /* printf */
S1933191 6:91cdad4e00e8 8 #include <stdlib.h> /* abs */
S1933191 6:91cdad4e00e8 9 #include <complex>
S1933191 6:91cdad4e00e8 10 #include "BiQuad.h"
S1933191 2:5093b66c9b26 11
S1933191 6:91cdad4e00e8 12
S1933191 6:91cdad4e00e8 13 const double PI = 3.1415926535897;
S1933191 2:5093b66c9b26 14
S1933191 3:16df793da2be 15 InterruptIn but1(D1);
S1933191 3:16df793da2be 16 InterruptIn but2(D0);
S1933191 4:7d0df890e801 17 DigitalIn butsw2(SW3);
S1933191 4:7d0df890e801 18 DigitalIn butsw3(SW2);
S1933191 5:810892d669f9 19 AnalogIn potMeter1(A5);
S1933191 5:810892d669f9 20 AnalogIn potMeter2(A4);
S1933191 2:5093b66c9b26 21
S1933191 5:810892d669f9 22 DigitalOut motor1_direction(D4); //richting van motor1
S1933191 5:810892d669f9 23 FastPWM motor1_pwm(D5); //Motor 1 PWM controle van de snelheid
S1933191 5:810892d669f9 24 DigitalOut motor2_direction(D7); //richting van motor2
S1933191 5:810892d669f9 25 FastPWM motor2_pwm(D6); //Motor 2 PWM controle van de snelheid
S1933191 2:5093b66c9b26 26
S1933191 6:91cdad4e00e8 27 Ticker loop_ticker;
S1933191 2:5093b66c9b26 28 // SERIAL COMMUNICATION WITH PC.................................................
S1933191 2:5093b66c9b26 29 Serial pc(USBTX, USBRX);
S1933191 2:5093b66c9b26 30
S1933191 6:91cdad4e00e8 31 enum States {s_idle, s_calibratie_encoder, s_demonstratie, s_EMGcalibration, s_EMG};
S1933191 2:5093b66c9b26 32 States state;
S1933191 2:5093b66c9b26 33
S1933191 2:5093b66c9b26 34 // ENCODER .....................................................................
S1933191 4:7d0df890e801 35 QEI enc1 (D13, D12, NC, 4200); //encoder 1 gebruiken
S1933191 6:91cdad4e00e8 36 QEI enc2 (D9, D8, NC, 4200); //encoder 2 gebruiken
S1933191 6:91cdad4e00e8 37
S1933191 5:810892d669f9 38 const float dt = 0.001;
S1933191 2:5093b66c9b26 39 double e_int = 0;
S1933191 2:5093b66c9b26 40 double e_int2 = 0;
S1933191 5:810892d669f9 41 double theta1;
S1933191 5:810892d669f9 42 double theta2;
S1933191 5:810892d669f9 43 int enc1_zero = 0;
S1933191 5:810892d669f9 44 int enc2_zero = 0;
S1933191 2:5093b66c9b26 45 int enc1_value;
S1933191 2:5093b66c9b26 46 int enc2_value;
S1933191 5:810892d669f9 47 const int maxwaarde = 400;
S1933191 5:810892d669f9 48
S1933191 5:810892d669f9 49 volatile double Huidigepositie1=0;
S1933191 5:810892d669f9 50 volatile double Huidigepositie2=0;
S1933191 3:16df793da2be 51 volatile double motorValue1;
S1933191 3:16df793da2be 52 volatile double motorValue2;
S1933191 3:16df793da2be 53 volatile double refP;
S1933191 3:16df793da2be 54 volatile double refP2;
S1933191 3:16df793da2be 55 volatile double error1;
S1933191 3:16df793da2be 56 volatile double error2;
S1933191 5:810892d669f9 57
S1933191 6:91cdad4e00e8 58 bool state_changed = false;
S1933191 3:16df793da2be 59 volatile bool A=true;
S1933191 3:16df793da2be 60 volatile bool B=true;
S1933191 2:5093b66c9b26 61 volatile bool but1_pressed = false;
S1933191 2:5093b66c9b26 62 volatile bool but2_pressed = false;
S1933191 3:16df793da2be 63 volatile bool butsw2_pressed = false;
S1933191 3:16df793da2be 64 volatile bool butsw3_pressed = false;
S1933191 2:5093b66c9b26 65 volatile bool failure_occurred = false;
S1933191 2:5093b66c9b26 66
S1933191 3:16df793da2be 67 const bool clockwise = true;
S1933191 3:16df793da2be 68 volatile bool direction1 = clockwise;
S1933191 3:16df793da2be 69 volatile bool direction2 = clockwise;
S1933191 3:16df793da2be 70
S1933191 6:91cdad4e00e8 71 // RKI VARIABELEN...............................................................
S1933191 5:810892d669f9 72 // Lengtes zijn in meters
S1933191 5:810892d669f9 73 // Vaste variabelen:
S1933191 6:91cdad4e00e8 74 const double L0 = 0.25; // lengte arm 1 --> moet nog goed worden opgemeten!
S1933191 6:91cdad4e00e8 75 const double L2 = 0.375; // Lengte arm 2 --> moet ook nog goed opgemeten worden!
S1933191 6:91cdad4e00e8 76 const double r_trans = 0.035; // gebruiken voor translation naar shaft rotation
S1933191 5:810892d669f9 77
S1933191 6:91cdad4e00e8 78 // Variërende variabelen:
S1933191 5:810892d669f9 79 double q1 = 0; // Motorhoek van joint 1 op beginpositie
S1933191 5:810892d669f9 80 double q2 = PI/2; // Motorhoek van joint 2 op beginpositie
S1933191 5:810892d669f9 81 double v_x; // Snelheid van end effector in x richting --> Door EMG signals
S1933191 5:810892d669f9 82 double v_y; // Snelheid van end effector in y richting --> Door EMG signals
S1933191 5:810892d669f9 83
S1933191 5:810892d669f9 84 double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
S1933191 5:810892d669f9 85 double Cq2; // Joint angle van het systeem (gecorrigeerd voor gear ratio 1:1.1)
S1933191 5:810892d669f9 86
S1933191 6:91cdad4e00e8 87 double q1_dot; // Benodigde hoeksnelheid van motor 1
S1933191 6:91cdad4e00e8 88 double q2_dot; // Benodigde hoeksnelheid van motor 2
S1933191 5:810892d669f9 89
S1933191 6:91cdad4e00e8 90 double q1_ii; // Referentie signaal voor motorhoek q1
S1933191 6:91cdad4e00e8 91 double q2_ii; // Referentie signaal voor motorhoek q2
S1933191 5:810892d669f9 92
S1933191 5:810892d669f9 93 //PI VARIABELEN................................................................
S1933191 5:810892d669f9 94 const double kp=0.002; // Soort van geschaald --> meer onderzoek nodig
S1933191 5:810892d669f9 95 const double ki=0.0001;
S1933191 5:810892d669f9 96 const double kp2=0.002;
S1933191 5:810892d669f9 97 const double ki2=0.0001;
S1933191 5:810892d669f9 98
S1933191 6:91cdad4e00e8 99 //FILTERS EMG...................................................................
S1933191 6:91cdad4e00e8 100
S1933191 6:91cdad4e00e8 101 //Filters Linker Biceps
S1933191 6:91cdad4e00e8 102 //In de volgorde High pass, notch, low pass
S1933191 6:91cdad4e00e8 103 BiQuad LBHP1(0.995566972017647, -1.991133944035294, 0.995566972017647, 1.000000000000000, -1.991114292201654, 0.991153595868935);
S1933191 6:91cdad4e00e8 104 BiQuad LBN1( 0.5, 0, 0.5, 0, 0);
S1933191 6:91cdad4e00e8 105 BiQuad LBLP1(0000.087655548754006, 0000.175311097508013, 0000.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
S1933191 6:91cdad4e00e8 106 BiQuadChain LeftBicepHP;
S1933191 6:91cdad4e00e8 107 BiQuadChain LeftBicepN;
S1933191 6:91cdad4e00e8 108 BiQuadChain LeftBicepLP;
S1933191 6:91cdad4e00e8 109
S1933191 6:91cdad4e00e8 110 //Filters Rechter Biceps
S1933191 6:91cdad4e00e8 111 //In de volgorde High pass, notch, low pass
S1933191 6:91cdad4e00e8 112 BiQuad RBHP1(0.995566972017647, -1.991133944035294, 0.995566972017647, 1.000000000000000, -1.991114292201654, 0.991153595868935);
S1933191 6:91cdad4e00e8 113 BiQuad RBN1( 0.5, 0, 0.5, 0, 0);
S1933191 6:91cdad4e00e8 114 BiQuad RBLP1(0000.087655548754006, 0000.175311097508013, 0000.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
S1933191 6:91cdad4e00e8 115 BiQuadChain RightBicepHP;
S1933191 6:91cdad4e00e8 116 BiQuadChain RightBicepN;
S1933191 6:91cdad4e00e8 117 BiQuadChain RightBicepLP;
S1933191 6:91cdad4e00e8 118
S1933191 6:91cdad4e00e8 119
S1933191 6:91cdad4e00e8 120 //Filters Rechter Quadriceps
S1933191 6:91cdad4e00e8 121 //In de volgorde High pass, notch, low pass
S1933191 6:91cdad4e00e8 122 BiQuad RQHP1(0.995566972017647, -1.991133944035294, 0.995566972017647,1.000000000000000, -1.991114292201654, 0.991153595868935);
S1933191 6:91cdad4e00e8 123 BiQuad RQN1( 0.5, 0, 0.5, 0, 0);
S1933191 6:91cdad4e00e8 124 BiQuad RQLP1(0000.087655548754006, 0.175311097508013, 0.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
S1933191 6:91cdad4e00e8 125 BiQuadChain RightLegHP;
S1933191 6:91cdad4e00e8 126 BiQuadChain RightLegN;
S1933191 6:91cdad4e00e8 127 BiQuadChain RightLegLP;
S1933191 6:91cdad4e00e8 128
S1933191 6:91cdad4e00e8 129 double emgLBHP;
S1933191 6:91cdad4e00e8 130 double emgLBN;
S1933191 6:91cdad4e00e8 131 double emgLBA;
S1933191 6:91cdad4e00e8 132
S1933191 6:91cdad4e00e8 133 double emgRBHP;
S1933191 6:91cdad4e00e8 134 double emgRBN;
S1933191 6:91cdad4e00e8 135 double emgRBA;
S1933191 6:91cdad4e00e8 136 double emgRBLP;
S1933191 6:91cdad4e00e8 137 double emgAbsRBNotch;
S1933191 6:91cdad4e00e8 138
S1933191 6:91cdad4e00e8 139 double emgRQN;
S1933191 6:91cdad4e00e8 140 double emgRQHP;
S1933191 6:91cdad4e00e8 141 double emgRQA;
S1933191 6:91cdad4e00e8 142
S1933191 6:91cdad4e00e8 143 double emgLBfiltered;
S1933191 6:91cdad4e00e8 144 double emgRBfiltered;
S1933191 6:91cdad4e00e8 145 double emgRQfiltered;
S1933191 6:91cdad4e00e8 146 double emgLBraw;
S1933191 6:91cdad4e00e8 147 double emgRBraw;
S1933191 6:91cdad4e00e8 148 double emgRQraw;
S1933191 6:91cdad4e00e8 149
S1933191 6:91cdad4e00e8 150 double threshold_emgLB;
S1933191 6:91cdad4e00e8 151 double threshold_emgRB;
S1933191 6:91cdad4e00e8 152 double threshold_emgRQ;
S1933191 6:91cdad4e00e8 153 double threshold = 0.15;
S1933191 6:91cdad4e00e8 154 double emgLB_max = 0.000;
S1933191 6:91cdad4e00e8 155 double emgRB_max = 0.000;
S1933191 6:91cdad4e00e8 156 double emgRQ_max = 0.000;
S1933191 6:91cdad4e00e8 157 double emgLB_maxrust = 0.000;
S1933191 6:91cdad4e00e8 158 double emgRB_maxrust = 0.000;
S1933191 6:91cdad4e00e8 159 double emgRQ_maxrust = 0.000;
S1933191 6:91cdad4e00e8 160 int tijd = 0;
S1933191 6:91cdad4e00e8 161 double timecalibration;
S1933191 6:91cdad4e00e8 162
S1933191 6:91cdad4e00e8 163 double emgsumLB;
S1933191 6:91cdad4e00e8 164 double emgsumRB;
S1933191 6:91cdad4e00e8 165 double emgsumRQ;
S1933191 6:91cdad4e00e8 166 double restmeanLB = 0.000;
S1933191 6:91cdad4e00e8 167 double restmeanRB = 0.000;
S1933191 6:91cdad4e00e8 168 double restmeanRQ = 0.000;
S1933191 6:91cdad4e00e8 169
S1933191 6:91cdad4e00e8 170 double LBvalue;
S1933191 6:91cdad4e00e8 171 double RBvalue;
S1933191 6:91cdad4e00e8 172 double RQvalue;
S1933191 6:91cdad4e00e8 173 double emgLBrust;
S1933191 6:91cdad4e00e8 174 double emgRBrust;
S1933191 6:91cdad4e00e8 175 double emgRQrust;
S1933191 6:91cdad4e00e8 176 double RestmeanLB;
S1933191 6:91cdad4e00e8 177 double RestmeanRB;
S1933191 6:91cdad4e00e8 178 double RestmeanRQ;
S1933191 6:91cdad4e00e8 179
S1933191 6:91cdad4e00e8 180 DigitalOut led5(LED_RED);
S1933191 6:91cdad4e00e8 181
S1933191 6:91cdad4e00e8 182 AnalogIn emgLB(A0); //read EMG left bicep
S1933191 6:91cdad4e00e8 183 AnalogIn emgRB(A1); //read EMG right bicep
S1933191 6:91cdad4e00e8 184 AnalogIn emgRQ(A2); //read EMG right quadriceps
S1933191 6:91cdad4e00e8 185
S1933191 6:91cdad4e00e8 186 HIDScope scope(6); //aantal kanalen HIDScope (voor test 5, voor echt 6)
S1933191 6:91cdad4e00e8 187
S1933191 3:16df793da2be 188 void rest()
S1933191 6:91cdad4e00e8 189 // Rust. Hier wordt niets uitgevoerd. Wanneer button1
S1933191 6:91cdad4e00e8 190 {
S1933191 6:91cdad4e00e8 191 // wordt ingedrukt gaan we naar de volgende state waar
S1933191 5:810892d669f9 192 if (but1_pressed) { // de encoders worden gekalibreerd.
S1933191 2:5093b66c9b26 193 state_changed = true;
S1933191 3:16df793da2be 194 state = s_calibratie_encoder;
S1933191 2:5093b66c9b26 195 but1_pressed = false;
S1933191 2:5093b66c9b26 196 }
S1933191 2:5093b66c9b26 197 }
RobertoO 0:67c50348f842 198
S1933191 6:91cdad4e00e8 199 void calibratie_encoder() // calibratie encoder. Hier worden de encoders gekalibreerd en op
S1933191 6:91cdad4e00e8 200 {
S1933191 6:91cdad4e00e8 201 // 0 gezet. Wanneer op button 1 wordt gedrukt gaan we naar de
S1933191 6:91cdad4e00e8 202 if (state_changed) { // demo modus, wanneer op button 2 wordt gedruk gaan we naar
S1933191 5:810892d669f9 203 pc.printf("Started encoder calibration\r\n"); // de EMG calibratie
S1933191 2:5093b66c9b26 204 state_changed = false;
S1933191 2:5093b66c9b26 205 }
S1933191 2:5093b66c9b26 206 if (but1_pressed) {
S1933191 2:5093b66c9b26 207 pc.printf("Encoder has been calibrated \r\n");
S1933191 6:91cdad4e00e8 208 enc1_value = enc1.getPulses(); // wat geeft deze voor soort waarde?
S1933191 4:7d0df890e801 209 enc2_value = enc2.getPulses();
S1933191 5:810892d669f9 210 //enc1_value -= enc1_zero;
S1933191 5:810892d669f9 211 //enc2_value -= enc2_zero;
S1933191 4:7d0df890e801 212 theta1 = (float)(enc1_value)/(float)(4200)*2*PI;
S1933191 4:7d0df890e801 213 theta2 = (float)(enc2_value)/(float)(4200)*2*PI;
S1933191 2:5093b66c9b26 214 enc1_zero = enc1_value;
S1933191 2:5093b66c9b26 215 enc2_zero = enc2_value;
S1933191 2:5093b66c9b26 216 but1_pressed = false;
S1933191 2:5093b66c9b26 217 state_changed = true;
S1933191 5:810892d669f9 218 state = s_demonstratie;
S1933191 2:5093b66c9b26 219 pc.printf("enc01: %i\r\n", enc1_zero);
S1933191 2:5093b66c9b26 220 pc.printf("enc1value: %i\r\n", enc1_value);
S1933191 2:5093b66c9b26 221 pc.printf("enc02: %i\r\n",enc2_zero);
S1933191 2:5093b66c9b26 222 pc.printf("enc2value: %i\r\n", enc2_value);
S1933191 2:5093b66c9b26 223 pc.printf("hoek 1: %f\r\n",theta1);
S1933191 2:5093b66c9b26 224 pc.printf("hoek 2: %f\r\n",theta2);
S1933191 6:91cdad4e00e8 225 }
S1933191 6:91cdad4e00e8 226
S1933191 5:810892d669f9 227 if (but2_pressed) {
S1933191 5:810892d669f9 228 pc.printf("Encoder has been calibrated \r\n");
S1933191 5:810892d669f9 229 enc1_value = enc1.getPulses();
S1933191 5:810892d669f9 230 enc2_value = enc2.getPulses();
S1933191 5:810892d669f9 231 //enc1_value -= enc1_zero;
S1933191 5:810892d669f9 232 //enc2_value -= enc2_zero;
S1933191 5:810892d669f9 233 theta1 = (float)(enc1_value)/(float)(4200)*2*PI;
S1933191 5:810892d669f9 234 theta2 = (float)(enc2_value)/(float)(4200)*2*PI;
S1933191 5:810892d669f9 235 enc1_zero = enc1_value;
S1933191 5:810892d669f9 236 enc2_zero = enc2_value;
S1933191 5:810892d669f9 237 but2_pressed = false;
S1933191 3:16df793da2be 238 state_changed = true;
S1933191 6:91cdad4e00e8 239 state = s_EMGcalibration;
S1933191 5:810892d669f9 240 pc.printf("enc01: %i\r\n", enc1_zero);
S1933191 5:810892d669f9 241 pc.printf("enc1value: %i\r\n", enc1_value);
S1933191 5:810892d669f9 242 pc.printf("enc02: %i\r\n",enc2_zero);
S1933191 5:810892d669f9 243 pc.printf("enc2value: %i\r\n", enc2_value);
S1933191 5:810892d669f9 244 pc.printf("hoek 1: %f\r\n",theta1);
S1933191 5:810892d669f9 245 pc.printf("hoek 2: %f\r\n",theta2);
S1933191 5:810892d669f9 246 }
S1933191 5:810892d669f9 247 }
S1933191 5:810892d669f9 248
S1933191 6:91cdad4e00e8 249 void filteren()
S1933191 5:810892d669f9 250 {
S1933191 6:91cdad4e00e8 251 //linkerbicep
S1933191 6:91cdad4e00e8 252 emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
S1933191 6:91cdad4e00e8 253 emgLBHP=LeftBicepHP.step(emgLBraw);
S1933191 6:91cdad4e00e8 254 emgLBN=LeftBicepN.step(emgLBHP);
S1933191 6:91cdad4e00e8 255 emgLBA= fabs(emgLBN);
S1933191 6:91cdad4e00e8 256 emgLBfiltered=LeftBicepLP.step(emgLBA);
S1933191 6:91cdad4e00e8 257
S1933191 6:91cdad4e00e8 258 //to show if filter is working
S1933191 6:91cdad4e00e8 259 scope.set(0, emgLBraw);
S1933191 6:91cdad4e00e8 260 scope.set(1, emgLBfiltered);
S1933191 6:91cdad4e00e8 261 scope.set(2, emgRBraw);
S1933191 6:91cdad4e00e8 262 scope.set(3, emgRBfiltered);
S1933191 6:91cdad4e00e8 263 scope.set(4, emgRQraw);
S1933191 6:91cdad4e00e8 264 scope.set(5, emgRQfiltered);
S1933191 6:91cdad4e00e8 265
S1933191 6:91cdad4e00e8 266 //rechterbicep
S1933191 6:91cdad4e00e8 267 emgRBraw= emgRB.read();
S1933191 6:91cdad4e00e8 268 emgRBHP= RightBicepHP.step(emgRBraw);
S1933191 6:91cdad4e00e8 269 emgRBN=RightBicepN.step(emgRBHP);
S1933191 6:91cdad4e00e8 270 emgRBA= fabs(emgRBN);
S1933191 6:91cdad4e00e8 271 emgRBfiltered=RightBicepLP.step(emgRBA);
S1933191 6:91cdad4e00e8 272
S1933191 6:91cdad4e00e8 273 //Right Quadriceps
S1933191 6:91cdad4e00e8 274 emgRQraw= emgRQ.read();
S1933191 6:91cdad4e00e8 275 emgRQHP= RightLegHP.step(emgRQraw);
S1933191 6:91cdad4e00e8 276 emgRQN= RightLegN.step(emgRQHP);
S1933191 6:91cdad4e00e8 277 emgRQA= fabs(emgRQN);
S1933191 6:91cdad4e00e8 278 emgRQfiltered=RightLegLP.step(emgRQA);
S1933191 6:91cdad4e00e8 279
S1933191 6:91cdad4e00e8 280 scope.send();
S1933191 5:810892d669f9 281
S1933191 5:810892d669f9 282 }
S1933191 5:810892d669f9 283
S1933191 6:91cdad4e00e8 284 void EMGcalibration()
S1933191 2:5093b66c9b26 285 {
S1933191 6:91cdad4e00e8 286 if(state_changed) {
S1933191 6:91cdad4e00e8 287 state_changed = false;
S1933191 6:91cdad4e00e8 288 }
S1933191 6:91cdad4e00e8 289 /*if (state_changed) { */
S1933191 6:91cdad4e00e8 290 //pc.printf("Started EMG calibration\r\n");
S1933191 6:91cdad4e00e8 291 tijd= tijd + 1; //idealiter op een andere plek haha
S1933191 6:91cdad4e00e8 292 /*if (but1_pressed) {
S1933191 6:91cdad4e00e8 293 pc.printf("EMG calibratie \r\n");
S1933191 6:91cdad4e00e8 294 but1_pressed = false;*/
S1933191 6:91cdad4e00e8 295 emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
S1933191 6:91cdad4e00e8 296 emgLBHP=LeftBicepHP.step(emgLBraw);
S1933191 6:91cdad4e00e8 297 emgLBN=LeftBicepN.step(emgLBHP);
S1933191 6:91cdad4e00e8 298 emgLBA= fabs(emgLBN);
S1933191 6:91cdad4e00e8 299 emgLBfiltered=LeftBicepLP.step(emgLBA);
S1933191 6:91cdad4e00e8 300
S1933191 6:91cdad4e00e8 301 emgRBraw= emgRB.read();
S1933191 6:91cdad4e00e8 302 emgRBHP= RightBicepHP.step(emgRBraw);
S1933191 6:91cdad4e00e8 303 emgRBN=RightBicepN.step(emgRBHP);
S1933191 6:91cdad4e00e8 304 emgRBA= fabs(emgRBN);
S1933191 6:91cdad4e00e8 305 emgRBfiltered=RightBicepLP.step(emgRBA);
S1933191 6:91cdad4e00e8 306
S1933191 6:91cdad4e00e8 307 emgRQraw= emgRQ.read();
S1933191 6:91cdad4e00e8 308 emgRQHP= RightLegHP.step(emgRQraw);
S1933191 6:91cdad4e00e8 309 emgRQN= RightLegN.step(emgRQHP);
S1933191 6:91cdad4e00e8 310 emgRQA= fabs(emgRQN);
S1933191 6:91cdad4e00e8 311 emgRQfiltered=RightLegLP.step(emgRQA);
S1933191 6:91cdad4e00e8 312
S1933191 6:91cdad4e00e8 313 if (tijd > 1000 && tijd < 6000) {
S1933191 6:91cdad4e00e8 314 emgLBraw= emgLB.read();
S1933191 6:91cdad4e00e8 315 emgLBHP=LeftBicepHP.step(emgLBraw);
S1933191 6:91cdad4e00e8 316 emgLBN=LeftBicepN.step(emgLBHP);
S1933191 6:91cdad4e00e8 317 emgLBA= fabs(emgLBN);
S1933191 6:91cdad4e00e8 318 emgLBfiltered=LeftBicepLP.step(emgLBA);
S1933191 6:91cdad4e00e8 319
S1933191 6:91cdad4e00e8 320 if (emgLBfiltered > emgLB_max) {
S1933191 6:91cdad4e00e8 321 emgLB_max = emgLBfiltered;
S1933191 6:91cdad4e00e8 322 }
S1933191 6:91cdad4e00e8 323 //pc.baud(115200);
S1933191 6:91cdad4e00e8 324 //pc.printf("emgLB_max = %f \r\n", emgLB_max);// geen tekst printen in ticker want dat gaat mis xjes
S1933191 6:91cdad4e00e8 325 led5=0; //led gaat aan zodra je linkerbicep moet aanspannen
S1933191 6:91cdad4e00e8 326 }
S1933191 6:91cdad4e00e8 327
S1933191 6:91cdad4e00e8 328 else if (tijd > 7000 && tijd < 12000) {
S1933191 6:91cdad4e00e8 329 emgRBraw= emgRB.read();
S1933191 6:91cdad4e00e8 330 emgRBHP= RightBicepHP.step(emgRBraw);
S1933191 6:91cdad4e00e8 331 emgRBN=RightBicepN.step(emgRBHP);
S1933191 6:91cdad4e00e8 332 emgRBA= fabs(emgRBN);
S1933191 6:91cdad4e00e8 333 emgRBfiltered=RightBicepLP.step(emgRBA);
S1933191 6:91cdad4e00e8 334 if (emgRBfiltered > emgRB_max) {
S1933191 6:91cdad4e00e8 335 emgRB_max = emgRBfiltered;
S1933191 6:91cdad4e00e8 336 }
S1933191 6:91cdad4e00e8 337 //pc.printf("emgRB_max = %f \r\n", emgRB_max);
S1933191 6:91cdad4e00e8 338 led5=0; //led gaat uit zodra je rechterbicep moet aanspannen
S1933191 6:91cdad4e00e8 339 }
S1933191 6:91cdad4e00e8 340
S1933191 6:91cdad4e00e8 341
S1933191 6:91cdad4e00e8 342 else if (tijd > 13000 && tijd < 18000) {
S1933191 6:91cdad4e00e8 343 emgRQraw= emgRQ.read();
S1933191 6:91cdad4e00e8 344 emgRQHP= RightLegHP.step(emgRQraw);
S1933191 6:91cdad4e00e8 345 emgRQN= RightLegN.step(emgRQHP);
S1933191 6:91cdad4e00e8 346
S1933191 6:91cdad4e00e8 347 emgRQA= fabs(emgRQN);
S1933191 6:91cdad4e00e8 348 emgRQfiltered=RightLegLP.step(emgRQA);
S1933191 6:91cdad4e00e8 349 if (emgRQfiltered > emgRQ_max) {
S1933191 6:91cdad4e00e8 350 emgRQ_max = emgRQfiltered;
S1933191 6:91cdad4e00e8 351 }
S1933191 6:91cdad4e00e8 352 //pc.printf("emgRQ_max = %f \r\n", emgRQ_max);
S1933191 6:91cdad4e00e8 353 led5=0; //led gaat aan zodra je rechterbeenspier moet aanspannen
S1933191 6:91cdad4e00e8 354
S1933191 6:91cdad4e00e8 355 } else {
S1933191 6:91cdad4e00e8 356 led5=1; //led gaat uit zodra kalibratie voltooid
S1933191 6:91cdad4e00e8 357 }
S1933191 6:91cdad4e00e8 358
S1933191 6:91cdad4e00e8 359
S1933191 6:91cdad4e00e8 360 threshold_emgLB = threshold*emgLB_max; //bepaal threshold, nu op 0.15*maximale waarde.
S1933191 6:91cdad4e00e8 361 threshold_emgRB = threshold*emgRB_max;
S1933191 6:91cdad4e00e8 362 threshold_emgRQ = threshold*emgRQ_max;
S1933191 6:91cdad4e00e8 363
S1933191 6:91cdad4e00e8 364 state_changed = true;
S1933191 6:91cdad4e00e8 365 state = s_EMG;
S1933191 6:91cdad4e00e8 366
S1933191 6:91cdad4e00e8 367 // wanneer threshold_emgx is bereikt, wordt de brushingmode geactiveerd.
S1933191 3:16df793da2be 368 }
S1933191 3:16df793da2be 369
S1933191 5:810892d669f9 370
S1933191 6:91cdad4e00e8 371
S1933191 6:91cdad4e00e8 372
S1933191 6:91cdad4e00e8 373 void SetMotor1(float motorValue1)
S1933191 6:91cdad4e00e8 374 {
S1933191 5:810892d669f9 375 // gegeven motorValue1 <=1, schrijft snelheid naar pwm.
S1933191 5:810892d669f9 376 // MotorValues buiten range worden afgekapt dat ze binnen de range vallen.
S1933191 3:16df793da2be 377 motor1_pwm.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
S1933191 2:5093b66c9b26 378 }
S1933191 3:16df793da2be 379
S1933191 6:91cdad4e00e8 380 void SetMotor2(float motorValue2)
S1933191 6:91cdad4e00e8 381 {
S1933191 3:16df793da2be 382 motor2_pwm.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
S1933191 6:91cdad4e00e8 383 }
S1933191 6:91cdad4e00e8 384
S1933191 6:91cdad4e00e8 385 void brushingmode()
S1933191 6:91cdad4e00e8 386 {
S1933191 6:91cdad4e00e8 387 if (state_changed) {
S1933191 6:91cdad4e00e8 388 state_changed = false;
S1933191 6:91cdad4e00e8 389 }
S1933191 6:91cdad4e00e8 390
S1933191 6:91cdad4e00e8 391
S1933191 6:91cdad4e00e8 392 if (emgLBfiltered > threshold_emgLB) {
S1933191 6:91cdad4e00e8 393 //tandenborstel naar links (cw/ccw) //direction1/2 = cw/ccw
S1933191 6:91cdad4e00e8 394 motor2_direction.write(direction1); //1/2 is welke motor
S1933191 6:91cdad4e00e8 395 //motor1_direction.write(direction1 = !direction1); //is counterclockwise
S1933191 6:91cdad4e00e8 396 SetMotor2(motorValue2); //1/2 welke motor
S1933191 6:91cdad4e00e8 397
S1933191 6:91cdad4e00e8 398 } else if (emgRBfiltered > threshold_emgRB) {
S1933191 6:91cdad4e00e8 399 //tandenborstel naar rechts
S1933191 6:91cdad4e00e8 400
S1933191 6:91cdad4e00e8 401 } else if (emgRQfiltered > threshold_emgRQ) {
S1933191 6:91cdad4e00e8 402 //tandenborstel naar achter
S1933191 6:91cdad4e00e8 403
S1933191 6:91cdad4e00e8 404 } else if (emgRBfiltered > threshold_emgRB && emgLBfiltered > threshold_emgLB) {
S1933191 6:91cdad4e00e8 405 //tandenborstel naar voren
S1933191 6:91cdad4e00e8 406
S1933191 6:91cdad4e00e8 407 }
S1933191 6:91cdad4e00e8 408 }
S1933191 6:91cdad4e00e8 409
S1933191 6:91cdad4e00e8 410 void demonstratie()
S1933191 6:91cdad4e00e8 411 {
S1933191 6:91cdad4e00e8 412 if (state_changed) { //
S1933191 6:91cdad4e00e8 413 pc.printf("Demonstratie gestart\r\n") ; //
S1933191 6:91cdad4e00e8 414 state_changed = false;
S1933191 6:91cdad4e00e8 415 }
S1933191 6:91cdad4e00e8 416 }
S1933191 6:91cdad4e00e8 417
S1933191 6:91cdad4e00e8 418
S1933191 6:91cdad4e00e8 419 void RKI()
S1933191 6:91cdad4e00e8 420 {
S1933191 6:91cdad4e00e8 421 Lq1 = q1*r_trans;
S1933191 6:91cdad4e00e8 422 Cq2 = q2/1.1; //1.1 is gear ratio
S1933191 6:91cdad4e00e8 423
S1933191 6:91cdad4e00e8 424 q1_dot = (v_x + (v_y*(L2*sin(q2/1.1)))/(L0 + q1*r_trans + L2*cos(q2/1.1)))/r_trans;
S1933191 6:91cdad4e00e8 425 q2_dot = (1.1*v_y)/(L0 + q1*r_trans + L2*cos(q2/1.1));
S1933191 6:91cdad4e00e8 426
S1933191 6:91cdad4e00e8 427 q1_ii = q1 + q1_dot*dt;
S1933191 6:91cdad4e00e8 428 q2_ii = q2 + q2_dot*dt;
S1933191 6:91cdad4e00e8 429
S1933191 6:91cdad4e00e8 430 q1 = q1_ii;
S1933191 6:91cdad4e00e8 431 q2 = q2_ii;
S1933191 6:91cdad4e00e8 432
S1933191 6:91cdad4e00e8 433 }
S1933191 6:91cdad4e00e8 434
S1933191 6:91cdad4e00e8 435 double GetReferencePosition()
S1933191 6:91cdad4e00e8 436 {
S1933191 6:91cdad4e00e8 437 double Potmeterwaarde = potMeter2.read();
S1933191 6:91cdad4e00e8 438
S1933191 6:91cdad4e00e8 439 double refP = Potmeterwaarde*maxwaarde;
S1933191 6:91cdad4e00e8 440 return refP;
S1933191 6:91cdad4e00e8 441 }
S1933191 6:91cdad4e00e8 442
S1933191 6:91cdad4e00e8 443 double GetReferencePosition2()
S1933191 6:91cdad4e00e8 444 {
S1933191 6:91cdad4e00e8 445 double potmeterwaarde2 = potMeter1.read();
S1933191 6:91cdad4e00e8 446 double refP2 = potmeterwaarde2*maxwaarde;
S1933191 6:91cdad4e00e8 447 return refP2;
S1933191 6:91cdad4e00e8 448 }
S1933191 6:91cdad4e00e8 449
S1933191 6:91cdad4e00e8 450 double FeedBackControl(double error, double &e_int)
S1933191 6:91cdad4e00e8 451 {
S1933191 6:91cdad4e00e8 452 double Proportional= kp*error1;
S1933191 6:91cdad4e00e8 453
S1933191 6:91cdad4e00e8 454 e_int = e_int+dt*error1;
S1933191 6:91cdad4e00e8 455 double Integrator = ki*e_int;
S1933191 6:91cdad4e00e8 456
S1933191 6:91cdad4e00e8 457 motorValue1 = Proportional + Integrator ;
S1933191 6:91cdad4e00e8 458 return motorValue1;
S1933191 6:91cdad4e00e8 459 }
S1933191 6:91cdad4e00e8 460
S1933191 6:91cdad4e00e8 461 double FeedBackControl2(double error2, double &e_int2)
S1933191 6:91cdad4e00e8 462 {
S1933191 6:91cdad4e00e8 463 double Proportional2= kp2*error2;
S1933191 6:91cdad4e00e8 464
S1933191 6:91cdad4e00e8 465 e_int2 = e_int2+dt*error2;
S1933191 6:91cdad4e00e8 466 double Integrator2 = ki2*e_int2;
S1933191 6:91cdad4e00e8 467
S1933191 6:91cdad4e00e8 468 double motorValue2 = Proportional2 + Integrator2 ;
S1933191 6:91cdad4e00e8 469 return motorValue2;
S1933191 6:91cdad4e00e8 470 }
S1933191 6:91cdad4e00e8 471
S1933191 5:810892d669f9 472 void MeasureAndControl()
S1933191 2:5093b66c9b26 473 {
S1933191 2:5093b66c9b26 474 // RKI aanroepen
S1933191 2:5093b66c9b26 475 //RKI();
S1933191 2:5093b66c9b26 476 // hier the control of the 1st control system
S1933191 5:810892d669f9 477 refP = GetReferencePosition(); //moet eigenlijk nog met RKI
S1933191 6:91cdad4e00e8 478 Huidigepositie1 = enc1.getPulses();
S1933191 3:16df793da2be 479 error1 = (refP - Huidigepositie1);// make an error
S1933191 3:16df793da2be 480 motorValue1 = FeedBackControl(error1, e_int);
S1933191 3:16df793da2be 481 SetMotor1(motorValue1);
S1933191 2:5093b66c9b26 482 // hier the control of the 2nd control system
S1933191 6:91cdad4e00e8 483 refP2 = GetReferencePosition2();
S1933191 6:91cdad4e00e8 484 Huidigepositie2 = enc2.getPulses();
S1933191 3:16df793da2be 485 error2 = (refP2 - Huidigepositie2);// make an error
S1933191 3:16df793da2be 486 motorValue2 = FeedBackControl2(error2, e_int2);
S1933191 2:5093b66c9b26 487 SetMotor2(motorValue2);
S1933191 5:810892d669f9 488 pc.printf("refP = %d, huidigepos = %d, motorvalue = %d, refP2 = %d, huidigepos2 = %d, motorvalue2 = %d \r\n", refP, Huidigepositie1, motorValue1, refP2, Huidigepositie2, Huidigepositie2);
S1933191 6:91cdad4e00e8 489
S1933191 2:5093b66c9b26 490 }
S1933191 3:16df793da2be 491
S1933191 3:16df793da2be 492 void direction()
S1933191 6:91cdad4e00e8 493 {
S1933191 3:16df793da2be 494 if (butsw2==0) {
S1933191 6:91cdad4e00e8 495 if (A==true) { // zodat het knopje 1 x wordt afgelezen
S1933191 6:91cdad4e00e8 496 // richting veranderen
S1933191 6:91cdad4e00e8 497 motor1_direction.write(direction1 = !direction1);
S1933191 6:91cdad4e00e8 498 pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
S1933191 6:91cdad4e00e8 499 A=false;
S1933191 3:16df793da2be 500 }
S1933191 6:91cdad4e00e8 501 } else {
S1933191 6:91cdad4e00e8 502 A=true;
S1933191 3:16df793da2be 503 }
S1933191 6:91cdad4e00e8 504
S1933191 6:91cdad4e00e8 505 if (butsw3==0) {
S1933191 6:91cdad4e00e8 506 if (B==true) {
S1933191 6:91cdad4e00e8 507 motor2_direction.write(direction2 = !direction2);
S1933191 6:91cdad4e00e8 508 pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
S1933191 6:91cdad4e00e8 509 B=false;
S1933191 3:16df793da2be 510 }
S1933191 6:91cdad4e00e8 511 } else {
S1933191 6:91cdad4e00e8 512 B=true;
S1933191 3:16df793da2be 513 }
S1933191 6:91cdad4e00e8 514
S1933191 3:16df793da2be 515 }
S1933191 6:91cdad4e00e8 516
S1933191 2:5093b66c9b26 517 void state_machine()
S1933191 2:5093b66c9b26 518 {
S1933191 2:5093b66c9b26 519 //run current state
S1933191 2:5093b66c9b26 520 switch (state) {
S1933191 5:810892d669f9 521 case s_idle: // in deze state gebeurd niets. Als op knop 1 wordt gedrukt
S1933191 5:810892d669f9 522 rest(); // gaan we over naar s_calibratie_encoder
S1933191 5:810892d669f9 523 break;
S1933191 5:810892d669f9 524 case s_calibratie_encoder: // in deze state worden de encoders gekalibreerd. knop 1 -> s_demonstratie
S1933191 5:810892d669f9 525 calibratie_encoder(); // knop 2 -> s_EMGcalibratie
S1933191 5:810892d669f9 526 break;
S1933191 5:810892d669f9 527 case s_demonstratie: // in deze state kunnen de motors worden bestuurd met de potmeters
S1933191 5:810892d669f9 528 MeasureAndControl(); // en switch 2 en 3( pot1,sw2->motor1 / pot2,sw3->motor2
S1933191 5:810892d669f9 529 direction(); // als op knop 2 wordt gedrukt komen we in de s_idle state
S1933191 5:810892d669f9 530 if (but2_pressed) {
S1933191 6:91cdad4e00e8 531 pc.printf("fail. \r\n");
S1933191 6:91cdad4e00e8 532 but2_pressed = false;
S1933191 6:91cdad4e00e8 533 state_changed = true;
S1933191 6:91cdad4e00e8 534 state = s_idle;
S1933191 5:810892d669f9 535 }
S1933191 5:810892d669f9 536 break;
S1933191 6:91cdad4e00e8 537 case s_EMGcalibration:
S1933191 6:91cdad4e00e8 538 EMGcalibration();
S1933191 2:5093b66c9b26 539 break;
S1933191 6:91cdad4e00e8 540 case s_EMG:
S1933191 6:91cdad4e00e8 541 filteren();
S1933191 6:91cdad4e00e8 542 brushingmode();
S1933191 6:91cdad4e00e8 543 break;
S1933191 6:91cdad4e00e8 544
S1933191 6:91cdad4e00e8 545 }
S1933191 2:5093b66c9b26 546 }
RobertoO 0:67c50348f842 547
S1933191 2:5093b66c9b26 548 void but1_interrupt()
S1933191 2:5093b66c9b26 549 {
S1933191 6:91cdad4e00e8 550 //if(but2.read()) {//both buttons are pressed
S1933191 6:91cdad4e00e8 551 // failure_occurred = true;
S1933191 6:91cdad4e00e8 552 //}
S1933191 2:5093b66c9b26 553 but1_pressed = true;
S1933191 6:91cdad4e00e8 554 pc.printf("Button 1 pressed \n\r");
S1933191 2:5093b66c9b26 555 }
S1933191 6:91cdad4e00e8 556
S1933191 2:5093b66c9b26 557 void but2_interrupt()
S1933191 2:5093b66c9b26 558 {
S1933191 6:91cdad4e00e8 559 //if(but1.read()) {//both buttons are pressed
S1933191 6:91cdad4e00e8 560 // failure_occurred = true;
S1933191 6:91cdad4e00e8 561 //}
S1933191 2:5093b66c9b26 562 but2_pressed = true;
S1933191 2:5093b66c9b26 563 pc.printf("Button 2 pressed \n\r");
S1933191 2:5093b66c9b26 564 }
S1933191 6:91cdad4e00e8 565
S1933191 2:5093b66c9b26 566 void main_loop()
S1933191 2:5093b66c9b26 567 {
S1933191 2:5093b66c9b26 568 state_machine();
S1933191 2:5093b66c9b26 569 }
RobertoO 0:67c50348f842 570
RobertoO 0:67c50348f842 571 int main()
RobertoO 0:67c50348f842 572 {
RobertoO 0:67c50348f842 573 pc.baud(115200);
S1933191 2:5093b66c9b26 574 pc.printf("Executing main()... \r\n");
S1933191 2:5093b66c9b26 575 state = s_idle;
S1933191 6:91cdad4e00e8 576
S1933191 6:91cdad4e00e8 577
S1933191 2:5093b66c9b26 578 but1.fall(&but1_interrupt);
S1933191 2:5093b66c9b26 579 but2.fall(&but2_interrupt);
S1933191 2:5093b66c9b26 580 loop_ticker.attach(&main_loop, dt);
S1933191 2:5093b66c9b26 581 pc.printf("main_loop is running\n\r");
S1933191 6:91cdad4e00e8 582
S1933191 5:810892d669f9 583 }
S1933191 2:5093b66c9b26 584
S1933191 6:91cdad4e00e8 585