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

Committer:
Joost38H
Date:
Wed Nov 01 16:21:57 2017 +0000
Revision:
21:3fdd135a3dfd
Parent:
20:bde79d7a4091
Child:
22:cdaa5c1208a4
Werkende tiller maar het normale stuk is nu gesloopt dus we gaan terug naar de vorige versie en vanaf daar wqeer een tiller toevoegen;

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