Biorobotics 2019 groep 24 / Mbed 2 deprecated kalibratieemg

kalibratie emg, motors worden soortvan aangestuurd met emg gaat nog niet gecontroleerd

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

emgmeting.cpp@5:a53081a119c0, 2019-10-31 (annotated)

Committer:
linde101
Date:
Thu Oct 31 22:17:15 2019 +0000
Revision:
5:a53081a119c0
Parent:
3:9b8d3180fe48 
laatste versie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
linde101 2:8ed9953fc4ab 1 #include "mbed.h"
linde101 2:8ed9953fc4ab 2 #include "HIDScope.h"
linde101 2:8ed9953fc4ab 3 #include "QEI.h"
linde101 2:8ed9953fc4ab 4 #include "MODSERIAL.h"
linde101 2:8ed9953fc4ab 5 #include "BiQuad.h"
linde101 2:8ed9953fc4ab 6 #include "FastPWM.h"
linde101 2:8ed9953fc4ab 7 #include "math.h"
linde101 2:8ed9953fc4ab 8 #include <stdio.h> /* printf */
linde101 2:8ed9953fc4ab 9 #include <stdlib.h> /* abs */
linde101 2:8ed9953fc4ab 10 #include <complex>
linde101 2:8ed9953fc4ab 11 #include "FastPWM.h"
linde101 2:8ed9953fc4ab 12
linde101 2:8ed9953fc4ab 13 MODSERIAL pc(USBTX, USBRX);
linde101 2:8ed9953fc4ab 14
linde101 2:8ed9953fc4ab 15 //Filters EMG
linde101 2:8ed9953fc4ab 16 //Filters Linker Biceps
linde101 2:8ed9953fc4ab 17 //In de volgorde High pass, notch, low pass
linde101 2:8ed9953fc4ab 18 //BiQuad LBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
linde101 2:8ed9953fc4ab 19 //BiQuad LBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
linde101 2:8ed9953fc4ab 20 BiQuad LBHP1(0.995566972017647, -1.991133944035294, 0.995566972017647, 1.000000000000000, -1.991114292201654, 0.991153595868935);
linde101 2:8ed9953fc4ab 21 //BiQuad LBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
linde101 2:8ed9953fc4ab 22 BiQuad LBN1( 0.5, 0, 0.5, 0, 0);
linde101 2:8ed9953fc4ab 23 BiQuad LBLP1(0000.087655548754006, 0000.175311097508013, 0000.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
linde101 2:8ed9953fc4ab 24 //BiQuad LBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
linde101 2:8ed9953fc4ab 25 BiQuadChain LeftBicepHP;
linde101 2:8ed9953fc4ab 26 BiQuadChain LeftBicepN;
linde101 2:8ed9953fc4ab 27 BiQuadChain LeftBicepLP;
linde101 2:8ed9953fc4ab 28
linde101 2:8ed9953fc4ab 29 //Filters Rechter Biceps
linde101 2:8ed9953fc4ab 30 //In de volgorde High pass, notch, low pass
linde101 2:8ed9953fc4ab 31 //BiQuad RBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
linde101 2:8ed9953fc4ab 32 //BiQuad RBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
linde101 2:8ed9953fc4ab 33 BiQuad RBHP1(0.995566972017647, -1.991133944035294, 0.995566972017647, 1.000000000000000, -1.991114292201654, 0.991153595868935);
linde101 2:8ed9953fc4ab 34 //BiQuad RBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
linde101 2:8ed9953fc4ab 35 BiQuad RBN1( 0.5, 0, 0.5, 0, 0);
linde101 2:8ed9953fc4ab 36 BiQuad RBLP1(0000.087655548754006, 0000.175311097508013, 0000.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
linde101 2:8ed9953fc4ab 37 //BiQuad RBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
linde101 2:8ed9953fc4ab 38 BiQuadChain RightBicepHP;
linde101 2:8ed9953fc4ab 39 BiQuadChain RightBicepN;
linde101 2:8ed9953fc4ab 40 BiQuadChain RightBicepLP;
linde101 2:8ed9953fc4ab 41
linde101 2:8ed9953fc4ab 42
linde101 2:8ed9953fc4ab 43 //Filters Rechter Quadriceps
linde101 2:8ed9953fc4ab 44 //In de volgorde High pass, notch, low pass
linde101 2:8ed9953fc4ab 45 //BiQuad RTHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
linde101 2:8ed9953fc4ab 46 //BiQuad RTHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
linde101 2:8ed9953fc4ab 47 BiQuad RTHP1(0.995566972017647, -1.991133944035294 , 0.995566972017647 ,1.000000000000000, -1.991114292201654 , 0.991153595868935);
linde101 2:8ed9953fc4ab 48 //BiQuad RTHP2(1, 1.999999966458334, 0.999999966458334, -1.988418014198592, 0.988451549797368);
linde101 2:8ed9953fc4ab 49 BiQuad RTN1( 0.5, 0, 0.5, 0, 0);
linde101 2:8ed9953fc4ab 50 BiQuad RTLP1(0000.087655548754006 , 0.175311097508013 , 0.087655548754006, 1.000000000000000 , -1.973344249781299 , 0.973694871976315);
linde101 2:8ed9953fc4ab 51 //BiQuad RTLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
linde101 2:8ed9953fc4ab 52 BiQuadChain RightLegHP;
linde101 2:8ed9953fc4ab 53 BiQuadChain RightLegN;
linde101 2:8ed9953fc4ab 54 BiQuadChain RightLegLP;
linde101 2:8ed9953fc4ab 55
linde101 2:8ed9953fc4ab 56 double emgLBHP;
linde101 2:8ed9953fc4ab 57 double emgLBN;
linde101 2:8ed9953fc4ab 58 double emgLBA;
linde101 2:8ed9953fc4ab 59 //double emgLBLP;
linde101 2:8ed9953fc4ab 60 //double emgAbsLBNotch;
linde101 2:8ed9953fc4ab 61
linde101 2:8ed9953fc4ab 62 double emgRBHP;
linde101 2:8ed9953fc4ab 63 double emgRBN;
linde101 2:8ed9953fc4ab 64 double emgRBA;
linde101 2:8ed9953fc4ab 65 double emgRBLP;
linde101 2:8ed9953fc4ab 66 double emgAbsRBNotch;
linde101 2:8ed9953fc4ab 67
linde101 2:8ed9953fc4ab 68 double emgRTN;
linde101 2:8ed9953fc4ab 69 double emgRTHP;
linde101 2:8ed9953fc4ab 70 double emgRTA;
linde101 2:8ed9953fc4ab 71 //double emgRTLP;
linde101 2:8ed9953fc4ab 72 //double emgAbsRTNotch;
linde101 2:8ed9953fc4ab 73
linde101 2:8ed9953fc4ab 74 double emgLBfiltered;
linde101 2:8ed9953fc4ab 75 double emgRBfiltered;
linde101 2:8ed9953fc4ab 76 double emgRTfiltered;
linde101 2:8ed9953fc4ab 77 double emgLBraw;
linde101 2:8ed9953fc4ab 78 double emgRBraw;
linde101 2:8ed9953fc4ab 79 double emgRTraw;
linde101 2:8ed9953fc4ab 80
linde101 2:8ed9953fc4ab 81
linde101 2:8ed9953fc4ab 82 double threshold_emgLB;
linde101 2:8ed9953fc4ab 83 double threshold_emgRB;
linde101 2:8ed9953fc4ab 84 double threshold_emgRT;
linde101 2:8ed9953fc4ab 85 double threshold = 0.25;
linde101 2:8ed9953fc4ab 86 double emgLB_max = 0.000;
linde101 2:8ed9953fc4ab 87 double emgRB_max = 0.000;
linde101 2:8ed9953fc4ab 88 double emgRT_max = 0.000;
linde101 2:8ed9953fc4ab 89 double emgLB_maxrust = 0.000;
linde101 2:8ed9953fc4ab 90 double emgRB_maxrust = 0.000;
linde101 2:8ed9953fc4ab 91 double emgRT_maxrust = 0.000;
linde101 2:8ed9953fc4ab 92 int tijd = 0;
linde101 2:8ed9953fc4ab 93 double timecalibration;
linde101 2:8ed9953fc4ab 94
linde101 2:8ed9953fc4ab 95 double emgsumLB;
linde101 2:8ed9953fc4ab 96 double emgsumRB;
linde101 2:8ed9953fc4ab 97 double emgsumRT;
linde101 2:8ed9953fc4ab 98 double restmeanLB = 0.000;
linde101 2:8ed9953fc4ab 99 double restmeanRB = 0.000;
linde101 2:8ed9953fc4ab 100 double restmeanRT = 0.000;
linde101 2:8ed9953fc4ab 101 //double emgmeansubLB;
linde101 2:8ed9953fc4ab 102 //double emgmeansubRB;
linde101 2:8ed9953fc4ab 103 //double emgmeansubRT;
linde101 2:8ed9953fc4ab 104 double LBvalue;
linde101 2:8ed9953fc4ab 105 double RBvalue;
linde101 2:8ed9953fc4ab 106 double RTvalue;
linde101 2:8ed9953fc4ab 107 double emgLBrust;
linde101 2:8ed9953fc4ab 108 double emgRBrust;
linde101 2:8ed9953fc4ab 109 double emgRTrust;
linde101 2:8ed9953fc4ab 110 double RestmeanLB;
linde101 2:8ed9953fc4ab 111 double RestmeanRB;
linde101 2:8ed9953fc4ab 112 double RestmeanRT;
linde101 2:8ed9953fc4ab 113
linde101 2:8ed9953fc4ab 114
linde101 2:8ed9953fc4ab 115
linde101 2:8ed9953fc4ab 116 const bool clockwise = true;
linde101 2:8ed9953fc4ab 117 volatile bool direction1 = clockwise;
linde101 2:8ed9953fc4ab 118 volatile bool direction2 = clockwise;
linde101 2:8ed9953fc4ab 119
linde101 2:8ed9953fc4ab 120 double maxPWM1 = 0.2;
linde101 2:8ed9953fc4ab 121 double maxPWM2 = 0.2;
linde101 2:8ed9953fc4ab 122
linde101 2:8ed9953fc4ab 123 DigitalOut led5(LED_RED);
linde101 2:8ed9953fc4ab 124
linde101 2:8ed9953fc4ab 125 DigitalOut motor1_direction(D4); //richting van motor1
linde101 2:8ed9953fc4ab 126 FastPWM motor1_pwm(D5); //Motor 1 PWM controle van de snelheid
linde101 2:8ed9953fc4ab 127 DigitalOut motor2_direction(D7); //richting van motor2
linde101 2:8ed9953fc4ab 128 FastPWM motor2_pwm(D6); //Motor 2 PWM controle van de snelheid
linde101 2:8ed9953fc4ab 129
linde101 2:8ed9953fc4ab 130 AnalogIn emgLB(A0); //read EMG left bicep
linde101 2:8ed9953fc4ab 131 AnalogIn emgRB(A1); //read EMG right bicep
linde101 2:8ed9953fc4ab 132 AnalogIn emgRT(A2); //read EMG right quadriceps
linde101 2:8ed9953fc4ab 133
linde101 2:8ed9953fc4ab 134 HIDScope scope(6); //aantal kanalen HIDScope (voor test 5, voor echt 6)
linde101 2:8ed9953fc4ab 135
linde101 2:8ed9953fc4ab 136 Ticker filter;
linde101 2:8ed9953fc4ab 137 Ticker calibreren;
linde101 2:8ed9953fc4ab 138 Ticker actie;
linde101 2:8ed9953fc4ab 139
linde101 2:8ed9953fc4ab 140 void Filteren()
linde101 2:8ed9953fc4ab 141 {
linde101 2:8ed9953fc4ab 142 //linkerbicep
linde101 2:8ed9953fc4ab 143 emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
linde101 2:8ed9953fc4ab 144 emgLBHP=LeftBicepHP.step(emgLBraw);
linde101 2:8ed9953fc4ab 145 emgLBN=LeftBicepN.step(emgLBHP);
linde101 2:8ed9953fc4ab 146 //emgmeansubLB = emgLBN - RestmeanLB;
linde101 2:8ed9953fc4ab 147 emgLBA= fabs(emgLBN);
linde101 2:8ed9953fc4ab 148 emgLBfiltered=LeftBicepLP.step(emgLBA);
linde101 2:8ed9953fc4ab 149 //LBvalue = emgLBfiltered/emgLB_max;
linde101 2:8ed9953fc4ab 150
linde101 2:8ed9953fc4ab 151 //to show if filter is working
linde101 2:8ed9953fc4ab 152 scope.set(0, emgLBraw);
linde101 2:8ed9953fc4ab 153 scope.set(1, emgLBfiltered);
linde101 2:8ed9953fc4ab 154 scope.set(2, emgRBraw);
linde101 2:8ed9953fc4ab 155 scope.set(3, emgRBfiltered);
linde101 2:8ed9953fc4ab 156 scope.set(4, emgRTraw);
linde101 2:8ed9953fc4ab 157 scope.set(5, emgRTfiltered);
linde101 2:8ed9953fc4ab 158
linde101 2:8ed9953fc4ab 159
linde101 2:8ed9953fc4ab 160 //rechterbicep
linde101 2:8ed9953fc4ab 161 emgRBraw= emgRB.read();
linde101 2:8ed9953fc4ab 162 emgRBHP= RightBicepHP.step(emgRBraw);
linde101 2:8ed9953fc4ab 163 emgRBN=RightBicepN.step(emgRBHP);
linde101 2:8ed9953fc4ab 164 //emgmeansubRB = emgRBN - RestmeanRB;
linde101 2:8ed9953fc4ab 165 emgRBA= fabs(emgRBN);
linde101 2:8ed9953fc4ab 166 emgRBfiltered=RightBicepLP.step(emgRBA);
linde101 2:8ed9953fc4ab 167 //RBvalue = emgRBfiltered/emgRB_max;
linde101 2:8ed9953fc4ab 168
linde101 2:8ed9953fc4ab 169 //Right Quadriceps
linde101 2:8ed9953fc4ab 170 emgRTraw= emgRT.read();
linde101 2:8ed9953fc4ab 171 emgRTHP= RightLegHP.step(emgRTraw);
linde101 2:8ed9953fc4ab 172 emgRTN= RightLegN.step(emgRTHP);
linde101 2:8ed9953fc4ab 173 // emgmeansubRT = emgRTHP - RestmeanRT;
linde101 2:8ed9953fc4ab 174 emgRTA= fabs(emgRTN);
linde101 2:8ed9953fc4ab 175 emgRTfiltered=RightLegLP.step(emgRTA);
linde101 2:8ed9953fc4ab 176 //RTvalue = emgRTfiltered/emgRT_max;
linde101 2:8ed9953fc4ab 177 scope.send();
linde101 2:8ed9953fc4ab 178
linde101 2:8ed9953fc4ab 179 }
linde101 2:8ed9953fc4ab 180
linde101 2:8ed9953fc4ab 181 void calibration()
linde101 2:8ed9953fc4ab 182 {
linde101 2:8ed9953fc4ab 183
linde101 2:8ed9953fc4ab 184 emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
linde101 2:8ed9953fc4ab 185 emgLBHP=LeftBicepHP.step(emgLBraw);
linde101 2:8ed9953fc4ab 186 emgLBN=LeftBicepN.step(emgLBHP);
linde101 2:8ed9953fc4ab 187 emgLBA= fabs(emgLBN);
linde101 2:8ed9953fc4ab 188 emgLBfiltered=LeftBicepLP.step(emgLBA);
linde101 2:8ed9953fc4ab 189
linde101 2:8ed9953fc4ab 190 emgRBraw= emgRB.read();
linde101 2:8ed9953fc4ab 191 emgRBHP= RightBicepHP.step(emgRBraw);
linde101 2:8ed9953fc4ab 192 emgRBN=RightBicepN.step(emgRBHP);
linde101 2:8ed9953fc4ab 193 emgRBA= fabs(emgRBN);
linde101 2:8ed9953fc4ab 194 emgRBfiltered=RightBicepLP.step(emgRBA);
linde101 2:8ed9953fc4ab 195
linde101 2:8ed9953fc4ab 196
linde101 2:8ed9953fc4ab 197 emgRTraw= emgRT.read();
linde101 2:8ed9953fc4ab 198 emgRTHP= RightLegHP.step(emgRTraw);
linde101 2:8ed9953fc4ab 199 emgRTN= RightLegN.step(emgRTHP);
linde101 2:8ed9953fc4ab 200 emgRTA= fabs(emgRTN);
linde101 2:8ed9953fc4ab 201 emgRTfiltered=RightLegLP.step(emgRTA);
linde101 2:8ed9953fc4ab 202
linde101 2:8ed9953fc4ab 203 if (tijd > 1000 && tijd < 6000) {
linde101 2:8ed9953fc4ab 204 emgLBraw= emgLB.read();
linde101 2:8ed9953fc4ab 205 emgLBHP=LeftBicepHP.step(emgLBraw);
linde101 2:8ed9953fc4ab 206 emgLBN=LeftBicepN.step(emgLBHP);
linde101 2:8ed9953fc4ab 207 //emgmeansubLB = emgLBN - restmeanLB;
linde101 2:8ed9953fc4ab 208 emgLBA= fabs(emgLBN);
linde101 2:8ed9953fc4ab 209 emgLBfiltered=LeftBicepLP.step(emgLBA);
linde101 2:8ed9953fc4ab 210 if (emgLBfiltered > emgLB_max) {
linde101 2:8ed9953fc4ab 211 emgLB_max = emgLBfiltered;
linde101 2:8ed9953fc4ab 212 }
linde101 2:8ed9953fc4ab 213 //pc.baud(115200);
linde101 2:8ed9953fc4ab 214 //pc.printf("emgLB_max = %f \r\n", emgLB_max);// geen tekst printen in ticker want dat gaat mis xjes
linde101 2:8ed9953fc4ab 215 led5=0; //led gaat aan zodra je linkerbicep moet aanspannen
linde101 2:8ed9953fc4ab 216 }
linde101 2:8ed9953fc4ab 217
linde101 2:8ed9953fc4ab 218 else if (tijd > 7000 && tijd < 12000) {
linde101 2:8ed9953fc4ab 219 emgRBraw= emgRB.read();
linde101 2:8ed9953fc4ab 220 emgRBHP= RightBicepHP.step(emgRBraw);
linde101 2:8ed9953fc4ab 221 emgRBN=RightBicepN.step(emgRBHP);
linde101 2:8ed9953fc4ab 222 //emgmeansubRB = emgRBN - RestmeanRB;
linde101 2:8ed9953fc4ab 223 emgRBA= fabs(emgRBN);
linde101 2:8ed9953fc4ab 224 emgRBfiltered=RightBicepLP.step(emgRBA);
linde101 2:8ed9953fc4ab 225 if (emgRBfiltered > emgRB_max) {
linde101 2:8ed9953fc4ab 226 emgRB_max = emgRBfiltered;
linde101 2:8ed9953fc4ab 227 }
linde101 2:8ed9953fc4ab 228 //pc.printf("emgRB_max = %f \r\n", emgRB_max);
linde101 2:8ed9953fc4ab 229 led5=0; //led gaat uit zodra je rechterbicep moet aanspannen
linde101 2:8ed9953fc4ab 230 }
linde101 2:8ed9953fc4ab 231
linde101 2:8ed9953fc4ab 232
linde101 2:8ed9953fc4ab 233 else if (tijd > 13000 && tijd < 18000) {
linde101 2:8ed9953fc4ab 234 emgRTraw= emgRT.read();
linde101 2:8ed9953fc4ab 235 emgRTHP= RightLegHP.step(emgRTraw);
linde101 2:8ed9953fc4ab 236 emgRTN= RightLegN.step(emgRTHP);
linde101 2:8ed9953fc4ab 237 //emgmeansubRT = emgRTHP - RestmeanRT;
linde101 2:8ed9953fc4ab 238 emgRTA= fabs(emgRTN);
linde101 2:8ed9953fc4ab 239 emgRTfiltered=RightLegLP.step(emgRTA);
linde101 2:8ed9953fc4ab 240 if (emgRTfiltered > emgRT_max) {
linde101 2:8ed9953fc4ab 241 emgRT_max = emgRTfiltered;
linde101 2:8ed9953fc4ab 242 }
linde101 2:8ed9953fc4ab 243 //pc.printf("emgRT_max = %f \r\n", emgRT_max);
linde101 5:a53081a119c0 244 led5=0; //led gaat aan zodra je rechtertricep moet aanspannen
linde101 2:8ed9953fc4ab 245 } else {
linde101 2:8ed9953fc4ab 246 led5=1; //led gaat uit zodra kalibratie voltooid
linde101 2:8ed9953fc4ab 247 }
linde101 2:8ed9953fc4ab 248 tijd= tijd + 1; //idealiter op een andere plek haha
linde101 2:8ed9953fc4ab 249
linde101 5:a53081a119c0 250 threshold_emgLB = threshold*emgLB_max;
linde101 2:8ed9953fc4ab 251 threshold_emgRB = threshold*emgRB_max;
linde101 2:8ed9953fc4ab 252 threshold_emgRT = threshold*emgRT_max;
linde101 2:8ed9953fc4ab 253
linde101 2:8ed9953fc4ab 254 // if threshold_emgx is reached, brushingmodes activated
linde101 3:9b8d3180fe48 255 }
linde101 2:8ed9953fc4ab 256
linde101 3:9b8d3180fe48 257 void brushingmode()
linde101 3:9b8d3180fe48 258 {
linde101 2:8ed9953fc4ab 259
linde101 2:8ed9953fc4ab 260 //boven draait arm 1 aan, dus motor 1
linde101 2:8ed9953fc4ab 261 //onder draait arm 2 aan, dus motor 2
linde101 3:9b8d3180fe48 262 if (emgLBfiltered > threshold_emgLB){ //tandenborstel naar links (cw/ccw) //direction1/2 = cw/ccw
linde101 2:8ed9953fc4ab 263 motor2_direction.write(direction1); //motor 2 gaat cw
linde101 2:8ed9953fc4ab 264 //motor2_direction.write(direction1 = !direction1); //is counterclockwise
linde101 3:9b8d3180fe48 265 motor2_pwm.write(maxPWM2);
linde101 2:8ed9953fc4ab 266
linde101 3:9b8d3180fe48 267 }else if (emgRBfiltered > threshold_emgRB){
linde101 5:a53081a119c0 268 motor2_direction.write(direction1 = !direction1); //is counterclockwise
linde101 5:a53081a119c0 269 motor2_pwm.write(maxPWM2);
linde101 2:8ed9953fc4ab 270 //tandenborstel naar rechts
linde101 2:8ed9953fc4ab 271
linde101 3:9b8d3180fe48 272 }else if (emgRTfiltered > threshold_emgRT){
linde101 3:9b8d3180fe48 273 motor1_direction.write(direction1 = !direction1);
linde101 3:9b8d3180fe48 274 motor1_pwm.write(maxPWM1);
linde101 2:8ed9953fc4ab 275 //tandenborstel naar achter
linde101 2:8ed9953fc4ab 276
linde101 3:9b8d3180fe48 277 }else if (emgRBfiltered > threshold_emgRB && emgLBfiltered > threshold_emgLB){
linde101 3:9b8d3180fe48 278 motor1_direction.write(direction1); //motor 1 gaat cw
linde101 3:9b8d3180fe48 279 motor1_pwm.write(maxPWM1);
linde101 2:8ed9953fc4ab 280 //tandenborstel naar voren
linde101 3:9b8d3180fe48 281
linde101 3:9b8d3180fe48 282 }
linde101 3:9b8d3180fe48 283 }
linde101 2:8ed9953fc4ab 284 int main()
linde101 2:8ed9953fc4ab 285 {
linde101 2:8ed9953fc4ab 286 // -------------------- Serial Comms --------------------------------
linde101 2:8ed9953fc4ab 287 pc.baud(115200);
linde101 2:8ed9953fc4ab 288 pc.printf("Hello");
linde101 2:8ed9953fc4ab 289
linde101 2:8ed9953fc4ab 290 // -------------------- BiQuad Chains -------------------------------
linde101 2:8ed9953fc4ab 291 LeftBicepHP.add( &LBHP1 ); //BiQuadChain bqc; //bqc.add( &bq1 ).add( &bq2 );
linde101 2:8ed9953fc4ab 292 LeftBicepN.add( &LBN1 );
linde101 2:8ed9953fc4ab 293 LeftBicepLP.add( &LBLP1 );
linde101 2:8ed9953fc4ab 294
linde101 2:8ed9953fc4ab 295
linde101 2:8ed9953fc4ab 296 RightBicepHP.add( &RBHP1 );
linde101 2:8ed9953fc4ab 297 RightBicepN.add( &RBN1 );
linde101 2:8ed9953fc4ab 298 RightBicepLP.add( &RBLP1 );
linde101 2:8ed9953fc4ab 299
linde101 2:8ed9953fc4ab 300 RightLegHP.add( &RTHP1 );
linde101 2:8ed9953fc4ab 301 RightLegN.add( &RTN1 );
linde101 2:8ed9953fc4ab 302 RightLegLP.add( &RTLP1 );
linde101 2:8ed9953fc4ab 303
linde101 2:8ed9953fc4ab 304 // -------------------- Tickers -------------------------------------
linde101 2:8ed9953fc4ab 305
linde101 2:8ed9953fc4ab 306 calibreren.attach(calibration, 0.001f);
linde101 2:8ed9953fc4ab 307 filter.attach(Filteren, 0.001f); //ticker aanroepen van filter
linde101 3:9b8d3180fe48 308 actie.attach(brushingmode, 0.001f);
linde101 2:8ed9953fc4ab 309
linde101 2:8ed9953fc4ab 310 }
linde101 2:8ed9953fc4ab 311
linde101 2:8ed9953fc4ab 312
linde101 2:8ed9953fc4ab 313