Joost Herijgers / Mbed 2 deprecated Code_MotorEMG_

EMG and motor script together, Not fully working yet,.

Dependencies:   Encoder MODSERIAL QEI biquadFilter mbed

Fork of Code_MotorEMG by Joost Herijgers

main.cpp@16:d75bf6d7d60e, 2017-11-01 (annotated)

Committer:
Joost38H
Date:
Wed Nov 01 12:26:20 2017 +0000
Revision:
16:d75bf6d7d60e
Parent:
15:46acc9b5decf
Child:
17:0acc8d4b142c
het aantal rondjes van de encoder komt niet overeen met de geprinte positie. misschien door te weinig rekenkracht.

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