Joost Herijgers / Mbed 2 deprecated Code_MotorEMG

EMG and motor script together, Not fully working yet,

Dependencies:   Encoder QEI biquadFilter mbed

main.cpp@4:fddab1c875a9, 2017-10-26 (annotated)

Committer:
Joost38H
Date:
Thu Oct 26 11:32:53 2017 +0000
Revision:
4:fddab1c875a9
Parent:
3:59b504840b95
Child:
5:81d3b53087c0
threshold goed en voordat de rest uitgevoerd wordt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joost38H 0:eb16ed402ffa 1 #include "mbed.h"
Joost38H 0:eb16ed402ffa 2 #include "math.h"
Joost38H 0:eb16ed402ffa 3 #include "encoder.h"
Joost38H 0:eb16ed402ffa 4 #include "QEI.h"
Joost38H 0:eb16ed402ffa 5 #include "BiQuad.h"
Joost38H 0:eb16ed402ffa 6
Joost38H 0:eb16ed402ffa 7 Serial pc(USBTX, USBRX);
Joost38H 0:eb16ed402ffa 8
Joost38H 0:eb16ed402ffa 9 //Defining all in- and outputs
Joost38H 0:eb16ed402ffa 10 //EMG input
Joost38H 0:eb16ed402ffa 11 AnalogIn emgBR( A0 ); //Right Biceps
Joost38H 0:eb16ed402ffa 12 AnalogIn emgBL( A1 ); //Left Biceps
Joost38H 0:eb16ed402ffa 13
Joost38H 0:eb16ed402ffa 14 //Output motor 1 and reading Encoder motor 1
Joost38H 0:eb16ed402ffa 15 DigitalOut motor1DirectionPin(D4);
Joost38H 0:eb16ed402ffa 16 PwmOut motor1MagnitudePin(D5);
Joost38H 0:eb16ed402ffa 17 QEI Encoder1(D12,D13,NC,32);
Joost38H 0:eb16ed402ffa 18
Joost38H 0:eb16ed402ffa 19
Joost38H 0:eb16ed402ffa 20 //Output motor 2 and reading Encoder motor 2
Joost38H 0:eb16ed402ffa 21 DigitalOut motor2DirectionPin(D7);
Joost38H 0:eb16ed402ffa 22 PwmOut motor2MagnitudePin(D6);
Joost38H 0:eb16ed402ffa 23 QEI Encoder2(D10,D11,NC,32);
Joost38H 0:eb16ed402ffa 24
Joost38H 0:eb16ed402ffa 25 //LED output, needed for feedback
Joost38H 0:eb16ed402ffa 26 DigitalOut led_R(LED_RED);
Joost38H 0:eb16ed402ffa 27 DigitalOut led_G(LED_GREEN);
Joost38H 0:eb16ed402ffa 28 DigitalOut led_B(LED_BLUE);
Joost38H 0:eb16ed402ffa 29
Joost38H 1:6aac013b0ba3 30 //Setting Tickers for sampling EMG and determing if the threshold is met
Joost38H 4:fddab1c875a9 31 Ticker sample_timer;
Joost38H 4:fddab1c875a9 32 Ticker threshold_timerR;
Joost38H 4:fddab1c875a9 33 Ticker threshold_timerL;
Joost38H 4:fddab1c875a9 34
Joost38H 4:fddab1c875a9 35 Timer t_thresholdR;
Joost38H 4:fddab1c875a9 36 Timer t_thresholdL;
Joost38H 4:fddab1c875a9 37
Joost38H 4:fddab1c875a9 38 double currentTimeTR;
Joost38H 4:fddab1c875a9 39 double currentTimeTL;
Joost38H 1:6aac013b0ba3 40
Joost38H 0:eb16ed402ffa 41 InterruptIn button(SW2); // Wordt uiteindelijk vervangen door EMG
Joost38H 0:eb16ed402ffa 42
Joost38H 0:eb16ed402ffa 43 Timer t;
Joost38H 0:eb16ed402ffa 44 double speedfactor; // = 0.01; snelheid in, zonder potmeter gebruik <- waarom is dit zo?
Joost38H 0:eb16ed402ffa 45
Joost38H 0:eb16ed402ffa 46 // Getting the counts from the Encoder
Joost38H 0:eb16ed402ffa 47 int counts1 = Encoder1.getPulses();
Joost38H 0:eb16ed402ffa 48 int counts2 = Encoder2.getPulses();
Joost38H 0:eb16ed402ffa 49
Joost38H 0:eb16ed402ffa 50 // Defining variables delta (the difference between position and desired position) <- Is dit zo?
Joost38H 0:eb16ed402ffa 51 int delta1;
Joost38H 0:eb16ed402ffa 52 int delta2;
Joost38H 0:eb16ed402ffa 53
Joost38H 3:59b504840b95 54 bool Move_done = false;
Joost38H 3:59b504840b95 55
Joost38H 1:6aac013b0ba3 56 /* Defining all the different BiQuad filters, which contain a Notch filter,
Joost38H 1:6aac013b0ba3 57 High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
Joost38H 1:6aac013b0ba3 58 between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
Joost38H 1:6aac013b0ba3 59 and the Low-pass filter cancels out all frequencies below 4 Hz */
Joost38H 1:6aac013b0ba3 60
Joost38H 1:6aac013b0ba3 61 /* Defining all the normalized values of b and a in the Notch filter for the
Joost38H 1:6aac013b0ba3 62 creation of the Notch BiQuad */
Joost38H 1:6aac013b0ba3 63
Joost38H 4:fddab1c875a9 64 BiQuad bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Joost38H 4:fddab1c875a9 65 BiQuad bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Joost38H 4:fddab1c875a9 66
Joost38H 4:fddab1c875a9 67 BiQuad bqNotchTR( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Joost38H 4:fddab1c875a9 68 BiQuad bqNotchTL( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Joost38H 1:6aac013b0ba3 69
Joost38H 1:6aac013b0ba3 70 /* Defining all the normalized values of b and a in the High-pass filter for the
Joost38H 1:6aac013b0ba3 71 creation of the High-pass BiQuad */
Joost38H 1:6aac013b0ba3 72
Joost38H 4:fddab1c875a9 73 BiQuad bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Joost38H 4:fddab1c875a9 74 BiQuad bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Joost38H 4:fddab1c875a9 75
Joost38H 4:fddab1c875a9 76 BiQuad bqHighTR( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Joost38H 4:fddab1c875a9 77 BiQuad bqHighTL( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Joost38H 1:6aac013b0ba3 78
Joost38H 1:6aac013b0ba3 79 /* Defining all the normalized values of b and a in the Low-pass filter for the
Joost38H 1:6aac013b0ba3 80 creation of the Low-pass BiQuad */
Joost38H 1:6aac013b0ba3 81
Joost38H 4:fddab1c875a9 82 BiQuad bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Joost38H 4:fddab1c875a9 83 BiQuad bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Joost38H 4:fddab1c875a9 84
Joost38H 4:fddab1c875a9 85 BiQuad bqLowTR( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Joost38H 4:fddab1c875a9 86 BiQuad bqLowTL( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Joost38H 1:6aac013b0ba3 87
Joost38H 1:6aac013b0ba3 88 // Creating a variable needed for the creation of the BiQuadChain
Joost38H 4:fddab1c875a9 89 BiQuadChain bqChain1;
Joost38H 4:fddab1c875a9 90 BiQuadChain bqChain2;
Joost38H 1:6aac013b0ba3 91
Joost38H 4:fddab1c875a9 92 BiQuadChain bqChainTR;
Joost38H 4:fddab1c875a9 93 BiQuadChain bqChainTL;
Joost38H 1:6aac013b0ba3 94
Joost38H 4:fddab1c875a9 95 //Defining all doubles needed in the filtering process
Joost38H 1:6aac013b0ba3 96 double emgBRfiltered; //Right biceps Notch+High pass filter
Joost38H 1:6aac013b0ba3 97 double emgBRrectified; //Right biceps rectified
Joost38H 1:6aac013b0ba3 98 double emgBRcomplete; //Right biceps low-pass filter, filtering complete
Joost38H 1:6aac013b0ba3 99
Joost38H 1:6aac013b0ba3 100 double emgBLfiltered; //Left biceps Notch+High pass filter
Joost38H 1:6aac013b0ba3 101 double emgBLrectified; //Left biceps rectified
Joost38H 1:6aac013b0ba3 102 double emgBLcomplete; //Left biceps low-pass filter, filtering complete
Joost38H 1:6aac013b0ba3 103
Joost38H 1:6aac013b0ba3 104 int countBR = 0;
Joost38H 1:6aac013b0ba3 105 int countBL = 0;
Joost38H 1:6aac013b0ba3 106
Joost38H 4:fddab1c875a9 107 //double threshold = 0.03;
Joost38H 4:fddab1c875a9 108
Joost38H 1:6aac013b0ba3 109 double numsamples = 500;
Joost38H 1:6aac013b0ba3 110 double emgBRsum = 0;
Joost38H 1:6aac013b0ba3 111 double emgBRmeanMVC;
Joost38H 1:6aac013b0ba3 112 double thresholdBR;
Joost38H 1:6aac013b0ba3 113
Joost38H 1:6aac013b0ba3 114 double emgBLsum = 0;
Joost38H 1:6aac013b0ba3 115 double emgBLmeanMVC;
Joost38H 1:6aac013b0ba3 116 double thresholdBL;
Joost38H 1:6aac013b0ba3 117
Joost38H 4:fddab1c875a9 118 /* Function to sample the EMG and get a Threshold value from it,
Joost38H 4:fddab1c875a9 119 which can be used throughout the process */
Joost38H 4:fddab1c875a9 120
Joost38H 4:fddab1c875a9 121 void Threshold_samplingBR() {
Joost38H 4:fddab1c875a9 122 t_thresholdR.start();
Joost38H 4:fddab1c875a9 123 currentTimeTR = t_thresholdR.read();
Joost38H 4:fddab1c875a9 124
Joost38H 4:fddab1c875a9 125 if (currentTimeTR <= 1) {
Joost38H 4:fddab1c875a9 126
Joost38H 4:fddab1c875a9 127 emgBRfiltered = bqChainTR.step( emgBR.read() ); //Notch+High-pass
Joost38H 4:fddab1c875a9 128 emgBRrectified = fabs(emgBRfiltered); //Rectification
Joost38H 4:fddab1c875a9 129 emgBRcomplete = bqLowTR.step(emgBRrectified); //Low-pass
Joost38H 4:fddab1c875a9 130
Joost38H 4:fddab1c875a9 131 emgBRsum = emgBRsum + emgBRcomplete;
Joost38H 4:fddab1c875a9 132 }
Joost38H 4:fddab1c875a9 133 emgBRmeanMVC = emgBRsum/numsamples;
Joost38H 4:fddab1c875a9 134 thresholdBR = emgBRmeanMVC * 0.20;
Joost38H 4:fddab1c875a9 135
Joost38H 4:fddab1c875a9 136
Joost38H 4:fddab1c875a9 137
Joost38H 4:fddab1c875a9 138 //pc.printf("ThresholdBR = %f \n", thresholdBR);
Joost38H 4:fddab1c875a9 139 }
Joost38H 4:fddab1c875a9 140 void Threshold_samplingBL() {
Joost38H 4:fddab1c875a9 141 t_thresholdL.start();
Joost38H 4:fddab1c875a9 142 currentTimeTL = t_thresholdL.read();
Joost38H 4:fddab1c875a9 143
Joost38H 4:fddab1c875a9 144 if (currentTimeTL <= 1) {
Joost38H 4:fddab1c875a9 145
Joost38H 4:fddab1c875a9 146 emgBLfiltered = bqChain2.step( emgBL.read() ); //Notch+High-pass
Joost38H 4:fddab1c875a9 147 emgBLrectified = fabs( emgBLfiltered ); //Rectification
Joost38H 4:fddab1c875a9 148 emgBLcomplete = bqLow2.step( emgBLrectified ); //Low-pass
Joost38H 4:fddab1c875a9 149
Joost38H 4:fddab1c875a9 150 emgBLsum = emgBLsum + emgBLcomplete;
Joost38H 4:fddab1c875a9 151 }
Joost38H 4:fddab1c875a9 152
Joost38H 4:fddab1c875a9 153 emgBLmeanMVC = emgBLsum/numsamples;
Joost38H 4:fddab1c875a9 154 thresholdBL = emgBLmeanMVC * 0.20;
Joost38H 4:fddab1c875a9 155
Joost38H 4:fddab1c875a9 156 }
Joost38H 4:fddab1c875a9 157
Joost38H 1:6aac013b0ba3 158 // EMG functions
Joost38H 1:6aac013b0ba3 159 void EMG_sample()
Joost38H 1:6aac013b0ba3 160 {
Joost38H 1:6aac013b0ba3 161 //Filtering steps for the Right Biceps EMG
Joost38H 1:6aac013b0ba3 162 emgBRfiltered = bqChain1.step( emgBR.read() ); //Notch+High-pass
Joost38H 1:6aac013b0ba3 163 emgBRrectified = fabs(emgBRfiltered); //Rectification
Joost38H 1:6aac013b0ba3 164 emgBRcomplete = bqLow1.step(emgBRrectified); //Low-pass
Joost38H 1:6aac013b0ba3 165
Joost38H 1:6aac013b0ba3 166
Joost38H 2:2c4ee76dc830 167 /*Getting threshold value for Right Biceps, a value of 20% of
Joost38H 1:6aac013b0ba3 168 Maximum Voluntary Contraction is chosen as threshold value */
Joost38H 4:fddab1c875a9 169 /*if (countBR < numsamples) {
Joost38H 1:6aac013b0ba3 170 emgBRsum = emgBRsum + emgBRcomplete;
Joost38H 1:6aac013b0ba3 171 countBR++;
Joost38H 2:2c4ee76dc830 172 led_R = 0;
Joost38H 2:2c4ee76dc830 173 led_B = 0;
Joost38H 2:2c4ee76dc830 174 led_G = 1;
Joost38H 1:6aac013b0ba3 175 }
Joost38H 1:6aac013b0ba3 176
Joost38H 1:6aac013b0ba3 177 emgBRmeanMVC = emgBRsum / numsamples;
Joost38H 1:6aac013b0ba3 178
Joost38H 4:fddab1c875a9 179 thresholdBR = emgBRmeanMVC * 0.25;*/
Joost38H 2:2c4ee76dc830 180
Joost38H 1:6aac013b0ba3 181 //Filtering steps for the Left Biceps EMG
Joost38H 1:6aac013b0ba3 182 emgBLfiltered = bqChain2.step( emgBL.read() ); //Notch+High-pass
Joost38H 1:6aac013b0ba3 183 emgBLrectified = fabs( emgBLfiltered ); //Rectification
Joost38H 1:6aac013b0ba3 184 emgBLcomplete = bqLow2.step( emgBLrectified ); //Low-pass
Joost38H 1:6aac013b0ba3 185
Joost38H 2:2c4ee76dc830 186
Joost38H 2:2c4ee76dc830 187 /*Getting threshold value for Left Biceps, a value of 20% of
Joost38H 1:6aac013b0ba3 188 Maximum Voluntary Contraction is chosen as threshold value */
Joost38H 4:fddab1c875a9 189 /*if (countBL < numsamples) {
Joost38H 1:6aac013b0ba3 190 emgBLsum = emgBLsum + emgBLcomplete;
Joost38H 1:6aac013b0ba3 191 countBL++;
Joost38H 1:6aac013b0ba3 192 }
Joost38H 1:6aac013b0ba3 193
Joost38H 1:6aac013b0ba3 194 emgBLmeanMVC = emgBLsum / numsamples;
Joost38H 1:6aac013b0ba3 195
Joost38H 2:2c4ee76dc830 196 thresholdBL = emgBLmeanMVC * 0.25;
Joost38H 2:2c4ee76dc830 197
Joost38H 4:fddab1c875a9 198 //pc.printf("ThresholdBR = %0.3f, ThresholdBL = %0.3f \n", thresholdBR,thresholdBL);*/
Joost38H 1:6aac013b0ba3 199 }
Joost38H 1:6aac013b0ba3 200
Joost38H 1:6aac013b0ba3 201 // Function to make the BiQuadChain for the Notch and High pass filter for both filters
Joost38H 1:6aac013b0ba3 202 void getbqChain()
Joost38H 1:6aac013b0ba3 203 {
Joost38H 1:6aac013b0ba3 204 bqChain1.add(&bqNotch1).add(&bqHigh1);
Joost38H 1:6aac013b0ba3 205 bqChain2.add(&bqNotch2).add(&bqHigh2);
Joost38H 1:6aac013b0ba3 206 }
Joost38H 1:6aac013b0ba3 207
Joost38H 0:eb16ed402ffa 208 // Initial input value for X
Joost38H 3:59b504840b95 209 int Xin=0;
Joost38H 3:59b504840b95 210 int Xin_new; //<- Hoe zit het met deze als we de robot daadwerkelijk gebruiken
Joost38H 0:eb16ed402ffa 211 double huidigetijdX;
Joost38H 0:eb16ed402ffa 212
Joost38H 0:eb16ed402ffa 213 // Feedback system for counting values of X
Joost38H 0:eb16ed402ffa 214 void ledtX(){
Joost38H 0:eb16ed402ffa 215 t.reset();
Joost38H 0:eb16ed402ffa 216 Xin++;
Joost38H 0:eb16ed402ffa 217 pc.printf("Xin is %i\n",Xin);
Joost38H 0:eb16ed402ffa 218 led_G=0;
Joost38H 0:eb16ed402ffa 219 led_R=1;
Joost38H 2:2c4ee76dc830 220 wait(0.2);
Joost38H 0:eb16ed402ffa 221 led_G=1;
Joost38H 0:eb16ed402ffa 222 led_R=0;
Joost38H 2:2c4ee76dc830 223 wait(0.5);
Joost38H 0:eb16ed402ffa 224 }
Joost38H 0:eb16ed402ffa 225
Joost38H 0:eb16ed402ffa 226
Joost38H 0:eb16ed402ffa 227 // Couting system for values of X
Joost38H 0:eb16ed402ffa 228 int tellerX(){
Joost38H 3:59b504840b95 229 if (Move_done == true) {
Joost38H 3:59b504840b95 230 t.reset();
Joost38H 3:59b504840b95 231 led_G=1;
Joost38H 3:59b504840b95 232 led_B=1;
Joost38H 3:59b504840b95 233 led_R=0;
Joost38H 0:eb16ed402ffa 234 while(true){
Joost38H 3:59b504840b95 235 button.fall(ledtX); //This has to be replaced by EMG
Joost38H 3:59b504840b95 236 if (emgBRcomplete > thresholdBR){
Joost38H 0:eb16ed402ffa 237 ledtX(); // dit is wat je uiteindelijk wil dat er staat
Joost38H 2:2c4ee76dc830 238 }
Joost38H 3:59b504840b95 239
Joost38H 3:59b504840b95 240 t.start();
Joost38H 3:59b504840b95 241 huidigetijdX=t.read();
Joost38H 3:59b504840b95 242 if (huidigetijdX>2){
Joost38H 3:59b504840b95 243 led_R=1; //Go to the next program (couting values for Y)
Joost38H 3:59b504840b95 244 Xin_new = Xin;
Joost38H 3:59b504840b95 245 Xin = 0;
Joost38H 3:59b504840b95 246 //button.fall(0); // Wat is deze?
Joost38H 3:59b504840b95 247
Joost38H 3:59b504840b95 248 return Xin_new;
Joost38H 3:59b504840b95 249 }
Joost38H 3:59b504840b95 250
Joost38H 3:59b504840b95 251 }
Joost38H 3:59b504840b95 252
Joost38H 3:59b504840b95 253 }
Joost38H 3:59b504840b95 254 return 0;
Joost38H 0:eb16ed402ffa 255 }
Joost38H 0:eb16ed402ffa 256
Joost38H 0:eb16ed402ffa 257 // Initial values needed for Y (see comments at X function)
Joost38H 0:eb16ed402ffa 258 int Yin=0;
Joost38H 3:59b504840b95 259 int Yin_new;
Joost38H 0:eb16ed402ffa 260 double huidigetijdY;
Joost38H 0:eb16ed402ffa 261
Joost38H 0:eb16ed402ffa 262 //Feedback system for couting values of Y
Joost38H 0:eb16ed402ffa 263 void ledtY(){
Joost38H 0:eb16ed402ffa 264 t.reset();
Joost38H 0:eb16ed402ffa 265 Yin++;
Joost38H 0:eb16ed402ffa 266 pc.printf("Yin is %i\n",Yin);
Joost38H 0:eb16ed402ffa 267 led_G=0;
Joost38H 0:eb16ed402ffa 268 led_B=1;
Joost38H 2:2c4ee76dc830 269 wait(0.2);
Joost38H 0:eb16ed402ffa 270 led_G=1;
Joost38H 0:eb16ed402ffa 271 led_B=0;
Joost38H 2:2c4ee76dc830 272 wait(0.5);
Joost38H 0:eb16ed402ffa 273 }
Joost38H 0:eb16ed402ffa 274
Joost38H 0:eb16ed402ffa 275 // Couting system for values of Y
Joost38H 0:eb16ed402ffa 276 int tellerY(){
Joost38H 3:59b504840b95 277 if (Move_done == true) {
Joost38H 0:eb16ed402ffa 278 t.reset();
Joost38H 0:eb16ed402ffa 279 led_G=1;
Joost38H 0:eb16ed402ffa 280 led_B=0;
Joost38H 2:2c4ee76dc830 281 led_R=1;
Joost38H 0:eb16ed402ffa 282 while(true){
Joost38H 1:6aac013b0ba3 283 //button.fall(ledtY); //See comments at X
Joost38H 1:6aac013b0ba3 284 if (emgBRcomplete > thresholdBR){
Joost38H 1:6aac013b0ba3 285 ledtY(); // dit is wat je uiteindelijk wil dat er staat
Joost38H 3:59b504840b95 286 }
Joost38H 0:eb16ed402ffa 287 t.start();
Joost38H 0:eb16ed402ffa 288 huidigetijdY=t.read();
Joost38H 0:eb16ed402ffa 289 if (huidigetijdY>2){
Joost38H 0:eb16ed402ffa 290 led_B=1;
Joost38H 3:59b504840b95 291 Yin_new = Yin;
Joost38H 3:59b504840b95 292 Yin = 0;
Joost38H 3:59b504840b95 293 //if (emgBRcomplete > thresholdBR){
Joost38H 3:59b504840b95 294 //0; // dit is wat je uiteindelijk wil dat er staat
Joost38H 3:59b504840b95 295 //}
Joost38H 3:59b504840b95 296 //button.fall(0); // Wat is deze?
Joost38H 3:59b504840b95 297 Move_done = false;
Joost38H 3:59b504840b95 298 return Yin_new;
Joost38H 2:2c4ee76dc830 299
Joost38H 0:eb16ed402ffa 300 }
Joost38H 0:eb16ed402ffa 301 }
Joost38H 3:59b504840b95 302 }
Joost38H 3:59b504840b95 303 return 0; // ga door naar het volgende programma
Joost38H 0:eb16ed402ffa 304 }
Joost38H 0:eb16ed402ffa 305
Joost38H 3:59b504840b95 306
Joost38H 0:eb16ed402ffa 307
Joost38H 0:eb16ed402ffa 308 // Declaring all variables needed for calculating rope lengths,
Joost38H 0:eb16ed402ffa 309 double Pox = 0;
Joost38H 0:eb16ed402ffa 310 double Poy = 0;
Joost38H 0:eb16ed402ffa 311 double Pbx = 0;
Joost38H 0:eb16ed402ffa 312 double Pby = 887;
Joost38H 0:eb16ed402ffa 313 double Prx = 768;
Joost38H 0:eb16ed402ffa 314 double Pry = 443;
Joost38H 0:eb16ed402ffa 315 double Pex=121;
Joost38H 0:eb16ed402ffa 316 double Pey=308;
Joost38H 0:eb16ed402ffa 317 double diamtrklosje=20;
Joost38H 0:eb16ed402ffa 318 double pi=3.14159265359;
Joost38H 0:eb16ed402ffa 319 double omtrekklosje=diamtrklosje*pi;
Joost38H 0:eb16ed402ffa 320 double Lou;
Joost38H 0:eb16ed402ffa 321 double Lbu;
Joost38H 0:eb16ed402ffa 322 double Lru;
Joost38H 0:eb16ed402ffa 323 double dLod;
Joost38H 0:eb16ed402ffa 324 double dLbd;
Joost38H 0:eb16ed402ffa 325 double dLrd;
Joost38H 0:eb16ed402ffa 326
Joost38H 0:eb16ed402ffa 327 // Declaring variables needed for calculating motor counts
Joost38H 0:eb16ed402ffa 328 double roto;
Joost38H 0:eb16ed402ffa 329 double rotb;
Joost38H 0:eb16ed402ffa 330 double rotr;
Joost38H 0:eb16ed402ffa 331 double rotzo;
Joost38H 0:eb16ed402ffa 332 double rotzb;
Joost38H 0:eb16ed402ffa 333 double rotzr;
Joost38H 0:eb16ed402ffa 334 double counto;
Joost38H 0:eb16ed402ffa 335 double countb;
Joost38H 0:eb16ed402ffa 336 double countr;
Joost38H 0:eb16ed402ffa 337 double countzo;
Joost38H 0:eb16ed402ffa 338 double countzb;
Joost38H 0:eb16ed402ffa 339 double countzr;
Joost38H 0:eb16ed402ffa 340
Joost38H 0:eb16ed402ffa 341 // Declaring variables neeeded for calculating motor movements to get to a certain point <- klopt dit?
Joost38H 0:eb16ed402ffa 342 double Psx;
Joost38H 0:eb16ed402ffa 343 double Psy;
Joost38H 0:eb16ed402ffa 344 double Vex;
Joost38H 0:eb16ed402ffa 345 double Vey;
Joost38H 0:eb16ed402ffa 346 double Kz=0.7; // nadersnelheid instellen
Joost38H 0:eb16ed402ffa 347 double modVe;
Joost38H 0:eb16ed402ffa 348 double Vmax=20;
Joost38H 0:eb16ed402ffa 349 double Pstx;
Joost38H 0:eb16ed402ffa 350 double Psty;
Joost38H 0:eb16ed402ffa 351 double T=0.02;//seconds
Joost38H 0:eb16ed402ffa 352
Joost38H 0:eb16ed402ffa 353
Joost38H 0:eb16ed402ffa 354 //Deel om motor(en) aan te sturen--------------------------------------------
Joost38H 0:eb16ed402ffa 355
Joost38H 0:eb16ed402ffa 356 void calcdelta1() {
Joost38H 0:eb16ed402ffa 357 delta1 = (counto - Encoder1.getPulses());
Joost38H 0:eb16ed402ffa 358 }
Joost38H 0:eb16ed402ffa 359
Joost38H 0:eb16ed402ffa 360 void calcdelta2() {
Joost38H 2:2c4ee76dc830 361 delta2 = (countb - Encoder2.getPulses()); // <------- de reden dat de delta negatief is (jitse)
Joost38H 0:eb16ed402ffa 362 }
Joost38H 0:eb16ed402ffa 363
Joost38H 0:eb16ed402ffa 364 double referenceVelocity1;
Joost38H 0:eb16ed402ffa 365 double motorValue1;
Joost38H 0:eb16ed402ffa 366
Joost38H 0:eb16ed402ffa 367 double referenceVelocity2;
Joost38H 0:eb16ed402ffa 368 double motorValue2;
Joost38H 0:eb16ed402ffa 369
Joost38H 0:eb16ed402ffa 370 Ticker controlmotor1; // één ticker van maken?
Joost38H 0:eb16ed402ffa 371 Ticker calculatedelta1;
Joost38H 0:eb16ed402ffa 372 Ticker printdata1; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
Joost38H 0:eb16ed402ffa 373
Joost38H 0:eb16ed402ffa 374 Ticker controlmotor2; // één ticker van maken?
Joost38H 0:eb16ed402ffa 375 Ticker calculatedelta2;
Joost38H 0:eb16ed402ffa 376 Ticker printdata2; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
Joost38H 0:eb16ed402ffa 377
Joost38H 0:eb16ed402ffa 378 double GetReferenceVelocity1()
Joost38H 0:eb16ed402ffa 379 {
Joost38H 0:eb16ed402ffa 380 // Returns reference velocity in rad/s. Positive value means clockwise rotation.
Joost38H 0:eb16ed402ffa 381 double maxVelocity1=Vex*25+Vey*25; // max 8.4 in rad/s of course!
Joost38H 0:eb16ed402ffa 382 referenceVelocity1 = (-1)*speedfactor * maxVelocity1;
Joost38H 0:eb16ed402ffa 383
Joost38H 0:eb16ed402ffa 384 if (Encoder1.getPulses() < (counto+1))
Joost38H 0:eb16ed402ffa 385 { speedfactor = 0.1;
Joost38H 0:eb16ed402ffa 386 }
Joost38H 0:eb16ed402ffa 387 else if (Encoder1.getPulses() > (counto-1))
Joost38H 0:eb16ed402ffa 388 { speedfactor = -0.1;
Joost38H 0:eb16ed402ffa 389 }
Joost38H 0:eb16ed402ffa 390 else
Joost38H 0:eb16ed402ffa 391 { speedfactor = 0;
Joost38H 0:eb16ed402ffa 392 }
Joost38H 0:eb16ed402ffa 393
Joost38H 0:eb16ed402ffa 394 return referenceVelocity1;
Joost38H 0:eb16ed402ffa 395 }
Joost38H 0:eb16ed402ffa 396
Joost38H 0:eb16ed402ffa 397 double GetReferenceVelocity2()
Joost38H 0:eb16ed402ffa 398 {
Joost38H 0:eb16ed402ffa 399 // Returns reference velocity in rad/s. Positive value means clockwise rotation.
Joost38H 0:eb16ed402ffa 400 double maxVelocity2=Vex*25+Vey*25; // max 8.4 in rad/s of course!
Joost38H 0:eb16ed402ffa 401 referenceVelocity2 = (-1)*speedfactor * maxVelocity2;
Joost38H 0:eb16ed402ffa 402
Joost38H 0:eb16ed402ffa 403 if (Encoder2.getPulses() < (counto+1))
Joost38H 0:eb16ed402ffa 404 { speedfactor = 0.1;
Joost38H 0:eb16ed402ffa 405 }
Joost38H 0:eb16ed402ffa 406 else if (Encoder2.getPulses() > (counto-1))
Joost38H 0:eb16ed402ffa 407 { speedfactor = -0.1;
Joost38H 0:eb16ed402ffa 408 }
Joost38H 0:eb16ed402ffa 409 else
Joost38H 0:eb16ed402ffa 410 { speedfactor = 0;
Joost38H 0:eb16ed402ffa 411 }
Joost38H 0:eb16ed402ffa 412
Joost38H 0:eb16ed402ffa 413 return referenceVelocity2;
Joost38H 0:eb16ed402ffa 414 }
Joost38H 0:eb16ed402ffa 415
Joost38H 0:eb16ed402ffa 416 void SetMotor1(double motorValue1)
Joost38H 0:eb16ed402ffa 417 {
Joost38H 0:eb16ed402ffa 418 // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
Joost38H 0:eb16ed402ffa 419 // motor rotating clockwise. motorValues outside range are truncated to within range
Joost38H 0:eb16ed402ffa 420 if (motorValue1 >=0) motor1DirectionPin=1;
Joost38H 0:eb16ed402ffa 421 else motor1DirectionPin=0;
Joost38H 0:eb16ed402ffa 422 if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
Joost38H 0:eb16ed402ffa 423 else motor1MagnitudePin = fabs(motorValue1);
Joost38H 0:eb16ed402ffa 424
Joost38H 0:eb16ed402ffa 425 }
Joost38H 0:eb16ed402ffa 426
Joost38H 0:eb16ed402ffa 427 void SetMotor2(double motorValue2)
Joost38H 0:eb16ed402ffa 428 {
Joost38H 0:eb16ed402ffa 429 // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
Joost38H 0:eb16ed402ffa 430 // motor rotating clockwise. motorValues outside range are truncated to within range
Joost38H 0:eb16ed402ffa 431 if (motorValue2 >=0) motor2DirectionPin=1;
Joost38H 0:eb16ed402ffa 432 else motor2DirectionPin=0;
Joost38H 0:eb16ed402ffa 433 if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
Joost38H 0:eb16ed402ffa 434 else motor2MagnitudePin = fabs(motorValue2);
Joost38H 0:eb16ed402ffa 435
Joost38H 0:eb16ed402ffa 436 }
Joost38H 0:eb16ed402ffa 437
Joost38H 0:eb16ed402ffa 438 double FeedForwardControl1(double referenceVelocity1)
Joost38H 0:eb16ed402ffa 439 {
Joost38H 0:eb16ed402ffa 440 // very simple linear feed-forward control
Joost38H 0:eb16ed402ffa 441 const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
Joost38H 0:eb16ed402ffa 442 double motorValue1 = referenceVelocity1 / MotorGain;
Joost38H 0:eb16ed402ffa 443 return motorValue1;
Joost38H 0:eb16ed402ffa 444 }
Joost38H 0:eb16ed402ffa 445
Joost38H 0:eb16ed402ffa 446 double FeedForwardControl2(double referenceVelocity2)
Joost38H 0:eb16ed402ffa 447 {
Joost38H 0:eb16ed402ffa 448 // very simple linear feed-forward control
Joost38H 0:eb16ed402ffa 449 const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
Joost38H 0:eb16ed402ffa 450 double motorValue2 = referenceVelocity2 / MotorGain;
Joost38H 0:eb16ed402ffa 451 return motorValue2;
Joost38H 0:eb16ed402ffa 452 }
Joost38H 0:eb16ed402ffa 453
Joost38H 0:eb16ed402ffa 454 void MeasureAndControl1()
Joost38H 0:eb16ed402ffa 455 {
Joost38H 0:eb16ed402ffa 456 // This function measures the potmeter position, extracts a reference velocity from it,
Joost38H 0:eb16ed402ffa 457 // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
Joost38H 0:eb16ed402ffa 458 double referenceVelocity1 = GetReferenceVelocity1();
Joost38H 0:eb16ed402ffa 459 double motorValue1 = FeedForwardControl1(referenceVelocity1);
Joost38H 0:eb16ed402ffa 460 SetMotor1(motorValue1);
Joost38H 0:eb16ed402ffa 461 }
Joost38H 0:eb16ed402ffa 462
Joost38H 0:eb16ed402ffa 463 void MeasureAndControl2()
Joost38H 0:eb16ed402ffa 464 {
Joost38H 0:eb16ed402ffa 465 // This function measures the potmeter position, extracts a reference velocity from it,
Joost38H 0:eb16ed402ffa 466 // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
Joost38H 0:eb16ed402ffa 467 double referenceVelocity2 = GetReferenceVelocity2();
Joost38H 0:eb16ed402ffa 468 double motorValue2 = FeedForwardControl2(referenceVelocity2);
Joost38H 0:eb16ed402ffa 469 SetMotor2(motorValue2);
Joost38H 0:eb16ed402ffa 470 }
Joost38H 0:eb16ed402ffa 471
Joost38H 0:eb16ed402ffa 472 void readdata1()
Joost38H 0:eb16ed402ffa 473 { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
Joost38H 0:eb16ed402ffa 474 pc.printf("Pulses_M1 = %i \r\n",Encoder1.getPulses());
Joost38H 0:eb16ed402ffa 475 //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
Joost38H 0:eb16ed402ffa 476 pc.printf("Delta_M1 = %i \r\n",delta1);
Joost38H 0:eb16ed402ffa 477 }
Joost38H 0:eb16ed402ffa 478
Joost38H 0:eb16ed402ffa 479 void readdata2()
Joost38H 0:eb16ed402ffa 480 { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
Joost38H 0:eb16ed402ffa 481 pc.printf("Pulses_M2 = %i \r\n",Encoder2.getPulses());
Joost38H 0:eb16ed402ffa 482 //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
Joost38H 0:eb16ed402ffa 483 pc.printf("Delta_M2 = %i \r\n",delta2);
Joost38H 0:eb16ed402ffa 484 }
Joost38H 0:eb16ed402ffa 485
Joost38H 0:eb16ed402ffa 486 // einde deel motor------------------------------------------------------------------------------------
Joost38H 0:eb16ed402ffa 487
Joost38H 0:eb16ed402ffa 488 Ticker loop;
Joost38H 0:eb16ed402ffa 489
Joost38H 0:eb16ed402ffa 490 /*Calculates ropelengths that are needed to get to new positions, based on the
Joost38H 0:eb16ed402ffa 491 set coordinates and the position of the poles */
Joost38H 0:eb16ed402ffa 492 double touwlengtes(){
Joost38H 0:eb16ed402ffa 493 Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));
Joost38H 0:eb16ed402ffa 494 Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2));
Joost38H 0:eb16ed402ffa 495 Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
Joost38H 0:eb16ed402ffa 496 return 0;
Joost38H 0:eb16ed402ffa 497 }
Joost38H 0:eb16ed402ffa 498
Joost38H 0:eb16ed402ffa 499 /* Calculates rotations (and associated counts) of the motor to get to the
Joost38H 0:eb16ed402ffa 500 desired new position*/
Joost38H 0:eb16ed402ffa 501 double turns(){
Joost38H 0:eb16ed402ffa 502
Joost38H 0:eb16ed402ffa 503 roto=Lou/omtrekklosje;
Joost38H 0:eb16ed402ffa 504 rotb=Lbu/omtrekklosje;
Joost38H 0:eb16ed402ffa 505 rotr=Lru/omtrekklosje;
Joost38H 0:eb16ed402ffa 506 counto=roto*4200;
Joost38H 0:eb16ed402ffa 507 //counto = (int)(counto + 0.5); // omzetten van rotaties naar counts
Joost38H 0:eb16ed402ffa 508 countb=rotb*4200;
Joost38H 0:eb16ed402ffa 509 //countb = (int)(countb + 0.5);
Joost38H 0:eb16ed402ffa 510 countr=rotr*4200;
Joost38H 0:eb16ed402ffa 511 //countr = (int)(countr + 0.5);
Joost38H 0:eb16ed402ffa 512 return 0;
Joost38H 0:eb16ed402ffa 513 }
Joost38H 0:eb16ed402ffa 514
Joost38H 0:eb16ed402ffa 515 // Waar komen Pstx en Psty vandaan en waar staan ze voor? En is dit maar voor een paal?
Joost38H 0:eb16ed402ffa 516 double Pst(){
Joost38H 0:eb16ed402ffa 517 Pstx=Pex+Vex*T;
Joost38H 0:eb16ed402ffa 518 Psty=Pey+Vey*T;
Joost38H 0:eb16ed402ffa 519 touwlengtes();
Joost38H 0:eb16ed402ffa 520 Pex=Pstx;
Joost38H 0:eb16ed402ffa 521 Pey=Psty;
Joost38H 0:eb16ed402ffa 522 //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
Joost38H 0:eb16ed402ffa 523 //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru);
Joost38H 0:eb16ed402ffa 524 turns();
Joost38H 0:eb16ed402ffa 525 //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr);
Joost38H 0:eb16ed402ffa 526 pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
Joost38H 0:eb16ed402ffa 527 /*float R;
Joost38H 0:eb16ed402ffa 528 R=Vex/Vey; // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constant is en de end efector dus een rechte lijn maakt
Joost38H 0:eb16ed402ffa 529 pc.printf("\n\r R=%f",R);*/
Joost38H 0:eb16ed402ffa 530 return 0;
Joost38H 0:eb16ed402ffa 531 }
Joost38H 0:eb16ed402ffa 532
Joost38H 0:eb16ed402ffa 533 //Calculating desired end position based on the EMG input <- Waarom maar voor een paal?
Joost38H 0:eb16ed402ffa 534 double Ps(){
Joost38H 3:59b504840b95 535 Psx=(Xin_new)*30+121;
Joost38H 3:59b504840b95 536 Psy=(Yin_new)*30+308;
Joost38H 0:eb16ed402ffa 537 //pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
Joost38H 0:eb16ed402ffa 538 return 0;
Joost38H 0:eb16ed402ffa 539 }
Joost38H 0:eb16ed402ffa 540
Joost38H 0:eb16ed402ffa 541 // Rekent dit de snelheid uit waarmee de motoren bewegen?
Joost38H 0:eb16ed402ffa 542 void Ve(){
Joost38H 0:eb16ed402ffa 543 Vex=Kz*(Psx-Pex);
Joost38H 0:eb16ed402ffa 544 Vey=Kz*(Psy-Pey);
Joost38H 0:eb16ed402ffa 545 modVe=sqrt(pow(Vex,2)+pow(Vey,2));
Joost38H 0:eb16ed402ffa 546 if(modVe>Vmax){
Joost38H 0:eb16ed402ffa 547 Vex=(Vex/modVe)*Vmax;
Joost38H 0:eb16ed402ffa 548 Vey=(Vey/modVe)*Vmax;
Joost38H 0:eb16ed402ffa 549 }
Joost38H 0:eb16ed402ffa 550 Pst();
Joost38H 0:eb16ed402ffa 551 pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
Joost38H 0:eb16ed402ffa 552 if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
Joost38H 3:59b504840b95 553 Move_done=true;
Joost38H 0:eb16ed402ffa 554 loop.detach();
Joost38H 0:eb16ed402ffa 555 }
Joost38H 0:eb16ed402ffa 556 }
Joost38H 0:eb16ed402ffa 557
Joost38H 0:eb16ed402ffa 558 // Calculating the desired position, so that the motors can go here
Joost38H 0:eb16ed402ffa 559 int calculator(){
Joost38H 0:eb16ed402ffa 560 Ps();
Joost38H 0:eb16ed402ffa 561 loop.attach(&Ve,0.02);
Joost38H 0:eb16ed402ffa 562 return 0;
Joost38H 0:eb16ed402ffa 563 }
Joost38H 0:eb16ed402ffa 564
Joost38H 0:eb16ed402ffa 565 // Function which makes it possible to lower the end-effector to pick up a piece
Joost38H 0:eb16ed402ffa 566 void zakker(){
Joost38H 0:eb16ed402ffa 567 while(1){
Joost38H 0:eb16ed402ffa 568 wait(1);
Joost38H 3:59b504840b95 569 if(Move_done==true){ //misschien moet je hier als voorwaarden een delta is 1 zetten // hierdoor wacht dit programma totdat de beweging klaar is
Joost38H 0:eb16ed402ffa 570 dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken
Joost38H 0:eb16ed402ffa 571 dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
Joost38H 0:eb16ed402ffa 572 dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
Joost38H 0:eb16ed402ffa 573 rotzo=dLod/omtrekklosje;
Joost38H 0:eb16ed402ffa 574 rotzb=dLbd/omtrekklosje;
Joost38H 0:eb16ed402ffa 575 rotzr=dLrd/omtrekklosje;
Joost38H 0:eb16ed402ffa 576 countzo=rotzo*4200;
Joost38H 0:eb16ed402ffa 577 countzb=rotzb*4200;
Joost38H 0:eb16ed402ffa 578 countzr=rotzr*4200;
Joost38H 0:eb16ed402ffa 579
Joost38H 0:eb16ed402ffa 580 pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat
Joost38H 0:eb16ed402ffa 581 }
Joost38H 0:eb16ed402ffa 582 }
Joost38H 0:eb16ed402ffa 583 }
Joost38H 0:eb16ed402ffa 584
Joost38H 0:eb16ed402ffa 585 int main()
Joost38H 0:eb16ed402ffa 586 {
Joost38H 0:eb16ed402ffa 587 pc.baud(115200);
Joost38H 1:6aac013b0ba3 588 getbqChain();
Joost38H 4:fddab1c875a9 589 threshold_timerR.attach(&Threshold_samplingBR, 0.002);
Joost38H 4:fddab1c875a9 590 threshold_timerL.attach(&Threshold_samplingBL, 0.002);
Joost38H 2:2c4ee76dc830 591 while(true){
Joost38H 2:2c4ee76dc830 592 sample_timer.attach(&EMG_sample, 0.002);
Joost38H 2:2c4ee76dc830 593 wait(2.5f);
Joost38H 2:2c4ee76dc830 594 tellerX();
Joost38H 2:2c4ee76dc830 595 tellerY();
Joost38H 2:2c4ee76dc830 596 calculator();
Joost38H 2:2c4ee76dc830 597 controlmotor1.attach(&MeasureAndControl1, 0.01);
Joost38H 2:2c4ee76dc830 598 calculatedelta1.attach(&calcdelta1, 0.01);
Joost38H 3:59b504840b95 599 //printdata1.attach(&readdata1, 0.5);
Joost38H 2:2c4ee76dc830 600 controlmotor2.attach(&MeasureAndControl2, 0.01);
Joost38H 2:2c4ee76dc830 601 calculatedelta2.attach(&calcdelta2, 0.01);
Joost38H 3:59b504840b95 602 //printdata2.attach(&readdata2, 0.5);
Joost38H 2:2c4ee76dc830 603 //zakker();
Joost38H 2:2c4ee76dc830 604 }
Joost38H 0:eb16ed402ffa 605 return 0;
Joost38H 0:eb16ed402ffa 606 }