Biorobotics_group_2 / Mbed 2 deprecated CompleteFunction

Alle subfiles samengevoegd

Dependencies:   HIDScope biquadFilter mbed MODSERIAL FastPWM QEI

main.cpp@1:e1267e72ade8, 2016-10-20 (annotated)

Committer:
s1600907
Date:
Thu Oct 20 14:28:58 2016 +0000
Revision:
1:e1267e72ade8
Parent:
0:c96cf9760185
Child:
2:92593c9d6146
EMG en theta berekenen samengevoegd en werkend op 0.002 s. (staat aan voor een HIDScope voor de rechter arm en x beweging)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1600907 0:c96cf9760185 1 #include "mbed.h"
s1600907 0:c96cf9760185 2 #include "HIDScope.h"
s1600907 0:c96cf9760185 3 #include "BiQuad.h"
s1600907 0:c96cf9760185 4 #include "math.h"
s1600907 1:e1267e72ade8 5 #include "MODSERIAL.h"
s1600907 0:c96cf9760185 6 #define SERIAL_BAUD 115200
s1600907 0:c96cf9760185 7
s1600907 1:e1267e72ade8 8 MODSERIAL pc(USBTX,USBRX);
s1600907 0:c96cf9760185 9 //EMG aansluitingen
s1600907 0:c96cf9760185 10 AnalogIn emg0( A0 ); //Biceps Rechts
s1600907 1:e1267e72ade8 11 //AnalogIn emg1( A1 ); //Bicpes Links
s1600907 1:e1267e72ade8 12 //AnalogIn emg2( A2 ); //Upper leg
s1600907 0:c96cf9760185 13 //HIDScope
s1600907 1:e1267e72ade8 14 HIDScope scope( 4 ); // aantal channels op de HIDscope
s1600907 0:c96cf9760185 15 //Button om Calibratie te beëindigen
s1600907 0:c96cf9760185 16 InterruptIn Button1(PTC6); // DIT WORDT EEN ANDERE BUTTON
s1600907 0:c96cf9760185 17
s1600907 0:c96cf9760185 18 // Tickers
s1600907 0:c96cf9760185 19 Ticker sample_timer;
s1600907 0:c96cf9760185 20 Ticker TickerCalculationsForTheta;
s1600907 1:e1267e72ade8 21 Ticker TickerSendScope;
s1600907 0:c96cf9760185 22
s1600907 0:c96cf9760185 23 //BiQuad
s1600907 0:c96cf9760185 24 // making the biquads and chains; imported from matlab
s1600907 0:c96cf9760185 25 BiQuadChain bqc; //Chain for right biceps
s1600907 0:c96cf9760185 26 BiQuadChain bqc2; //Chain for left biceps
s1600907 0:c96cf9760185 27 BiQuadChain bqc3; //Chain for Upper leg
s1600907 0:c96cf9760185 28 // 1 to 3 are for right biceps
s1600907 0:c96cf9760185 29 BiQuad bq1( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 30 BiQuad bq2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 31 BiQuad bq3( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
s1600907 0:c96cf9760185 32 // 4 to 6 are for left biceps
s1600907 0:c96cf9760185 33 BiQuad bq4( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 34 BiQuad bq5( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 35 BiQuad bq6( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
s1600907 0:c96cf9760185 36 // 7 to 9 are for upper leg
s1600907 0:c96cf9760185 37 BiQuad bq7( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 38 BiQuad bq8( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 39 BiQuad bq9( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
s1600907 0:c96cf9760185 40
s1600907 1:e1267e72ade8 41
s1600907 0:c96cf9760185 42 // Variabelen voor filter
s1600907 0:c96cf9760185 43 // spier EMG niet boven treshhold
s1600907 0:c96cf9760185 44 volatile bool BicepsLeft = false;
s1600907 0:c96cf9760185 45 volatile bool BicepsRight = false;
s1600907 0:c96cf9760185 46 volatile bool Leg = false;
s1600907 0:c96cf9760185 47 // filtergo uit zetten, zodat deze niet fout gaat.
s1600907 1:e1267e72ade8 48 //volatile bool filtergo = false;
s1600907 0:c96cf9760185 49
s1600907 0:c96cf9760185 50 // Variabelen voor MotorHoekBerekenen
s1600907 1:e1267e72ade8 51 volatile double x = 0.0; // beginpositie x en y
s1600907 1:e1267e72ade8 52 volatile double y = 305.5;
s1600907 0:c96cf9760185 53 volatile const double pi = 3.14159265359;
s1600907 1:e1267e72ade8 54 volatile double Theta1Gear = 1.0/3.0*pi; // Beginpositie op 60 graden
s1600907 1:e1267e72ade8 55 volatile double Theta2Gear = 1.0/3.0*pi;
s1600907 1:e1267e72ade8 56 volatile double Theta1 = Theta1Gear*42/12; // Beginpositie van MotorHoek
s1600907 1:e1267e72ade8 57 volatile double Theta2 = Theta2Gear*42/12;
s1600907 0:c96cf9760185 58 double Fr_Speed_Theta = 100.0; // frequentie in Hz waarmee theta wordt uigerekend
s1600907 0:c96cf9760185 59 bool Calibration = true; // beginnen met calibreren
s1600907 0:c96cf9760185 60 volatile bool Stepper_State = false; // false = we zijn niet bezig met flippen
s1600907 0:c96cf9760185 61
s1600907 0:c96cf9760185 62 /////////////////////// functies voor filters /////////////////////////////////////////////
s1600907 0:c96cf9760185 63 void sample()
s1600907 0:c96cf9760185 64 {
s1600907 0:c96cf9760185 65 /* Sample function
s1600907 0:c96cf9760185 66 samples the emg0 (of the rightBiceps)
s1600907 0:c96cf9760185 67 samples the emg1 (of the leftBiceps)
s1600907 0:c96cf9760185 68 samples the emg2 (of the Upper leg)
s1600907 0:c96cf9760185 69 filter the singals
s1600907 0:c96cf9760185 70 sends it to HIDScope
s1600907 0:c96cf9760185 71 */
s1600907 0:c96cf9760185 72
s1600907 0:c96cf9760185 73 // Rechts Biceps
s1600907 0:c96cf9760185 74 double inRechts = emg0.read(); // EMG signal
s1600907 0:c96cf9760185 75 double FilterRechts = bqc.step(inRechts); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 76 double RectifyRechts = fabs(FilterRechts); // Rectify
s1600907 0:c96cf9760185 77 double outRechts = bq3.step(RectifyRechts); // Low pass
s1600907 1:e1267e72ade8 78
s1600907 1:e1267e72ade8 79 /*
s1600907 0:c96cf9760185 80 // Links Biceps
s1600907 0:c96cf9760185 81 double inLinks = emg1.read(); // EMG signal
s1600907 0:c96cf9760185 82 double FilterLinks = bqc2.step(inLinks); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 83 double RectifyLinks = fabs(FilterLinks); // Rectify
s1600907 0:c96cf9760185 84 double outLinks = bq6.step(RectifyLinks); // Low pass
s1600907 0:c96cf9760185 85
s1600907 0:c96cf9760185 86 // Upper leg
s1600907 0:c96cf9760185 87 double inBeen = emg2.read(); // EMG signal
s1600907 0:c96cf9760185 88 double FilterBeen = bqc3.step(inBeen); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 89 double RectifyBeen = fabs(FilterBeen); // Rectify
s1600907 0:c96cf9760185 90 double outBeen = bq9.step(RectifyBeen); // Low pass
s1600907 1:e1267e72ade8 91 */
s1600907 0:c96cf9760185 92
s1600907 0:c96cf9760185 93
s1600907 0:c96cf9760185 94 /* EMG signal in channel 0 (the first channel)
s1600907 0:c96cf9760185 95 and the filtered EMG signal in channel 1 (the second channel)
s1600907 0:c96cf9760185 96 of the HIDscope */
s1600907 0:c96cf9760185 97 scope.set(0,inRechts);
s1600907 0:c96cf9760185 98 scope.set(1,outRechts);
s1600907 1:e1267e72ade8 99 /*scope.set(2,inLinks);
s1600907 0:c96cf9760185 100 scope.set(3,outLinks);
s1600907 0:c96cf9760185 101 scope.set(4,inBeen);
s1600907 0:c96cf9760185 102 scope.set(5,outBeen);
s1600907 1:e1267e72ade8 103 */
s1600907 0:c96cf9760185 104 // send all channels to the PC at once
s1600907 1:e1267e72ade8 105 //scope.send();
s1600907 0:c96cf9760185 106
s1600907 0:c96cf9760185 107 // To indicate that the function is working, the LED is on
s1600907 1:e1267e72ade8 108 if (outRechts >= 0.04){
s1600907 0:c96cf9760185 109 BicepsRight = true;
s1600907 0:c96cf9760185 110 }
s1600907 0:c96cf9760185 111 else{
s1600907 0:c96cf9760185 112 BicepsRight = false;
s1600907 0:c96cf9760185 113 }
s1600907 1:e1267e72ade8 114 /*
s1600907 0:c96cf9760185 115 // If Biceps links is actuated then:
s1600907 1:e1267e72ade8 116 if (outLinks >= 0.04){
s1600907 0:c96cf9760185 117 BicepsLeft = true;
s1600907 0:c96cf9760185 118 }
s1600907 0:c96cf9760185 119 else{
s1600907 0:c96cf9760185 120 BicepsLeft = false;
s1600907 0:c96cf9760185 121 }
s1600907 0:c96cf9760185 122 // If upper leg is actuated then:
s1600907 0:c96cf9760185 123 if (outBeen >= 0.01){
s1600907 0:c96cf9760185 124 Leg = true;
s1600907 0:c96cf9760185 125 }
s1600907 0:c96cf9760185 126 else{
s1600907 0:c96cf9760185 127 Leg = false;
s1600907 0:c96cf9760185 128 }
s1600907 1:e1267e72ade8 129 */
s1600907 1:e1267e72ade8 130 //filtergo = false;
s1600907 0:c96cf9760185 131 }
s1600907 0:c96cf9760185 132
s1600907 0:c96cf9760185 133 // zet de filter aan
s1600907 0:c96cf9760185 134 void filter()
s1600907 0:c96cf9760185 135 {
s1600907 1:e1267e72ade8 136 //filtergo=true;
s1600907 0:c96cf9760185 137 }
s1600907 0:c96cf9760185 138
s1600907 0:c96cf9760185 139 ///////////////////// functies voor motorhoekberekenen //////////////////////////
s1600907 0:c96cf9760185 140 // vorige x berekenen
s1600907 0:c96cf9760185 141 double Calc_Prev_x () {
s1600907 0:c96cf9760185 142 double Prev_x = x;
s1600907 0:c96cf9760185 143 //pc.printf("prev x = %f\r\n", Prev_x);
s1600907 0:c96cf9760185 144 return Prev_x;
s1600907 0:c96cf9760185 145 }
s1600907 0:c96cf9760185 146
s1600907 0:c96cf9760185 147 // vorige y berekenen
s1600907 0:c96cf9760185 148 double Calc_Prev_y () {
s1600907 0:c96cf9760185 149 double Prev_y = y;
s1600907 0:c96cf9760185 150 //pc.printf("prev y = %f\r\n", Prev_y);
s1600907 0:c96cf9760185 151 return Prev_y;
s1600907 0:c96cf9760185 152 }
s1600907 0:c96cf9760185 153
s1600907 0:c96cf9760185 154 // berekenen van de nieuwe x en y waardes
s1600907 0:c96cf9760185 155 bool CalcXY (bool BicepsLeft, bool BicepsRight, bool Leg, bool Stepper_State) {
s1600907 1:e1267e72ade8 156 double Step = 5/Fr_Speed_Theta ; //10 mm per seconde afleggen
s1600907 0:c96cf9760185 157
s1600907 0:c96cf9760185 158 if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) {
s1600907 0:c96cf9760185 159 Stepper_State = true; // we zijn aan het flipper dus geen andere dingen doen
s1600907 0:c96cf9760185 160 // flipper functie aanroepen
s1600907 0:c96cf9760185 161 }
s1600907 0:c96cf9760185 162 else if (BicepsLeft==true && BicepsRight==false && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 163 x = x - Step;
s1600907 0:c96cf9760185 164 }
s1600907 0:c96cf9760185 165 else if (BicepsLeft==false && BicepsRight==true && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 166 x = x + Step; // naar Right bewegen
s1600907 0:c96cf9760185 167 }
s1600907 0:c96cf9760185 168 else if (BicepsLeft==true && BicepsRight==true && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 169 y = y + Step; // naar voren bewegen
s1600907 0:c96cf9760185 170 }
s1600907 0:c96cf9760185 171 else if (BicepsLeft==false && BicepsRight==true && Leg==true && Stepper_State==false) {
s1600907 0:c96cf9760185 172 y = y - Step; // naar achter bewegen
s1600907 0:c96cf9760185 173 }
s1600907 0:c96cf9760185 174 else if (BicepsLeft==false && BicepsRight==false && Leg==false || Stepper_State==true) {
s1600907 0:c96cf9760185 175 }
s1600907 0:c96cf9760185 176
s1600907 0:c96cf9760185 177 // Grenswaardes LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
s1600907 0:c96cf9760185 178 if (x > 200) {
s1600907 0:c96cf9760185 179 x = 200;
s1600907 0:c96cf9760185 180 }
s1600907 0:c96cf9760185 181 else if (x < -200) {
s1600907 0:c96cf9760185 182 x = -200;
s1600907 0:c96cf9760185 183 }
s1600907 0:c96cf9760185 184 if (y > 306) {
s1600907 0:c96cf9760185 185 y = 306;
s1600907 0:c96cf9760185 186 }
s1600907 0:c96cf9760185 187 else if (y < 50) {
s1600907 0:c96cf9760185 188 y = 50; // GOKJE, UITPROBEREN
s1600907 0:c96cf9760185 189 }
s1600907 1:e1267e72ade8 190 //pc.printf("x = %f, y = %f\r\n", x, y);
s1600907 1:e1267e72ade8 191
s1600907 1:e1267e72ade8 192 scope.set(2,x);
s1600907 1:e1267e72ade8 193 scope.set(3,y);
s1600907 0:c96cf9760185 194
s1600907 0:c96cf9760185 195 return Stepper_State;
s1600907 0:c96cf9760185 196 }
s1600907 0:c96cf9760185 197
s1600907 0:c96cf9760185 198 // diagonaal berekenen voor linker arm
s1600907 0:c96cf9760185 199 double CalcDia1 (double x, double y) {
s1600907 0:c96cf9760185 200 double a = 50.0; // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden) KEERTJE NAMETEN
s1600907 0:c96cf9760185 201 double BV1 = sqrt(pow((a+x),2) + pow(y,2)); // diagonaal (afstand van armas tot locatie) berekenen
s1600907 0:c96cf9760185 202 double Dia1 = pow(BV1,2)/(2*BV1); // berekenen van de afstand oorsprong tot diagonaal
s1600907 0:c96cf9760185 203
s1600907 0:c96cf9760185 204 //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia1, x, y);
s1600907 0:c96cf9760185 205 return Dia1;
s1600907 0:c96cf9760185 206 }
s1600907 0:c96cf9760185 207
s1600907 0:c96cf9760185 208 // diagonaal berekenen voor rechter arm
s1600907 0:c96cf9760185 209 double CalcDia2 (double x, double y) {
s1600907 0:c96cf9760185 210 double a = 50.0;
s1600907 0:c96cf9760185 211 double BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm
s1600907 0:c96cf9760185 212 double Dia2 = pow(BV2,2)/(2*BV2);
s1600907 0:c96cf9760185 213
s1600907 0:c96cf9760185 214 //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia2, x, y);
s1600907 0:c96cf9760185 215 return Dia2;
s1600907 0:c96cf9760185 216 }
s1600907 0:c96cf9760185 217
s1600907 0:c96cf9760185 218 // calculate Theta1
s1600907 0:c96cf9760185 219 void CalcTheta1 (double Dia1, double Prev_x, double Prev_y) {
s1600907 0:c96cf9760185 220 double a = 50.0;
s1600907 0:c96cf9760185 221 double Bar = 200.0; // lengte van de armen
s1600907 0:c96cf9760185 222 double Prev_Theta1_Gear = Theta1Gear;
s1600907 0:c96cf9760185 223 double MaxThetaGear = pi - 30.0*pi/180.0; // de hoek voordat arm het opstakel raakt (max hoek is 24.9 tussen arm en opstakel)
s1600907 0:c96cf9760185 224
s1600907 0:c96cf9760185 225 // Hoek berekenen van het grote tandwiel (gear)
s1600907 0:c96cf9760185 226 if (x > -a) {
s1600907 0:c96cf9760185 227 Theta1Gear = pi - atan(y/(x+a)) - acos(Dia1/Bar);
s1600907 0:c96cf9760185 228 }
s1600907 0:c96cf9760185 229 else if (x > -a) {
s1600907 0:c96cf9760185 230 Theta1Gear = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
s1600907 0:c96cf9760185 231 }
s1600907 0:c96cf9760185 232 else { // als x=-a
s1600907 0:c96cf9760185 233 Theta1Gear = 0.5*pi - acos(Dia1/Bar);
s1600907 0:c96cf9760185 234 }
s1600907 0:c96cf9760185 235
s1600907 0:c96cf9760185 236 // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
s1600907 0:c96cf9760185 237 if (Theta1Gear >= MaxThetaGear) { // is de maximale hoek in radialen
s1600907 0:c96cf9760185 238 Theta1Gear = Prev_Theta1_Gear;
s1600907 0:c96cf9760185 239 x = Prev_x;
s1600907 0:c96cf9760185 240 y = Prev_y;
s1600907 0:c96cf9760185 241 }
s1600907 0:c96cf9760185 242
s1600907 0:c96cf9760185 243 // omrekenen van grote tandwiel naar motortandwiel
s1600907 0:c96cf9760185 244 Theta1 = Theta1Gear*42.0/12.0; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
s1600907 0:c96cf9760185 245
s1600907 1:e1267e72ade8 246 //pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta1, Theta1Gear, Prev_Theta1_Gear);
s1600907 0:c96cf9760185 247 }
s1600907 0:c96cf9760185 248
s1600907 0:c96cf9760185 249 void CalcTheta2 (double Dia2, double Prev_x, double Prev_y) {
s1600907 0:c96cf9760185 250 double a = 50.0;
s1600907 0:c96cf9760185 251 double Bar = 200.0; // lengte van de armen
s1600907 0:c96cf9760185 252 double Prev_Theta2_Gear = Theta2Gear;
s1600907 0:c96cf9760185 253 double MaxThetaGear = pi - 30.0*pi/180.0; // de hoek voordat arm het opstakel raakt (max hoek is 24.9 tussen arm en opstakel)
s1600907 0:c96cf9760185 254
s1600907 0:c96cf9760185 255 // Hoek berekenen van het grote tandwiel (gear)
s1600907 0:c96cf9760185 256 if (x < a) {
s1600907 0:c96cf9760185 257 Theta2Gear = pi + atan(y/(x-a)) - acos(Dia2/Bar);
s1600907 0:c96cf9760185 258 }
s1600907 0:c96cf9760185 259 else if (x > a) {
s1600907 0:c96cf9760185 260 Theta2Gear = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
s1600907 0:c96cf9760185 261 }
s1600907 0:c96cf9760185 262 else { // als x=a
s1600907 0:c96cf9760185 263 Theta2Gear = 0.5*pi - acos(Dia2/Bar);
s1600907 0:c96cf9760185 264 }
s1600907 0:c96cf9760185 265
s1600907 0:c96cf9760185 266 // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
s1600907 0:c96cf9760185 267 if (Theta2Gear >= MaxThetaGear) {
s1600907 0:c96cf9760185 268 Theta2Gear = Prev_Theta2_Gear;
s1600907 0:c96cf9760185 269 x = Prev_x;
s1600907 0:c96cf9760185 270 y = Prev_y;
s1600907 0:c96cf9760185 271 }
s1600907 0:c96cf9760185 272
s1600907 0:c96cf9760185 273 // omrekenen van grote tandwiel naar motortandwiel
s1600907 0:c96cf9760185 274 Theta2 = Theta2Gear*42.0/12.0; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
s1600907 1:e1267e72ade8 275
s1600907 1:e1267e72ade8 276 //pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta2, Theta2Gear, Prev_Theta2_Gear);
s1600907 0:c96cf9760185 277 }
s1600907 0:c96cf9760185 278
s1600907 0:c96cf9760185 279 void CalculationsForTheta () {
s1600907 1:e1267e72ade8 280 sample();
s1600907 0:c96cf9760185 281 double Prev_x = Calc_Prev_x ();
s1600907 0:c96cf9760185 282 double Prev_y = Calc_Prev_y ();
s1600907 0:c96cf9760185 283 bool Stepper_State = CalcXY (BicepsLeft, BicepsRight, Leg, Stepper_State);
s1600907 0:c96cf9760185 284 double Dia1 = CalcDia1 (x, y);
s1600907 0:c96cf9760185 285 double Dia2 = CalcDia2 (x, y);
s1600907 0:c96cf9760185 286 CalcTheta1 (Dia1, Prev_x, Prev_y);
s1600907 1:e1267e72ade8 287 CalcTheta2 (Dia2, Prev_x, Prev_y);
s1600907 1:e1267e72ade8 288
s1600907 1:e1267e72ade8 289 scope.send();
s1600907 0:c96cf9760185 290 }
s1600907 0:c96cf9760185 291
s1600907 0:c96cf9760185 292 // als de button ingedrukt wordt dan stoppen we met calibreren
s1600907 0:c96cf9760185 293 void EndCalibration () {
s1600907 0:c96cf9760185 294 Calibration = false;
s1600907 0:c96cf9760185 295 }
s1600907 0:c96cf9760185 296
s1600907 1:e1267e72ade8 297 void SendScope () {
s1600907 1:e1267e72ade8 298 scope.send();
s1600907 1:e1267e72ade8 299 }
s1600907 0:c96cf9760185 300 //////////////// functies voor aansturing motoren ///////////////////////////////
s1600907 0:c96cf9760185 301
s1600907 0:c96cf9760185 302
s1600907 0:c96cf9760185 303
s1600907 0:c96cf9760185 304 ////////////////////////// main loop ////////////////////////////////////////////
s1600907 0:c96cf9760185 305 int main()
s1600907 0:c96cf9760185 306 {
s1600907 0:c96cf9760185 307 pc.baud(SERIAL_BAUD);
s1600907 0:c96cf9760185 308 pc.printf("\r\n Nieuwe code uitproberen :) \r\n");
s1600907 0:c96cf9760185 309
s1600907 0:c96cf9760185 310 /* making the biquad chain for filtering the emg
s1600907 0:c96cf9760185 311 notch and high pass */
s1600907 0:c96cf9760185 312 bqc.add( &bq1 ).add( &bq2 );
s1600907 0:c96cf9760185 313 bqc2.add( &bq4 ).add( &bq5 );
s1600907 0:c96cf9760185 314 bqc3.add( &bq7 ).add( &bq8 );
s1600907 0:c96cf9760185 315
s1600907 0:c96cf9760185 316 // Calibreren
s1600907 0:c96cf9760185 317 while (Calibration == true) {
s1600907 1:e1267e72ade8 318 //potmeter dingen aanpassen
s1600907 0:c96cf9760185 319
s1600907 0:c96cf9760185 320 pc.printf("calibreren is bezig\r\n");
s1600907 0:c96cf9760185 321 Button1.fall(&EndCalibration);
s1600907 0:c96cf9760185 322 }
s1600907 0:c96cf9760185 323
s1600907 0:c96cf9760185 324 /**Attach the 'sample' function to the timer 'sample_timer'.
s1600907 0:c96cf9760185 325 * this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
s1600907 0:c96cf9760185 326 */
s1600907 1:e1267e72ade8 327 // sample_timer.attach(&sample, 0.002);
s1600907 0:c96cf9760185 328
s1600907 0:c96cf9760185 329 // extra stap, zodat er geen op onthoudt komt.
s1600907 1:e1267e72ade8 330 /*
s1600907 0:c96cf9760185 331 while (filtergo == true)
s1600907 0:c96cf9760185 332 {
s1600907 0:c96cf9760185 333 sample();
s1600907 0:c96cf9760185 334 }
s1600907 1:e1267e72ade8 335 */
s1600907 0:c96cf9760185 336 // ticker voor hoek berekenen aanzetten
s1600907 1:e1267e72ade8 337 TickerCalculationsForTheta.attach(&CalculationsForTheta,0.002);
s1600907 1:e1267e72ade8 338 //TickerSendScope.attach(&SendScope, 0.01);
s1600907 0:c96cf9760185 339 //empty loop, sample() is executed periodically
s1600907 1:e1267e72ade8 340 while(1) {
s1600907 1:e1267e72ade8 341
s1600907 1:e1267e72ade8 342 }
s1600907 0:c96cf9760185 343 }