Jitse Giesen / Mbed 2 deprecated master

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   biquadFilter mbed MODSERIAL

main.cpp@19:aa1ed300be11, 2017-11-07 (annotated)

Committer:
Jitse_Giesen
Date:
Tue Nov 07 13:38:27 2017 +0000
Revision:
19:aa1ed300be11
Parent:
18:7fb73aa6dbc0 
Final version for verslag

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jitse_Giesen 18:7fb73aa6dbc0 1 /* ___________ ___
Jitse_Giesen 19:aa1ed300be11 2 / | PG | \ / o\
Jitse_Giesen 19:aa1ed300be11 3 /___|_15_|____\_/ _ >
Jitse_Giesen 18:7fb73aa6dbc0 4 / / | \ _____/
Jitse_Giesen 19:aa1ed300be11 5 \_ / ___|__ \ __/
Jitse_Giesen 18:7fb73aa6dbc0 6 / / \ \
Jitse_Giesen 18:7fb73aa6dbc0 7 /__/ \__\ */
Jitse_Giesen 17:358e7e1213cf 8
Jitse_Giesen 0:51a6e38a4d4a 9 #include "mbed.h"
Jitse_Giesen 0:51a6e38a4d4a 10 #include "math.h"
Jitse_Giesen 0:51a6e38a4d4a 11 #include "QEI.h"
Jitse_Giesen 0:51a6e38a4d4a 12 #include "BiQuad.h"
Jitse_Giesen 4:9e18e041f7a0 13 #include "MODSERIAL.h"
Jitse_Giesen 15:3a09783b2406 14
Jitse_Giesen 4:9e18e041f7a0 15 MODSERIAL pc(USBTX, USBRX);
Jitse_Giesen 15:3a09783b2406 16
Jitse_Giesen 0:51a6e38a4d4a 17 //Defining all in- and outputs
Jitse_Giesen 0:51a6e38a4d4a 18 //EMG input
Jitse_Giesen 0:51a6e38a4d4a 19 AnalogIn emgBR( A0 ); //Right Biceps
Jitse_Giesen 0:51a6e38a4d4a 20 AnalogIn emgBL( A1 ); //Left Biceps
Jitse_Giesen 15:3a09783b2406 21
Jitse_Giesen 0:51a6e38a4d4a 22 //Output motor 1 and reading Encoder motor 1
Jitse_Giesen 0:51a6e38a4d4a 23 DigitalOut motor1DirectionPin(D4);
Jitse_Giesen 0:51a6e38a4d4a 24 PwmOut motor1MagnitudePin(D5);
Jitse_Giesen 0:51a6e38a4d4a 25 QEI Encoder1(D12,D13,NC,32);
Jitse_Giesen 15:3a09783b2406 26
Jitse_Giesen 0:51a6e38a4d4a 27 //Output motor 2 and reading Encoder motor 2
Jitse_Giesen 0:51a6e38a4d4a 28 DigitalOut motor2DirectionPin(D7);
Jitse_Giesen 0:51a6e38a4d4a 29 PwmOut motor2MagnitudePin(D6);
Jitse_Giesen 0:51a6e38a4d4a 30 QEI Encoder2(D10,D11,NC,32);
Jitse_Giesen 0:51a6e38a4d4a 31
Jitse_Giesen 4:9e18e041f7a0 32 //Output motor 3 and reading Encoder motor 3
Jitse_Giesen 4:9e18e041f7a0 33 DigitalOut motor3DirectionPin(D8);
Jitse_Giesen 4:9e18e041f7a0 34 PwmOut motor3MagnitudePin(D9);
Jitse_Giesen 4:9e18e041f7a0 35 QEI Encoder3(D2,D3,NC,32);
Jitse_Giesen 15:3a09783b2406 36
Jitse_Giesen 0:51a6e38a4d4a 37 //LED output, needed for feedback
Jitse_Giesen 0:51a6e38a4d4a 38 DigitalOut led_R(LED_RED);
Jitse_Giesen 0:51a6e38a4d4a 39 DigitalOut led_G(LED_GREEN);
Jitse_Giesen 0:51a6e38a4d4a 40 DigitalOut led_B(LED_BLUE);
Jitse_Giesen 15:3a09783b2406 41
Jitse_Giesen 0:51a6e38a4d4a 42 //Setting Tickers for sampling EMG and determing if the threshold is met
Jitse_Giesen 4:9e18e041f7a0 43 Ticker sample_timer;
Jitse_Giesen 4:9e18e041f7a0 44 Ticker threshold_timerR;
Jitse_Giesen 4:9e18e041f7a0 45 Ticker threshold_timerL;
Jitse_Giesen 15:3a09783b2406 46
Jitse_Giesen 15:3a09783b2406 47 //Timer needed to determine the threshold for a pre-set time period
Jitse_Giesen 4:9e18e041f7a0 48 Timer t_thresholdR;
Jitse_Giesen 4:9e18e041f7a0 49 Timer t_thresholdL;
Jitse_Giesen 15:3a09783b2406 50
Jitse_Giesen 15:3a09783b2406 51 //Variables to store the current time in
Jitse_Giesen 15:3a09783b2406 52 float currentTimeTR;
Jitse_Giesen 15:3a09783b2406 53 float currentTimeTL;
Jitse_Giesen 15:3a09783b2406 54
Jitse_Giesen 15:3a09783b2406 55 //This is used when testing without EMG
Jitse_Giesen 19:aa1ed300be11 56 InterruptIn button(SW2);
Jitse_Giesen 4:9e18e041f7a0 57 InterruptIn button2(SW3);
Jitse_Giesen 15:3a09783b2406 58
Jitse_Giesen 15:3a09783b2406 59 //Timer needed for timing EMG input for the X and Y coördinates
Jitse_Giesen 0:51a6e38a4d4a 60 Timer t;
Jitse_Giesen 0:51a6e38a4d4a 61
Jitse_Giesen 15:3a09783b2406 62 // Boolean needed to know if new input coordinates have to be given
Jitse_Giesen 4:9e18e041f7a0 63 bool Move_done = false;
Jitse_Giesen 4:9e18e041f7a0 64 bool Input_done = true;
Jitse_Giesen 15:3a09783b2406 65
Jitse_Giesen 0:51a6e38a4d4a 66 /* Defining all the different BiQuad filters, which contain a Notch filter,
Jitse_Giesen 0:51a6e38a4d4a 67 High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
Jitse_Giesen 0:51a6e38a4d4a 68 between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
Jitse_Giesen 2:bf1c9d7afabd 69 and the Low-pass filter cancels out all frequencies below 4 Hz. The filters are
Jitse_Giesen 2:bf1c9d7afabd 70 declared four times, so that they can be used for sampling of right and left
Jitse_Giesen 2:bf1c9d7afabd 71 biceps, during measurements and calibration. */
Jitse_Giesen 15:3a09783b2406 72
Jitse_Giesen 0:51a6e38a4d4a 73 /* Defining all the normalized values of b and a in the Notch filter for the
Jitse_Giesen 0:51a6e38a4d4a 74 creation of the Notch BiQuad */
Jitse_Giesen 15:3a09783b2406 75
Jitse_Giesen 4:9e18e041f7a0 76 BiQuad bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Jitse_Giesen 4:9e18e041f7a0 77 BiQuad bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Jitse_Giesen 15:3a09783b2406 78
Jitse_Giesen 4:9e18e041f7a0 79 BiQuad bqNotchTR( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Jitse_Giesen 4:9e18e041f7a0 80 BiQuad bqNotchTL( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Jitse_Giesen 15:3a09783b2406 81
Jitse_Giesen 2:bf1c9d7afabd 82 /* Defining all the normalized values of b and a in the High-pass filter for the
Jitse_Giesen 2:bf1c9d7afabd 83 creation of the High-pass BiQuad */
Jitse_Giesen 15:3a09783b2406 84
Jitse_Giesen 4:9e18e041f7a0 85 BiQuad bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Jitse_Giesen 4:9e18e041f7a0 86 BiQuad bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Jitse_Giesen 15:3a09783b2406 87
Jitse_Giesen 4:9e18e041f7a0 88 BiQuad bqHighTR( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Jitse_Giesen 4:9e18e041f7a0 89 BiQuad bqHighTL( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Jitse_Giesen 15:3a09783b2406 90
Jitse_Giesen 2:bf1c9d7afabd 91 /* Defining all the normalized values of b and a in the Low-pass filter for the
Jitse_Giesen 2:bf1c9d7afabd 92 creation of the Low-pass BiQuad */
Jitse_Giesen 15:3a09783b2406 93
Jitse_Giesen 4:9e18e041f7a0 94 BiQuad bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Jitse_Giesen 4:9e18e041f7a0 95 BiQuad bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Jitse_Giesen 15:3a09783b2406 96
Jitse_Giesen 4:9e18e041f7a0 97 BiQuad bqLowTR( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Jitse_Giesen 4:9e18e041f7a0 98 BiQuad bqLowTL( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Jitse_Giesen 15:3a09783b2406 99
Jitse_Giesen 2:bf1c9d7afabd 100 // Creating a variable needed for the creation of the BiQuadChain
Jitse_Giesen 4:9e18e041f7a0 101 BiQuadChain bqChain1;
Jitse_Giesen 4:9e18e041f7a0 102 BiQuadChain bqChain2;
Jitse_Giesen 15:3a09783b2406 103
Jitse_Giesen 4:9e18e041f7a0 104 BiQuadChain bqChainTR;
Jitse_Giesen 4:9e18e041f7a0 105 BiQuadChain bqChainTL;
Jitse_Giesen 15:3a09783b2406 106
Jitse_Giesen 4:9e18e041f7a0 107 //Declaring all floats needed in the filtering process
Jitse_Giesen 4:9e18e041f7a0 108 float emgBRfiltered; //Right biceps Notch+High pass filter
Jitse_Giesen 4:9e18e041f7a0 109 float emgBRrectified; //Right biceps rectified
Jitse_Giesen 4:9e18e041f7a0 110 float emgBRcomplete; //Right biceps low-pass filter, filtering complete
Jitse_Giesen 15:3a09783b2406 111
Jitse_Giesen 4:9e18e041f7a0 112 float emgBLfiltered; //Left biceps Notch+High pass filter
Jitse_Giesen 4:9e18e041f7a0 113 float emgBLrectified; //Left biceps rectified
Jitse_Giesen 4:9e18e041f7a0 114 float emgBLcomplete; //Left biceps low-pass filter, filtering complete
Jitse_Giesen 2:bf1c9d7afabd 115
Jitse_Giesen 15:3a09783b2406 116 // Declaring all variables needed for getting the Threshold value
Jitse_Giesen 4:9e18e041f7a0 117 float numsamples = 500;
Jitse_Giesen 4:9e18e041f7a0 118 float emgBRsum = 0;
Jitse_Giesen 4:9e18e041f7a0 119 float emgBRmeanMVC;
Jitse_Giesen 4:9e18e041f7a0 120 float thresholdBR;
Jitse_Giesen 15:3a09783b2406 121
Jitse_Giesen 4:9e18e041f7a0 122 float emgBLsum = 0;
Jitse_Giesen 4:9e18e041f7a0 123 float emgBLmeanMVC;
Jitse_Giesen 4:9e18e041f7a0 124 float thresholdBL;
Jitse_Giesen 15:3a09783b2406 125
Jitse_Giesen 15:3a09783b2406 126 /* Function to sample the EMG of the Right Biceps and get a Threshold value
Jitse_Giesen 4:9e18e041f7a0 127 from it, which can be used throughout the process */
Jitse_Giesen 15:3a09783b2406 128
Jitse_Giesen 15:3a09783b2406 129 void Threshold_samplingBR()
Jitse_Giesen 15:3a09783b2406 130 {
Jitse_Giesen 2:bf1c9d7afabd 131 t_thresholdR.start();
Jitse_Giesen 2:bf1c9d7afabd 132 currentTimeTR = t_thresholdR.read();
Jitse_Giesen 15:3a09783b2406 133
Jitse_Giesen 2:bf1c9d7afabd 134 if (currentTimeTR <= 1) {
Jitse_Giesen 15:3a09783b2406 135
Jitse_Giesen 17:358e7e1213cf 136 emgBRfiltered = bqChainTR.step( emgBR.read() ); //Notch+High-pass
Jitse_Giesen 17:358e7e1213cf 137 emgBRrectified = fabs(emgBRfiltered); //Rectification
Jitse_Giesen 17:358e7e1213cf 138 emgBRcomplete = bqLowTR.step(emgBRrectified); //Low-pass
Jitse_Giesen 15:3a09783b2406 139
Jitse_Giesen 2:bf1c9d7afabd 140 emgBRsum = emgBRsum + emgBRcomplete;
Jitse_Giesen 15:3a09783b2406 141 }
Jitse_Giesen 15:3a09783b2406 142 emgBRmeanMVC = emgBRsum/numsamples;
Jitse_Giesen 2:bf1c9d7afabd 143 thresholdBR = emgBRmeanMVC * 0.20;
Jitse_Giesen 10:a8a07e4ce85c 144
Jitse_Giesen 2:bf1c9d7afabd 145 }
Jitse_Giesen 15:3a09783b2406 146 /* Function to sample the EMG of the Left Biceps and get a Threshold value
Jitse_Giesen 4:9e18e041f7a0 147 from it, which can be used throughout the process */
Jitse_Giesen 4:9e18e041f7a0 148
Jitse_Giesen 15:3a09783b2406 149 void Threshold_samplingBL()
Jitse_Giesen 15:3a09783b2406 150 {
Jitse_Giesen 15:3a09783b2406 151 t_thresholdL.start();
Jitse_Giesen 2:bf1c9d7afabd 152 currentTimeTL = t_thresholdL.read();
Jitse_Giesen 15:3a09783b2406 153
Jitse_Giesen 2:bf1c9d7afabd 154 if (currentTimeTL <= 1) {
Jitse_Giesen 15:3a09783b2406 155
Jitse_Giesen 2:bf1c9d7afabd 156 emgBLfiltered = bqChain2.step( emgBL.read() ); //Notch+High-pass
Jitse_Giesen 2:bf1c9d7afabd 157 emgBLrectified = fabs( emgBLfiltered ); //Rectification
Jitse_Giesen 2:bf1c9d7afabd 158 emgBLcomplete = bqLow2.step( emgBLrectified ); //Low-pass
Jitse_Giesen 15:3a09783b2406 159
Jitse_Giesen 2:bf1c9d7afabd 160 emgBLsum = emgBLsum + emgBLcomplete;
Jitse_Giesen 15:3a09783b2406 161 }
Jitse_Giesen 15:3a09783b2406 162
Jitse_Giesen 2:bf1c9d7afabd 163 emgBLmeanMVC = emgBLsum/numsamples;
Jitse_Giesen 2:bf1c9d7afabd 164 thresholdBL = emgBLmeanMVC * 0.20;
Jitse_Giesen 19:aa1ed300be11 165
Jitse_Giesen 2:bf1c9d7afabd 166 }
Jitse_Giesen 15:3a09783b2406 167
Jitse_Giesen 4:9e18e041f7a0 168 // EMG sampling and filtering
Jitse_Giesen 2:bf1c9d7afabd 169
Jitse_Giesen 2:bf1c9d7afabd 170 void EMG_sample()
Jitse_Giesen 2:bf1c9d7afabd 171 {
Jitse_Giesen 2:bf1c9d7afabd 172 //Filtering steps for the Right Biceps EMG
Jitse_Giesen 2:bf1c9d7afabd 173 emgBRfiltered = bqChain1.step( emgBR.read() ); //Notch+High-pass
Jitse_Giesen 2:bf1c9d7afabd 174 emgBRrectified = fabs(emgBRfiltered); //Rectification
Jitse_Giesen 2:bf1c9d7afabd 175 emgBRcomplete = bqLow1.step(emgBRrectified); //Low-pass
Jitse_Giesen 15:3a09783b2406 176
Jitse_Giesen 2:bf1c9d7afabd 177 //Filtering steps for the Left Biceps EMG
Jitse_Giesen 2:bf1c9d7afabd 178 emgBLfiltered = bqChain2.step( emgBL.read() ); //Notch+High-pass
Jitse_Giesen 2:bf1c9d7afabd 179 emgBLrectified = fabs( emgBLfiltered ); //Rectification
Jitse_Giesen 2:bf1c9d7afabd 180 emgBLcomplete = bqLow2.step( emgBLrectified ); //Low-pass
Jitse_Giesen 17:358e7e1213cf 181 }
Jitse_Giesen 15:3a09783b2406 182
Jitse_Giesen 19:aa1ed300be11 183 /*Function to make the BiQuadChain for the Notch and High pass filter for all
Jitse_Giesen 19:aa1ed300be11 184 three filters*/
Jitse_Giesen 2:bf1c9d7afabd 185 void getbqChain()
Jitse_Giesen 2:bf1c9d7afabd 186 {
Jitse_Giesen 19:aa1ed300be11 187 bqChain1.add(&bqNotch1).add(&bqHigh1); //Making the BiQuadChain
Jitse_Giesen 2:bf1c9d7afabd 188 bqChain2.add(&bqNotch2).add(&bqHigh2);
Jitse_Giesen 15:3a09783b2406 189
Jitse_Giesen 2:bf1c9d7afabd 190 bqChainTR.add(&bqNotchTR).add(&bqHighTR);
Jitse_Giesen 2:bf1c9d7afabd 191 bqChainTL.add(&bqNotchTR).add(&bqHighTL);
Jitse_Giesen 2:bf1c9d7afabd 192 }
Jitse_Giesen 15:3a09783b2406 193
Jitse_Giesen 17:358e7e1213cf 194 //Initial input value for couting the X-values
Jitse_Giesen 17:358e7e1213cf 195 int Xin=0 ; //set X to zero for the first input sequence
Jitse_Giesen 15:3a09783b2406 196 int Xin_new;
Jitse_Giesen 4:9e18e041f7a0 197 float huidigetijdX;
Jitse_Giesen 15:3a09783b2406 198
Jitse_Giesen 17:358e7e1213cf 199 /*Feedback system for counting values of X:
Jitse_Giesen 19:aa1ed300be11 200 The user has 2 secondes to give input before the program jumps to the next
Jitse_Giesen 19:aa1ed300be11 201 section. If input is regesered the timer is reset so the user has 2 secondes
Jitse_Giesen 17:358e7e1213cf 202 again for the next input.*/
Jitse_Giesen 15:3a09783b2406 203 void ledtX()
Jitse_Giesen 15:3a09783b2406 204 {
Jitse_Giesen 17:358e7e1213cf 205 t.reset(); //Reset (restart) the timer
Jitse_Giesen 0:51a6e38a4d4a 206 Xin++;
Jitse_Giesen 0:51a6e38a4d4a 207 pc.printf("Xin is %i\n",Xin);
Jitse_Giesen 17:358e7e1213cf 208 led_G=0; //Feedback for user to ensure his input is regestered
Jitse_Giesen 19:aa1ed300be11 209 led_R=1;
Jitse_Giesen 16:cd11083c754a 210 wait(0.2); //time led green on
Jitse_Giesen 0:51a6e38a4d4a 211 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 212 led_R=0;
Jitse_Giesen 17:358e7e1213cf 213 wait(0.5); /*prevent multiple counts for one muscle contraction. This way
Jitse_Giesen 17:358e7e1213cf 214 only one contraction can be regestered per half second*/
Jitse_Giesen 15:3a09783b2406 215 }
Jitse_Giesen 15:3a09783b2406 216
Jitse_Giesen 0:51a6e38a4d4a 217 // Couting system for values of X
Jitse_Giesen 15:3a09783b2406 218 int tellerX()
Jitse_Giesen 15:3a09783b2406 219 {
Jitse_Giesen 0:51a6e38a4d4a 220 if (Move_done == true) {
Jitse_Giesen 0:51a6e38a4d4a 221 t.reset();
Jitse_Giesen 15:3a09783b2406 222 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 223 led_B=1;
Jitse_Giesen 0:51a6e38a4d4a 224 led_R=0;
Jitse_Giesen 15:3a09783b2406 225 while(true) {
Jitse_Giesen 15:3a09783b2406 226 //button.fall(ledtX); // this can be used for testing without EMG
Jitse_Giesen 15:3a09783b2406 227 if (emgBRcomplete > thresholdBR) {
Jitse_Giesen 15:3a09783b2406 228 ledtX();
Jitse_Giesen 0:51a6e38a4d4a 229 }
Jitse_Giesen 19:aa1ed300be11 230 t.start(); //Start timer
Jitse_Giesen 15:3a09783b2406 231 huidigetijdX=t.read();
Jitse_Giesen 17:358e7e1213cf 232 if (huidigetijdX>2) { //After 2 seconds without input
Jitse_Giesen 19:aa1ed300be11 233 led_R=1;
Jitse_Giesen 15:3a09783b2406 234 Xin_new = Xin;
Jitse_Giesen 17:358e7e1213cf 235 Xin = 0; //Reset X to zero for the next input sequence
Jitse_Giesen 15:3a09783b2406 236
Jitse_Giesen 19:aa1ed300be11 237 return Xin_new; //Go to the next program
Jitse_Giesen 0:51a6e38a4d4a 238 }
Jitse_Giesen 15:3a09783b2406 239
Jitse_Giesen 15:3a09783b2406 240 }
Jitse_Giesen 15:3a09783b2406 241
Jitse_Giesen 15:3a09783b2406 242 }
Jitse_Giesen 15:3a09783b2406 243 return 0;
Jitse_Giesen 15:3a09783b2406 244 }
Jitse_Giesen 15:3a09783b2406 245
Jitse_Giesen 15:3a09783b2406 246 // Initial values needed for Y (see comments at X function)
Jitse_Giesen 0:51a6e38a4d4a 247 int Yin=0;
Jitse_Giesen 0:51a6e38a4d4a 248 int Yin_new;
Jitse_Giesen 4:9e18e041f7a0 249 float huidigetijdY;
Jitse_Giesen 15:3a09783b2406 250
Jitse_Giesen 17:358e7e1213cf 251 /*Feedback system for counting values of Y:
Jitse_Giesen 19:aa1ed300be11 252 The user has 2 secondes to give input before the program jumps to the next
Jitse_Giesen 19:aa1ed300be11 253 section. If input is regesered the timer is reset so the user has 2 secondes
Jitse_Giesen 17:358e7e1213cf 254 again for the next input.*/
Jitse_Giesen 15:3a09783b2406 255 void ledtY()
Jitse_Giesen 15:3a09783b2406 256 {
Jitse_Giesen 17:358e7e1213cf 257 t.reset(); //Reset (restart) the timer
Jitse_Giesen 0:51a6e38a4d4a 258 Yin++;
Jitse_Giesen 0:51a6e38a4d4a 259 pc.printf("Yin is %i\n",Yin);
Jitse_Giesen 17:358e7e1213cf 260 led_G=0; //Feedback for user to ensure his input is regestered
Jitse_Giesen 0:51a6e38a4d4a 261 led_B=1;
Jitse_Giesen 17:358e7e1213cf 262 wait(0.2); //time led green on
Jitse_Giesen 0:51a6e38a4d4a 263 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 264 led_B=0;
Jitse_Giesen 17:358e7e1213cf 265 wait(0.5); /*prevent multiple counts for one muscle contraction. This way
Jitse_Giesen 17:358e7e1213cf 266 only one contraction can be regestered per half second*/
Jitse_Giesen 15:3a09783b2406 267 }
Jitse_Giesen 15:3a09783b2406 268
Jitse_Giesen 0:51a6e38a4d4a 269 // Couting system for values of Y
Jitse_Giesen 15:3a09783b2406 270 int tellerY()
Jitse_Giesen 15:3a09783b2406 271 {
Jitse_Giesen 4:9e18e041f7a0 272 if (Move_done == true) {
Jitse_Giesen 15:3a09783b2406 273 t.reset();
Jitse_Giesen 15:3a09783b2406 274 led_G=1;
Jitse_Giesen 15:3a09783b2406 275 led_B=0;
Jitse_Giesen 15:3a09783b2406 276 led_R=1;
Jitse_Giesen 15:3a09783b2406 277 while(true) {
Jitse_Giesen 15:3a09783b2406 278 //button.fall(ledtY); // this can be used for testing without EMG
Jitse_Giesen 15:3a09783b2406 279 if (emgBRcomplete > thresholdBR) {
Jitse_Giesen 15:3a09783b2406 280 ledtY();
Jitse_Giesen 15:3a09783b2406 281 }
Jitse_Giesen 17:358e7e1213cf 282 t.start(); //Start timer
Jitse_Giesen 15:3a09783b2406 283 huidigetijdY=t.read();
Jitse_Giesen 17:358e7e1213cf 284 if (huidigetijdY>2) { //After 2 seconds without input
Jitse_Giesen 15:3a09783b2406 285 led_B=1;
Jitse_Giesen 15:3a09783b2406 286 Yin_new = Yin;
Jitse_Giesen 15:3a09783b2406 287 Yin = 0;
Jitse_Giesen 17:358e7e1213cf 288 Input_done = true; //Set input done to True
Jitse_Giesen 17:358e7e1213cf 289 Move_done = false; //Next section is moving so move done is false
Jitse_Giesen 19:aa1ed300be11 290 return Yin_new; //Go to the next program
Jitse_Giesen 15:3a09783b2406 291
Jitse_Giesen 15:3a09783b2406 292 }
Jitse_Giesen 0:51a6e38a4d4a 293 }
Jitse_Giesen 0:51a6e38a4d4a 294 }
Jitse_Giesen 19:aa1ed300be11 295 return 0; //Go to the next program
Jitse_Giesen 0:51a6e38a4d4a 296 }
Jitse_Giesen 15:3a09783b2406 297
Jitse_Giesen 15:3a09783b2406 298 // Declaring all variables needed for calculating rope lengths,
Jitse_Giesen 15:3a09783b2406 299 /* The following six floats were found using our SOLIDWORKS model*/
Jitse_Giesen 4:9e18e041f7a0 300 float Pox = 0;
Jitse_Giesen 4:9e18e041f7a0 301 float Poy = 0;
Jitse_Giesen 4:9e18e041f7a0 302 float Pbx = 0;
Jitse_Giesen 4:9e18e041f7a0 303 float Pby = 887;
Jitse_Giesen 4:9e18e041f7a0 304 float Prx = 768;
Jitse_Giesen 4:9e18e041f7a0 305 float Pry = 443;
Jitse_Giesen 15:3a09783b2406 306 /* The end-effector is manually placed in this (see beneath) position*/
Jitse_Giesen 15:3a09783b2406 307 float Pex=91; //initial value choosen for calibration
Jitse_Giesen 15:3a09783b2406 308 float Pey=278; //initial value choosen for calibration
Jitse_Giesen 4:9e18e041f7a0 309 float diamtrklosje=20;
Jitse_Giesen 15:3a09783b2406 310 float pi=3.14159265359; //M_PI didn't work for some reason
Jitse_Giesen 4:9e18e041f7a0 311 float omtrekklosje=diamtrklosje*pi;
Jitse_Giesen 4:9e18e041f7a0 312 float Lou;
Jitse_Giesen 4:9e18e041f7a0 313 float Lbu;
Jitse_Giesen 4:9e18e041f7a0 314 float Lru;
Jitse_Giesen 4:9e18e041f7a0 315 float dLod;
Jitse_Giesen 4:9e18e041f7a0 316 float dLbd;
Jitse_Giesen 4:9e18e041f7a0 317 float dLrd;
Jitse_Giesen 15:3a09783b2406 318
Jitse_Giesen 0:51a6e38a4d4a 319 // Declaring variables needed for calculating motor counts
Jitse_Giesen 4:9e18e041f7a0 320 float roto;
Jitse_Giesen 4:9e18e041f7a0 321 float rotb;
Jitse_Giesen 4:9e18e041f7a0 322 float rotr;
Jitse_Giesen 4:9e18e041f7a0 323 float rotzo;
Jitse_Giesen 4:9e18e041f7a0 324 float rotzb;
Jitse_Giesen 4:9e18e041f7a0 325 float rotzr;
Jitse_Giesen 4:9e18e041f7a0 326 float counto;
Jitse_Giesen 4:9e18e041f7a0 327 float countb;
Jitse_Giesen 4:9e18e041f7a0 328 float countr;
Jitse_Giesen 4:9e18e041f7a0 329 float countzo;
Jitse_Giesen 4:9e18e041f7a0 330 float countzb;
Jitse_Giesen 4:9e18e041f7a0 331 float countzr;
Jitse_Giesen 4:9e18e041f7a0 332
Jitse_Giesen 19:aa1ed300be11 333 /*Declaring variables needed for calculating motor movements to get to a
Jitse_Giesen 19:aa1ed300be11 334 certain point*/
Jitse_Giesen 4:9e18e041f7a0 335 float hcounto;
Jitse_Giesen 4:9e18e041f7a0 336 float hcountb;
Jitse_Giesen 4:9e18e041f7a0 337 float hcountr;
Jitse_Giesen 17:358e7e1213cf 338 int reference_o;
Jitse_Giesen 17:358e7e1213cf 339 int reference_b;
Jitse_Giesen 17:358e7e1213cf 340 int reference_r;
Jitse_Giesen 17:358e7e1213cf 341 int position_o;
Jitse_Giesen 17:358e7e1213cf 342 int position_b;
Jitse_Giesen 17:358e7e1213cf 343 int position_r;
Jitse_Giesen 17:358e7e1213cf 344 int error_o;
Jitse_Giesen 17:358e7e1213cf 345 int error_b;
Jitse_Giesen 17:358e7e1213cf 346 int error_r;
Jitse_Giesen 17:358e7e1213cf 347 float motorValue1;
Jitse_Giesen 17:358e7e1213cf 348 float motorValue2;
Jitse_Giesen 17:358e7e1213cf 349 float motorValue3;
Jitse_Giesen 4:9e18e041f7a0 350 float Psx;
Jitse_Giesen 4:9e18e041f7a0 351 float Psy;
Jitse_Giesen 4:9e18e041f7a0 352 float Vex;
Jitse_Giesen 4:9e18e041f7a0 353 float Vey;
Jitse_Giesen 4:9e18e041f7a0 354 float Pstx;
Jitse_Giesen 4:9e18e041f7a0 355 float Psty;
Jitse_Giesen 4:9e18e041f7a0 356 float T=0.02;//seconds
Jitse_Giesen 4:9e18e041f7a0 357 float kpo = 21;
Jitse_Giesen 4:9e18e041f7a0 358 float kpb = 21;
Jitse_Giesen 4:9e18e041f7a0 359 float kpr = 21;
Jitse_Giesen 19:aa1ed300be11 360 Ticker controlmotor1;
Jitse_Giesen 19:aa1ed300be11 361 Ticker controlmotor2;
Jitse_Giesen 19:aa1ed300be11 362 Ticker controlmotor3;
Jitse_Giesen 4:9e18e041f7a0 363
Jitse_Giesen 15:3a09783b2406 364 //Deel om motor(en) aan te sturen--------------------------------------------
Jitse_Giesen 15:3a09783b2406 365 // start Motor 1 ------------------------------------------------------------
Jitse_Giesen 17:358e7e1213cf 366 float P1(int error_o, float kpo) //Virtual spring with springconstant kpo
Jitse_Giesen 15:3a09783b2406 367 {
Jitse_Giesen 15:3a09783b2406 368 return error_o*kpo;
Jitse_Giesen 15:3a09783b2406 369 }
Jitse_Giesen 0:51a6e38a4d4a 370
Jitse_Giesen 4:9e18e041f7a0 371 void MotorController1()
Jitse_Giesen 4:9e18e041f7a0 372 {
Jitse_Giesen 19:aa1ed300be11 373 /*The reference is the place you want to go to but you have to subtract the
Jitse_Giesen 19:aa1ed300be11 374 initial set position (hcounts) since the encoders 'think' they are at 0 when
Jitse_Giesen 19:aa1ed300be11 375 starting*/
Jitse_Giesen 15:3a09783b2406 376 reference_o = (int) (counto-hcounto);
Jitse_Giesen 15:3a09783b2406 377 position_o = Encoder1.getPulses();
Jitse_Giesen 15:3a09783b2406 378 error_o = reference_o - position_o;
Jitse_Giesen 15:3a09783b2406 379
Jitse_Giesen 19:aa1ed300be11 380 if (-20<error_o && error_o<20) {
Jitse_Giesen 19:aa1ed300be11 381 /*within this bandwith we are satisfied
Jitse_Giesen 19:aa1ed300be11 382 with the error thus the motor should not move anymore*/
Jitse_Giesen 4:9e18e041f7a0 383 motorValue1 = 0;
Jitse_Giesen 15:3a09783b2406 384 } else {
Jitse_Giesen 15:3a09783b2406 385 motorValue1 = P1(error_o, kpo)/4200;
Jitse_Giesen 15:3a09783b2406 386 }
Jitse_Giesen 19:aa1ed300be11 387 /*differs from the other to due to the motor being on the opposite side of
Jitse_Giesen 19:aa1ed300be11 388 the pillar*/
Jitse_Giesen 19:aa1ed300be11 389 if (motorValue1 >=0) motor1DirectionPin=0;
Jitse_Giesen 15:3a09783b2406 390 else motor1DirectionPin=1;
Jitse_Giesen 4:9e18e041f7a0 391 if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
Jitse_Giesen 15:3a09783b2406 392 else motor1MagnitudePin = fabs(motorValue1);
Jitse_Giesen 15:3a09783b2406 393 }
Jitse_Giesen 15:3a09783b2406 394 // end Motor 1 --------------------------------------------------------------
Jitse_Giesen 15:3a09783b2406 395 // start Motor 2 ------------------------------------------------------------
Jitse_Giesen 17:358e7e1213cf 396 float P2(int error_b, float kpb) //Virtual spring with springconstant kpb
Jitse_Giesen 15:3a09783b2406 397 {
Jitse_Giesen 15:3a09783b2406 398 return error_b*kpb;
Jitse_Giesen 15:3a09783b2406 399 }
Jitse_Giesen 0:51a6e38a4d4a 400
Jitse_Giesen 4:9e18e041f7a0 401 void MotorController2()
Jitse_Giesen 0:51a6e38a4d4a 402 {
Jitse_Giesen 19:aa1ed300be11 403 /*The reference is the place you want to go to but you have to subtract the
Jitse_Giesen 19:aa1ed300be11 404 initial set position (hcounts) since the encoders 'think' they are at 0 when
Jitse_Giesen 19:aa1ed300be11 405 starting*/
Jitse_Giesen 15:3a09783b2406 406 reference_b = (int) (-(countb-hcountb));
Jitse_Giesen 15:3a09783b2406 407 position_b = Encoder2.getPulses();
Jitse_Giesen 15:3a09783b2406 408 error_b = reference_b - position_b;
Jitse_Giesen 19:aa1ed300be11 409
Jitse_Giesen 19:aa1ed300be11 410 if (-20<error_b && error_b<20) {
Jitse_Giesen 19:aa1ed300be11 411 /*within this bandwith we are satisfied
Jitse_Giesen 19:aa1ed300be11 412 with the error thus the motor should not move anymore*/
Jitse_Giesen 4:9e18e041f7a0 413 motorValue2 = 0;
Jitse_Giesen 15:3a09783b2406 414 } else {
Jitse_Giesen 15:3a09783b2406 415 motorValue2 = P2(error_b, kpb)/4200;
Jitse_Giesen 15:3a09783b2406 416 }
Jitse_Giesen 15:3a09783b2406 417
Jitse_Giesen 4:9e18e041f7a0 418 if (motorValue2 <=0) motor2DirectionPin=0;
Jitse_Giesen 15:3a09783b2406 419 else motor2DirectionPin=1;
Jitse_Giesen 4:9e18e041f7a0 420 if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
Jitse_Giesen 15:3a09783b2406 421 else motor2MagnitudePin = fabs(motorValue2);
Jitse_Giesen 15:3a09783b2406 422 }
Jitse_Giesen 15:3a09783b2406 423 // end Motor 2 --------------------------------------------------------------
Jitse_Giesen 15:3a09783b2406 424 // start Motor 3 ------------------------------------------------------------
Jitse_Giesen 17:358e7e1213cf 425 float P3(int error_r, float kpr) //Virtual spring with springconstant kpr
Jitse_Giesen 15:3a09783b2406 426 {
Jitse_Giesen 15:3a09783b2406 427 return error_r*kpr;
Jitse_Giesen 15:3a09783b2406 428 }
Jitse_Giesen 4:9e18e041f7a0 429
Jitse_Giesen 4:9e18e041f7a0 430 void MotorController3()
Jitse_Giesen 0:51a6e38a4d4a 431 {
Jitse_Giesen 19:aa1ed300be11 432 /*The reference is the place you want to go to but you have to subtract the
Jitse_Giesen 19:aa1ed300be11 433 initial set position (hcounts) since the encoders 'think' they are at 0 when
Jitse_Giesen 19:aa1ed300be11 434 starting*/
Jitse_Giesen 15:3a09783b2406 435 reference_r = (int) (-(countr-hcountr));
Jitse_Giesen 15:3a09783b2406 436 position_r = Encoder3.getPulses();
Jitse_Giesen 15:3a09783b2406 437 error_r = reference_r - position_r;
Jitse_Giesen 0:51a6e38a4d4a 438
Jitse_Giesen 19:aa1ed300be11 439 if (-20<error_r && error_r<20) {
Jitse_Giesen 19:aa1ed300be11 440 /*within this bandwith we are satisfied
Jitse_Giesen 19:aa1ed300be11 441 with the error thus the motor should not move anymore*/
Jitse_Giesen 15:3a09783b2406 442 motorValue3 = 0;
Jitse_Giesen 15:3a09783b2406 443 } else {
Jitse_Giesen 15:3a09783b2406 444 motorValue3 = P3(error_r, kpr)/4200;
Jitse_Giesen 15:3a09783b2406 445 }
Jitse_Giesen 15:3a09783b2406 446
Jitse_Giesen 15:3a09783b2406 447 if (motorValue3 <=0) motor3DirectionPin=0;
Jitse_Giesen 15:3a09783b2406 448 else motor3DirectionPin=1;
Jitse_Giesen 15:3a09783b2406 449 if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
Jitse_Giesen 15:3a09783b2406 450 else motor3MagnitudePin = fabs(motorValue3);
Jitse_Giesen 15:3a09783b2406 451 }
Jitse_Giesen 15:3a09783b2406 452 // end Motor 3 --------------------------------------------------------------
Jitse_Giesen 15:3a09783b2406 453 // einde deel motor----------------------------------------------------------
Jitse_Giesen 15:3a09783b2406 454
Jitse_Giesen 0:51a6e38a4d4a 455 Ticker loop;
Jitse_Giesen 15:3a09783b2406 456
Jitse_Giesen 19:aa1ed300be11 457 /*Calculates ropelengths that are needed to get to new positions for each time
Jitse_Giesen 15:3a09783b2406 458 step, based on the set coordinates and the position of the poles */
Jitse_Giesen 17:358e7e1213cf 459 /*We think a lot of float with return zero could have been voids*/
Jitse_Giesen 15:3a09783b2406 460 float touwlengtes()
Jitse_Giesen 15:3a09783b2406 461 {
Jitse_Giesen 15:3a09783b2406 462 Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));
Jitse_Giesen 0:51a6e38a4d4a 463 Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2));
Jitse_Giesen 0:51a6e38a4d4a 464 Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
Jitse_Giesen 0:51a6e38a4d4a 465 return 0;
Jitse_Giesen 0:51a6e38a4d4a 466 }
Jitse_Giesen 15:3a09783b2406 467
Jitse_Giesen 15:3a09783b2406 468 /* Calculates rotations (and associated counts) of the motor to get to the
Jitse_Giesen 15:3a09783b2406 469 desired new position for each time step*/
Jitse_Giesen 15:3a09783b2406 470 float turns()
Jitse_Giesen 15:3a09783b2406 471 {
Jitse_Giesen 15:3a09783b2406 472 roto=Lou/omtrekklosje;
Jitse_Giesen 15:3a09783b2406 473 rotb=Lbu/omtrekklosje;
Jitse_Giesen 15:3a09783b2406 474 rotr=Lru/omtrekklosje;
Jitse_Giesen 15:3a09783b2406 475 counto=roto*4200;
Jitse_Giesen 0:51a6e38a4d4a 476 countb=rotb*4200;
Jitse_Giesen 0:51a6e38a4d4a 477 countr=rotr*4200;
Jitse_Giesen 0:51a6e38a4d4a 478 return 0;
Jitse_Giesen 0:51a6e38a4d4a 479 }
Jitse_Giesen 15:3a09783b2406 480
Jitse_Giesen 17:358e7e1213cf 481 //calculate the setpoint for each time step in coördinates, ropelenghts and counts
Jitse_Giesen 15:3a09783b2406 482 float Pst()
Jitse_Giesen 15:3a09783b2406 483 {
Jitse_Giesen 19:aa1ed300be11 484 Pstx=Pex+Vex*T;
Jitse_Giesen 0:51a6e38a4d4a 485 Psty=Pey+Vey*T;
Jitse_Giesen 0:51a6e38a4d4a 486 touwlengtes();
Jitse_Giesen 15:3a09783b2406 487 Pex=Pstx;
Jitse_Giesen 0:51a6e38a4d4a 488 Pey=Psty;
Jitse_Giesen 0:51a6e38a4d4a 489 turns();
Jitse_Giesen 0:51a6e38a4d4a 490 return 0;
Jitse_Giesen 15:3a09783b2406 491 }
Jitse_Giesen 15:3a09783b2406 492
Jitse_Giesen 19:aa1ed300be11 493 //Calculating desired end position based on the EMG input
Jitse_Giesen 15:3a09783b2406 494 float Ps()
Jitse_Giesen 15:3a09783b2406 495 {
Jitse_Giesen 4:9e18e041f7a0 496 Psx=(Xin_new)*30+91;
Jitse_Giesen 15:3a09783b2406 497 Psy=(Yin_new)*30+278;
Jitse_Giesen 0:51a6e38a4d4a 498 return 0;
Jitse_Giesen 0:51a6e38a4d4a 499 }
Jitse_Giesen 15:3a09783b2406 500
Jitse_Giesen 17:358e7e1213cf 501 //Calculates the vector pointing from position to setpoint
Jitse_Giesen 15:3a09783b2406 502 void Ve()
Jitse_Giesen 15:3a09783b2406 503 {
Jitse_Giesen 4:9e18e041f7a0 504 Vex=(Psx-Pex);
Jitse_Giesen 4:9e18e041f7a0 505 Vey=(Psy-Pey);
Jitse_Giesen 17:358e7e1213cf 506 Pst(); //calculates the new position for the next time step
Jitse_Giesen 19:aa1ed300be11 507 if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)) {
Jitse_Giesen 19:aa1ed300be11 508 /*If the velocities are lower
Jitse_Giesen 19:aa1ed300be11 509 than 0.01 m/s the move is done and the loop can be detached*/
Jitse_Giesen 0:51a6e38a4d4a 510 Move_done=true;
Jitse_Giesen 0:51a6e38a4d4a 511 loop.detach();
Jitse_Giesen 15:3a09783b2406 512 }
Jitse_Giesen 0:51a6e38a4d4a 513 }
Jitse_Giesen 15:3a09783b2406 514
Jitse_Giesen 17:358e7e1213cf 515 // Calculating the desired position
Jitse_Giesen 15:3a09783b2406 516 int calculator()
Jitse_Giesen 15:3a09783b2406 517 {
Jitse_Giesen 0:51a6e38a4d4a 518 Ps();
Jitse_Giesen 19:aa1ed300be11 519 if (Move_done == false) {
Jitse_Giesen 19:aa1ed300be11 520 /*While the move is being executed the new
Jitse_Giesen 19:aa1ed300be11 521 vector and new position (Pst) have to be calculated continiously*/
Jitse_Giesen 15:3a09783b2406 522 loop.attach(&Ve,0.02);
Jitse_Giesen 4:9e18e041f7a0 523 }
Jitse_Giesen 0:51a6e38a4d4a 524 return 0;
Jitse_Giesen 0:51a6e38a4d4a 525 }
Jitse_Giesen 15:3a09783b2406 526
Jitse_Giesen 0:51a6e38a4d4a 527 // Function which makes it possible to lower the end-effector to pick up a piece
Jitse_Giesen 15:3a09783b2406 528 void zakker()
Jitse_Giesen 15:3a09783b2406 529 {
Jitse_Giesen 17:358e7e1213cf 530 /*277.85 is the distance between the board and the bottom of the magnet */
Jitse_Giesen 15:3a09783b2406 531 dLod=sqrt(pow(Lou,2)+pow((277.85),2))-Lou;
Jitse_Giesen 19:aa1ed300be11 532 dLbd=sqrt(pow(Lbu,2)+pow((277.85),2))-Lbu;
Jitse_Giesen 8:8e7c928aadc6 533 dLrd=sqrt(pow(Lru,2)+pow((277.85),2))-Lru;
Jitse_Giesen 0:51a6e38a4d4a 534 rotzo=dLod/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 535 rotzb=dLbd/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 536 rotzr=dLrd/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 537 countzo=rotzo*4200;
Jitse_Giesen 0:51a6e38a4d4a 538 countzb=rotzb*4200;
Jitse_Giesen 0:51a6e38a4d4a 539 countzr=rotzr*4200;
Jitse_Giesen 19:aa1ed300be11 540 /*first one (counto)differs from the other to due to the motor being on the
Jitse_Giesen 17:358e7e1213cf 541 opposite side of the pillar*/
Jitse_Giesen 19:aa1ed300be11 542 counto=countzo+hcounto+reference_o;
Jitse_Giesen 15:3a09783b2406 543 countb=-(reference_b-countzb-hcountb);
Jitse_Giesen 15:3a09783b2406 544 countr=-(reference_r-countzr-hcountr);
Jitse_Giesen 15:3a09783b2406 545 }
Jitse_Giesen 8:8e7c928aadc6 546
Jitse_Giesen 19:aa1ed300be11 547 //Checks if the threshold for the left biceps is reached
Jitse_Giesen 15:3a09783b2406 548 void zakken_threshold()
Jitse_Giesen 15:3a09783b2406 549 {
Jitse_Giesen 17:358e7e1213cf 550 if (Move_done == true) { //should only be executed when the move is done
Jitse_Giesen 15:3a09783b2406 551 if (emgBLcomplete > thresholdBL) {
Jitse_Giesen 15:3a09783b2406 552 zakker();
Jitse_Giesen 15:3a09783b2406 553 }
Jitse_Giesen 4:9e18e041f7a0 554 }
Jitse_Giesen 15:3a09783b2406 555 }
Jitse_Giesen 17:358e7e1213cf 556 /*Calculates the counts corrosponding with the set position (which is (0,0))*/
Jitse_Giesen 15:3a09783b2406 557 void setcurrentposition()
Jitse_Giesen 15:3a09783b2406 558 {
Jitse_Giesen 15:3a09783b2406 559 if(Input_done==true) {
Jitse_Giesen 4:9e18e041f7a0 560 hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje);
Jitse_Giesen 4:9e18e041f7a0 561 hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje);
Jitse_Giesen 4:9e18e041f7a0 562 hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje);
Jitse_Giesen 4:9e18e041f7a0 563 Input_done=false;
Jitse_Giesen 4:9e18e041f7a0 564 }
Jitse_Giesen 15:3a09783b2406 565 }
Jitse_Giesen 15:3a09783b2406 566
Jitse_Giesen 0:51a6e38a4d4a 567 int main()
Jitse_Giesen 0:51a6e38a4d4a 568 {
Jitse_Giesen 0:51a6e38a4d4a 569 pc.baud(115200);
Jitse_Giesen 17:358e7e1213cf 570 wait(1.0f);//Gives you one second between starting te program and calibrating
Jitse_Giesen 2:bf1c9d7afabd 571 getbqChain();
Jitse_Giesen 2:bf1c9d7afabd 572 threshold_timerR.attach(&Threshold_samplingBR, 0.002);
Jitse_Giesen 2:bf1c9d7afabd 573 threshold_timerL.attach(&Threshold_samplingBL, 0.002);
Jitse_Giesen 4:9e18e041f7a0 574 setcurrentposition();
Jitse_Giesen 15:3a09783b2406 575 while(true) {
Jitse_Giesen 4:9e18e041f7a0 576 sample_timer.attach(&EMG_sample, 0.002);
Jitse_Giesen 8:8e7c928aadc6 577 zakken_threshold();
Jitse_Giesen 19:aa1ed300be11 578 wait(2.5f); /*To give the user time between calibration and input, and
Jitse_Giesen 19:aa1ed300be11 579 allow the lowering to take place before new input is asked*/
Jitse_Giesen 18:7fb73aa6dbc0 580 tellerX();
Jitse_Giesen 0:51a6e38a4d4a 581 tellerY();
Jitse_Giesen 0:51a6e38a4d4a 582 calculator();
Jitse_Giesen 4:9e18e041f7a0 583 controlmotor1.attach(&MotorController1, 0.01);
Jitse_Giesen 4:9e18e041f7a0 584 controlmotor2.attach(&MotorController2, 0.01);
Jitse_Giesen 4:9e18e041f7a0 585 controlmotor3.attach(&MotorController3, 0.01);
Jitse_Giesen 19:aa1ed300be11 586 wait(4.0f); /*To allow the move in the XY-plane to finish before
Jitse_Giesen 18:7fb73aa6dbc0 587 lowering can start*/
Jitse_Giesen 15:3a09783b2406 588 }
Jitse_Giesen 0:51a6e38a4d4a 589 }