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@18:7fb73aa6dbc0, 2017-11-07 (annotated)

Committer:
Jitse_Giesen
Date:
Tue Nov 07 11:05:47 2017 +0000
Revision:
18:7fb73aa6dbc0
Parent:
17:358e7e1213cf
Child:
19:aa1ed300be11
Final script with useful comments;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jitse_Giesen 18:7fb73aa6dbc0 1 /* ___________ ___
Jitse_Giesen 18:7fb73aa6dbc0 2 / | | \ / o\
Jitse_Giesen 18:7fb73aa6dbc0 3 /___|____|____\_/ _ >
Jitse_Giesen 18:7fb73aa6dbc0 4 / / | \ _____/
Jitse_Giesen 18:7fb73aa6dbc0 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 15:3a09783b2406 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 10:a8a07e4ce85c 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 17:358e7e1213cf 183 //Function to make the BiQuadChain for the Notch and High pass filter for all three filters
Jitse_Giesen 2:bf1c9d7afabd 184 void getbqChain()
Jitse_Giesen 2:bf1c9d7afabd 185 {
Jitse_Giesen 2:bf1c9d7afabd 186 bqChain1.add(&bqNotch1).add(&bqHigh1); //Making the BiQuadChain
Jitse_Giesen 2:bf1c9d7afabd 187 bqChain2.add(&bqNotch2).add(&bqHigh2);
Jitse_Giesen 15:3a09783b2406 188
Jitse_Giesen 2:bf1c9d7afabd 189 bqChainTR.add(&bqNotchTR).add(&bqHighTR);
Jitse_Giesen 2:bf1c9d7afabd 190 bqChainTL.add(&bqNotchTR).add(&bqHighTL);
Jitse_Giesen 2:bf1c9d7afabd 191 }
Jitse_Giesen 15:3a09783b2406 192
Jitse_Giesen 17:358e7e1213cf 193 //Initial input value for couting the X-values
Jitse_Giesen 17:358e7e1213cf 194 int Xin=0 ; //set X to zero for the first input sequence
Jitse_Giesen 15:3a09783b2406 195 int Xin_new;
Jitse_Giesen 4:9e18e041f7a0 196 float huidigetijdX;
Jitse_Giesen 15:3a09783b2406 197
Jitse_Giesen 17:358e7e1213cf 198 /*Feedback system for counting values of X:
Jitse_Giesen 17:358e7e1213cf 199 The user has 2 secondes to give input before the program jumps to the next
Jitse_Giesen 17:358e7e1213cf 200 section. If input is regesered the timer is reset so the user has 2 secondes
Jitse_Giesen 17:358e7e1213cf 201 again for the next input.*/
Jitse_Giesen 15:3a09783b2406 202 void ledtX()
Jitse_Giesen 15:3a09783b2406 203 {
Jitse_Giesen 17:358e7e1213cf 204 t.reset(); //Reset (restart) the timer
Jitse_Giesen 0:51a6e38a4d4a 205 Xin++;
Jitse_Giesen 0:51a6e38a4d4a 206 pc.printf("Xin is %i\n",Xin);
Jitse_Giesen 17:358e7e1213cf 207 led_G=0; //Feedback for user to ensure his input is regestered
Jitse_Giesen 17:358e7e1213cf 208 led_R=1;
Jitse_Giesen 16:cd11083c754a 209 wait(0.2); //time led green on
Jitse_Giesen 0:51a6e38a4d4a 210 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 211 led_R=0;
Jitse_Giesen 17:358e7e1213cf 212 wait(0.5); /*prevent multiple counts for one muscle contraction. This way
Jitse_Giesen 17:358e7e1213cf 213 only one contraction can be regestered per half second*/
Jitse_Giesen 15:3a09783b2406 214 }
Jitse_Giesen 15:3a09783b2406 215
Jitse_Giesen 0:51a6e38a4d4a 216 // Couting system for values of X
Jitse_Giesen 15:3a09783b2406 217 int tellerX()
Jitse_Giesen 15:3a09783b2406 218 {
Jitse_Giesen 0:51a6e38a4d4a 219 if (Move_done == true) {
Jitse_Giesen 0:51a6e38a4d4a 220 t.reset();
Jitse_Giesen 15:3a09783b2406 221 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 222 led_B=1;
Jitse_Giesen 0:51a6e38a4d4a 223 led_R=0;
Jitse_Giesen 15:3a09783b2406 224 while(true) {
Jitse_Giesen 15:3a09783b2406 225 //button.fall(ledtX); // this can be used for testing without EMG
Jitse_Giesen 15:3a09783b2406 226 if (emgBRcomplete > thresholdBR) {
Jitse_Giesen 15:3a09783b2406 227 ledtX();
Jitse_Giesen 0:51a6e38a4d4a 228 }
Jitse_Giesen 17:358e7e1213cf 229 t.start(); //Start timer
Jitse_Giesen 15:3a09783b2406 230 huidigetijdX=t.read();
Jitse_Giesen 17:358e7e1213cf 231 if (huidigetijdX>2) { //After 2 seconds without input
Jitse_Giesen 17:358e7e1213cf 232 led_R=1;
Jitse_Giesen 15:3a09783b2406 233 Xin_new = Xin;
Jitse_Giesen 17:358e7e1213cf 234 Xin = 0; //Reset X to zero for the next input sequence
Jitse_Giesen 15:3a09783b2406 235
Jitse_Giesen 17:358e7e1213cf 236 return Xin_new; //Go to the next program
Jitse_Giesen 0:51a6e38a4d4a 237 }
Jitse_Giesen 15:3a09783b2406 238
Jitse_Giesen 15:3a09783b2406 239 }
Jitse_Giesen 15:3a09783b2406 240
Jitse_Giesen 15:3a09783b2406 241 }
Jitse_Giesen 15:3a09783b2406 242 return 0;
Jitse_Giesen 15:3a09783b2406 243 }
Jitse_Giesen 15:3a09783b2406 244
Jitse_Giesen 15:3a09783b2406 245 // Initial values needed for Y (see comments at X function)
Jitse_Giesen 0:51a6e38a4d4a 246 int Yin=0;
Jitse_Giesen 0:51a6e38a4d4a 247 int Yin_new;
Jitse_Giesen 4:9e18e041f7a0 248 float huidigetijdY;
Jitse_Giesen 15:3a09783b2406 249
Jitse_Giesen 17:358e7e1213cf 250 /*Feedback system for counting values of Y:
Jitse_Giesen 17:358e7e1213cf 251 The user has 2 secondes to give input before the program jumps to the next
Jitse_Giesen 17:358e7e1213cf 252 section. If input is regesered the timer is reset so the user has 2 secondes
Jitse_Giesen 17:358e7e1213cf 253 again for the next input.*/
Jitse_Giesen 15:3a09783b2406 254 void ledtY()
Jitse_Giesen 15:3a09783b2406 255 {
Jitse_Giesen 17:358e7e1213cf 256 t.reset(); //Reset (restart) the timer
Jitse_Giesen 0:51a6e38a4d4a 257 Yin++;
Jitse_Giesen 0:51a6e38a4d4a 258 pc.printf("Yin is %i\n",Yin);
Jitse_Giesen 17:358e7e1213cf 259 led_G=0; //Feedback for user to ensure his input is regestered
Jitse_Giesen 0:51a6e38a4d4a 260 led_B=1;
Jitse_Giesen 17:358e7e1213cf 261 wait(0.2); //time led green on
Jitse_Giesen 0:51a6e38a4d4a 262 led_G=1;
Jitse_Giesen 0:51a6e38a4d4a 263 led_B=0;
Jitse_Giesen 17:358e7e1213cf 264 wait(0.5); /*prevent multiple counts for one muscle contraction. This way
Jitse_Giesen 17:358e7e1213cf 265 only one contraction can be regestered per half second*/
Jitse_Giesen 15:3a09783b2406 266 }
Jitse_Giesen 15:3a09783b2406 267
Jitse_Giesen 0:51a6e38a4d4a 268 // Couting system for values of Y
Jitse_Giesen 15:3a09783b2406 269 int tellerY()
Jitse_Giesen 15:3a09783b2406 270 {
Jitse_Giesen 4:9e18e041f7a0 271 if (Move_done == true) {
Jitse_Giesen 15:3a09783b2406 272 t.reset();
Jitse_Giesen 15:3a09783b2406 273 led_G=1;
Jitse_Giesen 15:3a09783b2406 274 led_B=0;
Jitse_Giesen 15:3a09783b2406 275 led_R=1;
Jitse_Giesen 15:3a09783b2406 276 while(true) {
Jitse_Giesen 15:3a09783b2406 277 //button.fall(ledtY); // this can be used for testing without EMG
Jitse_Giesen 15:3a09783b2406 278 if (emgBRcomplete > thresholdBR) {
Jitse_Giesen 15:3a09783b2406 279 ledtY();
Jitse_Giesen 15:3a09783b2406 280 }
Jitse_Giesen 17:358e7e1213cf 281 t.start(); //Start timer
Jitse_Giesen 15:3a09783b2406 282 huidigetijdY=t.read();
Jitse_Giesen 17:358e7e1213cf 283 if (huidigetijdY>2) { //After 2 seconds without input
Jitse_Giesen 15:3a09783b2406 284 led_B=1;
Jitse_Giesen 15:3a09783b2406 285 Yin_new = Yin;
Jitse_Giesen 15:3a09783b2406 286 Yin = 0;
Jitse_Giesen 17:358e7e1213cf 287 Input_done = true; //Set input done to True
Jitse_Giesen 17:358e7e1213cf 288 Move_done = false; //Next section is moving so move done is false
Jitse_Giesen 17:358e7e1213cf 289 return Yin_new; //Go to the next program
Jitse_Giesen 15:3a09783b2406 290
Jitse_Giesen 15:3a09783b2406 291 }
Jitse_Giesen 0:51a6e38a4d4a 292 }
Jitse_Giesen 0:51a6e38a4d4a 293 }
Jitse_Giesen 17:358e7e1213cf 294 return 0; //Go to the next program
Jitse_Giesen 0:51a6e38a4d4a 295 }
Jitse_Giesen 15:3a09783b2406 296
Jitse_Giesen 15:3a09783b2406 297 // Declaring all variables needed for calculating rope lengths,
Jitse_Giesen 15:3a09783b2406 298 /* The following six floats were found using our SOLIDWORKS model*/
Jitse_Giesen 4:9e18e041f7a0 299 float Pox = 0;
Jitse_Giesen 4:9e18e041f7a0 300 float Poy = 0;
Jitse_Giesen 4:9e18e041f7a0 301 float Pbx = 0;
Jitse_Giesen 4:9e18e041f7a0 302 float Pby = 887;
Jitse_Giesen 4:9e18e041f7a0 303 float Prx = 768;
Jitse_Giesen 4:9e18e041f7a0 304 float Pry = 443;
Jitse_Giesen 15:3a09783b2406 305 /* The end-effector is manually placed in this (see beneath) position*/
Jitse_Giesen 15:3a09783b2406 306 float Pex=91; //initial value choosen for calibration
Jitse_Giesen 15:3a09783b2406 307 float Pey=278; //initial value choosen for calibration
Jitse_Giesen 4:9e18e041f7a0 308 float diamtrklosje=20;
Jitse_Giesen 15:3a09783b2406 309 float pi=3.14159265359; //M_PI didn't work for some reason
Jitse_Giesen 4:9e18e041f7a0 310 float omtrekklosje=diamtrklosje*pi;
Jitse_Giesen 4:9e18e041f7a0 311 float Lou;
Jitse_Giesen 4:9e18e041f7a0 312 float Lbu;
Jitse_Giesen 4:9e18e041f7a0 313 float Lru;
Jitse_Giesen 4:9e18e041f7a0 314 float dLod;
Jitse_Giesen 4:9e18e041f7a0 315 float dLbd;
Jitse_Giesen 4:9e18e041f7a0 316 float dLrd;
Jitse_Giesen 15:3a09783b2406 317
Jitse_Giesen 0:51a6e38a4d4a 318 // Declaring variables needed for calculating motor counts
Jitse_Giesen 4:9e18e041f7a0 319 float roto;
Jitse_Giesen 4:9e18e041f7a0 320 float rotb;
Jitse_Giesen 4:9e18e041f7a0 321 float rotr;
Jitse_Giesen 4:9e18e041f7a0 322 float rotzo;
Jitse_Giesen 4:9e18e041f7a0 323 float rotzb;
Jitse_Giesen 4:9e18e041f7a0 324 float rotzr;
Jitse_Giesen 4:9e18e041f7a0 325 float counto;
Jitse_Giesen 4:9e18e041f7a0 326 float countb;
Jitse_Giesen 4:9e18e041f7a0 327 float countr;
Jitse_Giesen 4:9e18e041f7a0 328 float countzo;
Jitse_Giesen 4:9e18e041f7a0 329 float countzb;
Jitse_Giesen 4:9e18e041f7a0 330 float countzr;
Jitse_Giesen 4:9e18e041f7a0 331
Jitse_Giesen 17:358e7e1213cf 332 // Declaring variables needed for calculating motor movements to get to a certain point
Jitse_Giesen 4:9e18e041f7a0 333 float hcounto;
Jitse_Giesen 4:9e18e041f7a0 334 float hcountb;
Jitse_Giesen 4:9e18e041f7a0 335 float hcountr;
Jitse_Giesen 17:358e7e1213cf 336 int reference_o;
Jitse_Giesen 17:358e7e1213cf 337 int reference_b;
Jitse_Giesen 17:358e7e1213cf 338 int reference_r;
Jitse_Giesen 17:358e7e1213cf 339 int position_o;
Jitse_Giesen 17:358e7e1213cf 340 int position_b;
Jitse_Giesen 17:358e7e1213cf 341 int position_r;
Jitse_Giesen 17:358e7e1213cf 342 int error_o;
Jitse_Giesen 17:358e7e1213cf 343 int error_b;
Jitse_Giesen 17:358e7e1213cf 344 int error_r;
Jitse_Giesen 17:358e7e1213cf 345 float motorValue1;
Jitse_Giesen 17:358e7e1213cf 346 float motorValue2;
Jitse_Giesen 17:358e7e1213cf 347 float motorValue3;
Jitse_Giesen 4:9e18e041f7a0 348 float Psx;
Jitse_Giesen 4:9e18e041f7a0 349 float Psy;
Jitse_Giesen 4:9e18e041f7a0 350 float Vex;
Jitse_Giesen 4:9e18e041f7a0 351 float Vey;
Jitse_Giesen 4:9e18e041f7a0 352 float Pstx;
Jitse_Giesen 4:9e18e041f7a0 353 float Psty;
Jitse_Giesen 4:9e18e041f7a0 354 float T=0.02;//seconds
Jitse_Giesen 4:9e18e041f7a0 355 float kpo = 21;
Jitse_Giesen 4:9e18e041f7a0 356 float kpb = 21;
Jitse_Giesen 4:9e18e041f7a0 357 float kpr = 21;
Jitse_Giesen 17:358e7e1213cf 358 Ticker controlmotor1;
Jitse_Giesen 17:358e7e1213cf 359 Ticker controlmotor2;
Jitse_Giesen 17:358e7e1213cf 360 Ticker controlmotor3;
Jitse_Giesen 4:9e18e041f7a0 361
Jitse_Giesen 15:3a09783b2406 362 //Deel om motor(en) aan te sturen--------------------------------------------
Jitse_Giesen 15:3a09783b2406 363 // start Motor 1 ------------------------------------------------------------
Jitse_Giesen 17:358e7e1213cf 364 float P1(int error_o, float kpo) //Virtual spring with springconstant kpo
Jitse_Giesen 15:3a09783b2406 365 {
Jitse_Giesen 15:3a09783b2406 366 return error_o*kpo;
Jitse_Giesen 15:3a09783b2406 367 }
Jitse_Giesen 0:51a6e38a4d4a 368
Jitse_Giesen 4:9e18e041f7a0 369 void MotorController1()
Jitse_Giesen 4:9e18e041f7a0 370 {
Jitse_Giesen 17:358e7e1213cf 371 /*The reference is the place you want to go to but you have to subtract the
Jitse_Giesen 17:358e7e1213cf 372 initial set position (hcounts) since the encoders 'think' they are at 0 when
Jitse_Giesen 17:358e7e1213cf 373 starting*/
Jitse_Giesen 15:3a09783b2406 374 reference_o = (int) (counto-hcounto);
Jitse_Giesen 15:3a09783b2406 375 position_o = Encoder1.getPulses();
Jitse_Giesen 15:3a09783b2406 376 error_o = reference_o - position_o;
Jitse_Giesen 15:3a09783b2406 377
Jitse_Giesen 17:358e7e1213cf 378 if (-20<error_o && error_o<20) { /*within this bandwith we are satisfied
Jitse_Giesen 17:358e7e1213cf 379 with the error thus the motor should not move anymore*/
Jitse_Giesen 4:9e18e041f7a0 380 motorValue1 = 0;
Jitse_Giesen 15:3a09783b2406 381 } else {
Jitse_Giesen 15:3a09783b2406 382 motorValue1 = P1(error_o, kpo)/4200;
Jitse_Giesen 15:3a09783b2406 383 }
Jitse_Giesen 15:3a09783b2406 384 /*differs from the other to due to the motor being on the opposite side of the pillar*/
Jitse_Giesen 15:3a09783b2406 385 if (motorValue1 >=0) motor1DirectionPin=0;
Jitse_Giesen 15:3a09783b2406 386 else motor1DirectionPin=1;
Jitse_Giesen 4:9e18e041f7a0 387 if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
Jitse_Giesen 15:3a09783b2406 388 else motor1MagnitudePin = fabs(motorValue1);
Jitse_Giesen 15:3a09783b2406 389 }
Jitse_Giesen 15:3a09783b2406 390 // end Motor 1 --------------------------------------------------------------
Jitse_Giesen 15:3a09783b2406 391 // start Motor 2 ------------------------------------------------------------
Jitse_Giesen 17:358e7e1213cf 392 float P2(int error_b, float kpb) //Virtual spring with springconstant kpb
Jitse_Giesen 15:3a09783b2406 393 {
Jitse_Giesen 15:3a09783b2406 394 return error_b*kpb;
Jitse_Giesen 15:3a09783b2406 395 }
Jitse_Giesen 0:51a6e38a4d4a 396
Jitse_Giesen 4:9e18e041f7a0 397 void MotorController2()
Jitse_Giesen 0:51a6e38a4d4a 398 {
Jitse_Giesen 17:358e7e1213cf 399 /*The reference is the place you want to go to but you have to subtract the
Jitse_Giesen 17:358e7e1213cf 400 initial set position (hcounts) since the encoders 'think' they are at 0 when
Jitse_Giesen 17:358e7e1213cf 401 starting*/
Jitse_Giesen 15:3a09783b2406 402 reference_b = (int) (-(countb-hcountb));
Jitse_Giesen 15:3a09783b2406 403 position_b = Encoder2.getPulses();
Jitse_Giesen 15:3a09783b2406 404 error_b = reference_b - position_b;
Jitse_Giesen 4:9e18e041f7a0 405
Jitse_Giesen 17:358e7e1213cf 406 if (-20<error_b && error_b<20) { /*within this bandwith we are satisfied
Jitse_Giesen 17:358e7e1213cf 407 with the error thus the motor should not move anymore*/
Jitse_Giesen 4:9e18e041f7a0 408 motorValue2 = 0;
Jitse_Giesen 15:3a09783b2406 409 } else {
Jitse_Giesen 15:3a09783b2406 410 motorValue2 = P2(error_b, kpb)/4200;
Jitse_Giesen 15:3a09783b2406 411 }
Jitse_Giesen 15:3a09783b2406 412
Jitse_Giesen 4:9e18e041f7a0 413 if (motorValue2 <=0) motor2DirectionPin=0;
Jitse_Giesen 15:3a09783b2406 414 else motor2DirectionPin=1;
Jitse_Giesen 4:9e18e041f7a0 415 if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
Jitse_Giesen 15:3a09783b2406 416 else motor2MagnitudePin = fabs(motorValue2);
Jitse_Giesen 15:3a09783b2406 417 }
Jitse_Giesen 15:3a09783b2406 418 // end Motor 2 --------------------------------------------------------------
Jitse_Giesen 15:3a09783b2406 419 // start Motor 3 ------------------------------------------------------------
Jitse_Giesen 17:358e7e1213cf 420 float P3(int error_r, float kpr) //Virtual spring with springconstant kpr
Jitse_Giesen 15:3a09783b2406 421 {
Jitse_Giesen 15:3a09783b2406 422 return error_r*kpr;
Jitse_Giesen 15:3a09783b2406 423 }
Jitse_Giesen 4:9e18e041f7a0 424
Jitse_Giesen 4:9e18e041f7a0 425 void MotorController3()
Jitse_Giesen 0:51a6e38a4d4a 426 {
Jitse_Giesen 17:358e7e1213cf 427 /*The reference is the place you want to go to but you have to subtract the
Jitse_Giesen 17:358e7e1213cf 428 initial set position (hcounts) since the encoders 'think' they are at 0 when
Jitse_Giesen 17:358e7e1213cf 429 starting*/
Jitse_Giesen 15:3a09783b2406 430 reference_r = (int) (-(countr-hcountr));
Jitse_Giesen 15:3a09783b2406 431 position_r = Encoder3.getPulses();
Jitse_Giesen 15:3a09783b2406 432 error_r = reference_r - position_r;
Jitse_Giesen 0:51a6e38a4d4a 433
Jitse_Giesen 17:358e7e1213cf 434 if (-20<error_r && error_r<20) { /*within this bandwith we are satisfied
Jitse_Giesen 17:358e7e1213cf 435 with the error thus the motor should not move anymore*/
Jitse_Giesen 15:3a09783b2406 436 motorValue3 = 0;
Jitse_Giesen 15:3a09783b2406 437 } else {
Jitse_Giesen 15:3a09783b2406 438 motorValue3 = P3(error_r, kpr)/4200;
Jitse_Giesen 15:3a09783b2406 439 }
Jitse_Giesen 15:3a09783b2406 440
Jitse_Giesen 15:3a09783b2406 441 if (motorValue3 <=0) motor3DirectionPin=0;
Jitse_Giesen 15:3a09783b2406 442 else motor3DirectionPin=1;
Jitse_Giesen 15:3a09783b2406 443 if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
Jitse_Giesen 15:3a09783b2406 444 else motor3MagnitudePin = fabs(motorValue3);
Jitse_Giesen 15:3a09783b2406 445 }
Jitse_Giesen 15:3a09783b2406 446 // end Motor 3 --------------------------------------------------------------
Jitse_Giesen 15:3a09783b2406 447 // einde deel motor----------------------------------------------------------
Jitse_Giesen 15:3a09783b2406 448
Jitse_Giesen 0:51a6e38a4d4a 449 Ticker loop;
Jitse_Giesen 15:3a09783b2406 450
Jitse_Giesen 15:3a09783b2406 451 /*Calculates ropelengths that are needed to get to new positions for each time
Jitse_Giesen 15:3a09783b2406 452 step, based on the set coordinates and the position of the poles */
Jitse_Giesen 17:358e7e1213cf 453 /*We think a lot of float with return zero could have been voids*/
Jitse_Giesen 15:3a09783b2406 454 float touwlengtes()
Jitse_Giesen 15:3a09783b2406 455 {
Jitse_Giesen 15:3a09783b2406 456 Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));
Jitse_Giesen 0:51a6e38a4d4a 457 Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2));
Jitse_Giesen 0:51a6e38a4d4a 458 Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
Jitse_Giesen 0:51a6e38a4d4a 459 return 0;
Jitse_Giesen 0:51a6e38a4d4a 460 }
Jitse_Giesen 15:3a09783b2406 461
Jitse_Giesen 15:3a09783b2406 462 /* Calculates rotations (and associated counts) of the motor to get to the
Jitse_Giesen 15:3a09783b2406 463 desired new position for each time step*/
Jitse_Giesen 15:3a09783b2406 464 float turns()
Jitse_Giesen 15:3a09783b2406 465 {
Jitse_Giesen 15:3a09783b2406 466 roto=Lou/omtrekklosje;
Jitse_Giesen 15:3a09783b2406 467 rotb=Lbu/omtrekklosje;
Jitse_Giesen 15:3a09783b2406 468 rotr=Lru/omtrekklosje;
Jitse_Giesen 15:3a09783b2406 469 counto=roto*4200;
Jitse_Giesen 0:51a6e38a4d4a 470 countb=rotb*4200;
Jitse_Giesen 0:51a6e38a4d4a 471 countr=rotr*4200;
Jitse_Giesen 0:51a6e38a4d4a 472 return 0;
Jitse_Giesen 0:51a6e38a4d4a 473 }
Jitse_Giesen 15:3a09783b2406 474
Jitse_Giesen 17:358e7e1213cf 475 //calculate the setpoint for each time step in coördinates, ropelenghts and counts
Jitse_Giesen 15:3a09783b2406 476 float Pst()
Jitse_Giesen 15:3a09783b2406 477 {
Jitse_Giesen 17:358e7e1213cf 478 Pstx=Pex+Vex*T;
Jitse_Giesen 0:51a6e38a4d4a 479 Psty=Pey+Vey*T;
Jitse_Giesen 0:51a6e38a4d4a 480 touwlengtes();
Jitse_Giesen 15:3a09783b2406 481 Pex=Pstx;
Jitse_Giesen 0:51a6e38a4d4a 482 Pey=Psty;
Jitse_Giesen 0:51a6e38a4d4a 483 turns();
Jitse_Giesen 0:51a6e38a4d4a 484 return 0;
Jitse_Giesen 15:3a09783b2406 485 }
Jitse_Giesen 15:3a09783b2406 486
Jitse_Giesen 17:358e7e1213cf 487 //Calculating desired end position based on the EMG input
Jitse_Giesen 15:3a09783b2406 488 float Ps()
Jitse_Giesen 15:3a09783b2406 489 {
Jitse_Giesen 4:9e18e041f7a0 490 Psx=(Xin_new)*30+91;
Jitse_Giesen 15:3a09783b2406 491 Psy=(Yin_new)*30+278;
Jitse_Giesen 0:51a6e38a4d4a 492 return 0;
Jitse_Giesen 0:51a6e38a4d4a 493 }
Jitse_Giesen 15:3a09783b2406 494
Jitse_Giesen 17:358e7e1213cf 495 //Calculates the vector pointing from position to setpoint
Jitse_Giesen 15:3a09783b2406 496 void Ve()
Jitse_Giesen 15:3a09783b2406 497 {
Jitse_Giesen 4:9e18e041f7a0 498 Vex=(Psx-Pex);
Jitse_Giesen 4:9e18e041f7a0 499 Vey=(Psy-Pey);
Jitse_Giesen 17:358e7e1213cf 500 Pst(); //calculates the new position for the next time step
Jitse_Giesen 17:358e7e1213cf 501 if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)) { /*If the velocities are lower
Jitse_Giesen 17:358e7e1213cf 502 than 0.01 m/s the move is done and the loop can be detached*/
Jitse_Giesen 0:51a6e38a4d4a 503 Move_done=true;
Jitse_Giesen 0:51a6e38a4d4a 504 loop.detach();
Jitse_Giesen 15:3a09783b2406 505 }
Jitse_Giesen 0:51a6e38a4d4a 506 }
Jitse_Giesen 15:3a09783b2406 507
Jitse_Giesen 17:358e7e1213cf 508 // Calculating the desired position
Jitse_Giesen 15:3a09783b2406 509 int calculator()
Jitse_Giesen 15:3a09783b2406 510 {
Jitse_Giesen 0:51a6e38a4d4a 511 Ps();
Jitse_Giesen 17:358e7e1213cf 512 if (Move_done == false) { /*While the move is being executed the new
Jitse_Giesen 17:358e7e1213cf 513 vector and new position (Pst) have to be calculated continiously*/
Jitse_Giesen 15:3a09783b2406 514 loop.attach(&Ve,0.02);
Jitse_Giesen 4:9e18e041f7a0 515 }
Jitse_Giesen 0:51a6e38a4d4a 516 return 0;
Jitse_Giesen 0:51a6e38a4d4a 517 }
Jitse_Giesen 15:3a09783b2406 518
Jitse_Giesen 0:51a6e38a4d4a 519 // Function which makes it possible to lower the end-effector to pick up a piece
Jitse_Giesen 15:3a09783b2406 520 void zakker()
Jitse_Giesen 15:3a09783b2406 521 {
Jitse_Giesen 17:358e7e1213cf 522 /*277.85 is the distance between the board and the bottom of the magnet */
Jitse_Giesen 15:3a09783b2406 523 dLod=sqrt(pow(Lou,2)+pow((277.85),2))-Lou;
Jitse_Giesen 15:3a09783b2406 524 dLbd=sqrt(pow(Lbu,2)+pow((277.85),2))-Lbu;
Jitse_Giesen 8:8e7c928aadc6 525 dLrd=sqrt(pow(Lru,2)+pow((277.85),2))-Lru;
Jitse_Giesen 0:51a6e38a4d4a 526 rotzo=dLod/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 527 rotzb=dLbd/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 528 rotzr=dLrd/omtrekklosje;
Jitse_Giesen 0:51a6e38a4d4a 529 countzo=rotzo*4200;
Jitse_Giesen 0:51a6e38a4d4a 530 countzb=rotzb*4200;
Jitse_Giesen 0:51a6e38a4d4a 531 countzr=rotzr*4200;
Jitse_Giesen 17:358e7e1213cf 532 /*first one (counto)differs from the other to due to the motor being on the
Jitse_Giesen 17:358e7e1213cf 533 opposite side of the pillar*/
Jitse_Giesen 17:358e7e1213cf 534 counto=countzo+hcounto+reference_o;
Jitse_Giesen 15:3a09783b2406 535 countb=-(reference_b-countzb-hcountb);
Jitse_Giesen 15:3a09783b2406 536 countr=-(reference_r-countzr-hcountr);
Jitse_Giesen 15:3a09783b2406 537 }
Jitse_Giesen 8:8e7c928aadc6 538
Jitse_Giesen 17:358e7e1213cf 539 //Checks if the threshold for the left biceps is reached
Jitse_Giesen 15:3a09783b2406 540 void zakken_threshold()
Jitse_Giesen 15:3a09783b2406 541 {
Jitse_Giesen 17:358e7e1213cf 542 if (Move_done == true) { //should only be executed when the move is done
Jitse_Giesen 15:3a09783b2406 543 if (emgBLcomplete > thresholdBL) {
Jitse_Giesen 15:3a09783b2406 544 zakker();
Jitse_Giesen 15:3a09783b2406 545 }
Jitse_Giesen 4:9e18e041f7a0 546 }
Jitse_Giesen 15:3a09783b2406 547 }
Jitse_Giesen 17:358e7e1213cf 548 /*Calculates the counts corrosponding with the set position (which is (0,0))*/
Jitse_Giesen 15:3a09783b2406 549 void setcurrentposition()
Jitse_Giesen 15:3a09783b2406 550 {
Jitse_Giesen 15:3a09783b2406 551 if(Input_done==true) {
Jitse_Giesen 4:9e18e041f7a0 552 hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje);
Jitse_Giesen 4:9e18e041f7a0 553 hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje);
Jitse_Giesen 4:9e18e041f7a0 554 hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje);
Jitse_Giesen 4:9e18e041f7a0 555 Input_done=false;
Jitse_Giesen 4:9e18e041f7a0 556 }
Jitse_Giesen 15:3a09783b2406 557 }
Jitse_Giesen 15:3a09783b2406 558
Jitse_Giesen 0:51a6e38a4d4a 559 int main()
Jitse_Giesen 0:51a6e38a4d4a 560 {
Jitse_Giesen 0:51a6e38a4d4a 561 pc.baud(115200);
Jitse_Giesen 17:358e7e1213cf 562 wait(1.0f);//Gives you one second between starting te program and calibrating
Jitse_Giesen 2:bf1c9d7afabd 563 getbqChain();
Jitse_Giesen 2:bf1c9d7afabd 564 threshold_timerR.attach(&Threshold_samplingBR, 0.002);
Jitse_Giesen 2:bf1c9d7afabd 565 threshold_timerL.attach(&Threshold_samplingBL, 0.002);
Jitse_Giesen 4:9e18e041f7a0 566 setcurrentposition();
Jitse_Giesen 15:3a09783b2406 567 while(true) {
Jitse_Giesen 4:9e18e041f7a0 568 sample_timer.attach(&EMG_sample, 0.002);
Jitse_Giesen 8:8e7c928aadc6 569 zakken_threshold();
Jitse_Giesen 18:7fb73aa6dbc0 570 wait(2.5f); /*To give the user time between calibration and input, and
Jitse_Giesen 18:7fb73aa6dbc0 571 allow the lowering to take place before new input is asked*/
Jitse_Giesen 18:7fb73aa6dbc0 572 tellerX();
Jitse_Giesen 0:51a6e38a4d4a 573 tellerY();
Jitse_Giesen 0:51a6e38a4d4a 574 calculator();
Jitse_Giesen 4:9e18e041f7a0 575 controlmotor1.attach(&MotorController1, 0.01);
Jitse_Giesen 4:9e18e041f7a0 576 controlmotor2.attach(&MotorController2, 0.01);
Jitse_Giesen 4:9e18e041f7a0 577 controlmotor3.attach(&MotorController3, 0.01);
Jitse_Giesen 18:7fb73aa6dbc0 578 wait(4.0f); /*To allow the move in the XY-plane to finish before
Jitse_Giesen 18:7fb73aa6dbc0 579 lowering can start*/
Jitse_Giesen 15:3a09783b2406 580 }
Jitse_Giesen 0:51a6e38a4d4a 581 }