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@11:f23fe69ba3ef, 2017-11-01 (annotated)

Committer:
Joost38H
Date:
Wed Nov 01 11:36:04 2017 +0000
Revision:
11:f23fe69ba3ef
Parent:
10:952377fbbbfe
Child:
12:ea9afe159eb1
Hij leest de encoders, blijft in het begin actief tussen de -100 en 100. bewegingen gaan niet goed

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