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@16:cd11083c754a, 2017-11-06 (annotated)

Committer:
Jitse_Giesen
Date:
Mon Nov 06 17:32:09 2017 +0000
Revision:
16:cd11083c754a
Parent:
15:3a09783b2406
Child:
17:358e7e1213cf
netter;

Who changed what in which revision?

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