Joost Herijgers / Mbed 2 deprecated Code_MotorEMG

EMG and motor script together, Not fully working yet,

Dependencies:   Encoder QEI biquadFilter mbed

main.cpp@5:81d3b53087c0, 2017-10-27 (annotated)

Committer:
Joost38H
Date:
Fri Oct 27 08:44:21 2017 +0000
Revision:
5:81d3b53087c0
Parent:
4:fddab1c875a9 
new mastercode, with code for three motors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joost38H 0:eb16ed402ffa 1 #include "mbed.h"
Joost38H 0:eb16ed402ffa 2 #include "math.h"
Joost38H 0:eb16ed402ffa 3 #include "encoder.h"
Joost38H 0:eb16ed402ffa 4 #include "QEI.h"
Joost38H 0:eb16ed402ffa 5 #include "BiQuad.h"
Joost38H 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 5:81d3b53087c0 42 double currentTimeTR;
Joost38H 5:81d3b53087c0 43 double 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 0:eb16ed402ffa 48 double speedfactor; // = 0.01; snelheid in, zonder potmeter gebruik <- waarom is dit zo?
Joost38H 5:81d3b53087c0 49
Joost38H 0:eb16ed402ffa 50 // Getting the counts from the Encoder
Joost38H 0:eb16ed402ffa 51 int counts1 = Encoder1.getPulses();
Joost38H 0:eb16ed402ffa 52 int counts2 = Encoder2.getPulses();
Joost38H 5:81d3b53087c0 53 int counts3 = Encoder3.getPulses();
Joost38H 5:81d3b53087c0 54
Joost38H 0:eb16ed402ffa 55 // Defining variables delta (the difference between position and desired position) <- Is dit zo?
Joost38H 0:eb16ed402ffa 56 int delta1;
Joost38H 0:eb16ed402ffa 57 int delta2;
Joost38H 5:81d3b53087c0 58 int delta3;
Joost38H 0:eb16ed402ffa 59
Joost38H 5:81d3b53087c0 60 // Boolean needed to know if new input coordinates have to be given
Joost38H 5:81d3b53087c0 61 bool Move_done = false;
Joost38H 5:81d3b53087c0 62
Joost38H 1:6aac013b0ba3 63 /* Defining all the different BiQuad filters, which contain a Notch filter,
Joost38H 1:6aac013b0ba3 64 High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
Joost38H 1:6aac013b0ba3 65 between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
Joost38H 5:81d3b53087c0 66 and the Low-pass filter cancels out all frequencies below 4 Hz. The filters are
Joost38H 5:81d3b53087c0 67 declared four times, so that they can be used for sampling of right and left
Joost38H 5:81d3b53087c0 68 biceps, during measurements and calibration. */
Joost38H 5:81d3b53087c0 69
Joost38H 1:6aac013b0ba3 70 /* Defining all the normalized values of b and a in the Notch filter for the
Joost38H 1:6aac013b0ba3 71 creation of the Notch BiQuad */
Joost38H 5:81d3b53087c0 72
Joost38H 5:81d3b53087c0 73 BiQuad bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Joost38H 5:81d3b53087c0 74 BiQuad bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Joost38H 5:81d3b53087c0 75
Joost38H 5:81d3b53087c0 76 BiQuad bqNotchTR( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Joost38H 5:81d3b53087c0 77 BiQuad bqNotchTL( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
Joost38H 5:81d3b53087c0 78
Joost38H 1:6aac013b0ba3 79 /* Defining all the normalized values of b and a in the High-pass filter for the
Joost38H 1:6aac013b0ba3 80 creation of the High-pass BiQuad */
Joost38H 5:81d3b53087c0 81
Joost38H 5:81d3b53087c0 82 BiQuad bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Joost38H 5:81d3b53087c0 83 BiQuad bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Joost38H 5:81d3b53087c0 84
Joost38H 5:81d3b53087c0 85 BiQuad bqHighTR( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Joost38H 5:81d3b53087c0 86 BiQuad bqHighTL( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
Joost38H 5:81d3b53087c0 87
Joost38H 1:6aac013b0ba3 88 /* Defining all the normalized values of b and a in the Low-pass filter for the
Joost38H 1:6aac013b0ba3 89 creation of the Low-pass BiQuad */
Joost38H 5:81d3b53087c0 90
Joost38H 5:81d3b53087c0 91 BiQuad bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Joost38H 5:81d3b53087c0 92 BiQuad bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Joost38H 5:81d3b53087c0 93
Joost38H 5:81d3b53087c0 94 BiQuad bqLowTR( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Joost38H 5:81d3b53087c0 95 BiQuad bqLowTL( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
Joost38H 5:81d3b53087c0 96
Joost38H 1:6aac013b0ba3 97 // Creating a variable needed for the creation of the BiQuadChain
Joost38H 5:81d3b53087c0 98 BiQuadChain bqChain1;
Joost38H 5:81d3b53087c0 99 BiQuadChain bqChain2;
Joost38H 5:81d3b53087c0 100
Joost38H 5:81d3b53087c0 101 BiQuadChain bqChainTR;
Joost38H 5:81d3b53087c0 102 BiQuadChain bqChainTL;
Joost38H 5:81d3b53087c0 103
Joost38H 5:81d3b53087c0 104 //Declaring all doubles needed in the filtering process
Joost38H 5:81d3b53087c0 105 double emgBRfiltered; //Right biceps Notch+High pass filter
Joost38H 5:81d3b53087c0 106 double emgBRrectified; //Right biceps rectified
Joost38H 5:81d3b53087c0 107 double emgBRcomplete; //Right biceps low-pass filter, filtering complete
Joost38H 5:81d3b53087c0 108
Joost38H 5:81d3b53087c0 109 double emgBLfiltered; //Left biceps Notch+High pass filter
Joost38H 5:81d3b53087c0 110 double emgBLrectified; //Left biceps rectified
Joost38H 5:81d3b53087c0 111 double emgBLcomplete; //Left biceps low-pass filter, filtering complete
Joost38H 1:6aac013b0ba3 112
Joost38H 5:81d3b53087c0 113 // Declaring all variables needed for getting the Threshold value
Joost38H 5:81d3b53087c0 114 double numsamples = 500;
Joost38H 5:81d3b53087c0 115 double emgBRsum = 0;
Joost38H 5:81d3b53087c0 116 double emgBRmeanMVC;
Joost38H 5:81d3b53087c0 117 double thresholdBR;
Joost38H 5:81d3b53087c0 118
Joost38H 5:81d3b53087c0 119 double emgBLsum = 0;
Joost38H 5:81d3b53087c0 120 double emgBLmeanMVC;
Joost38H 5:81d3b53087c0 121 double thresholdBL;
Joost38H 5:81d3b53087c0 122
Joost38H 5:81d3b53087c0 123 /* Function to sample the EMG of the Right Biceps and get a Threshold value
Joost38H 5:81d3b53087c0 124 from it, which can be used throughout the process */
Joost38H 5:81d3b53087c0 125
Joost38H 4:fddab1c875a9 126 void Threshold_samplingBR() {
Joost38H 4:fddab1c875a9 127 t_thresholdR.start();
Joost38H 4:fddab1c875a9 128 currentTimeTR = t_thresholdR.read();
Joost38H 4:fddab1c875a9 129
Joost38H 4:fddab1c875a9 130 if (currentTimeTR <= 1) {
Joost38H 4:fddab1c875a9 131
Joost38H 4:fddab1c875a9 132 emgBRfiltered = bqChainTR.step( emgBR.read() ); //Notch+High-pass
Joost38H 4:fddab1c875a9 133 emgBRrectified = fabs(emgBRfiltered); //Rectification
Joost38H 4:fddab1c875a9 134 emgBRcomplete = bqLowTR.step(emgBRrectified); //Low-pass
Joost38H 4:fddab1c875a9 135
Joost38H 4:fddab1c875a9 136 emgBRsum = emgBRsum + emgBRcomplete;
Joost38H 4:fddab1c875a9 137 }
Joost38H 4:fddab1c875a9 138 emgBRmeanMVC = emgBRsum/numsamples;
Joost38H 4:fddab1c875a9 139 thresholdBR = emgBRmeanMVC * 0.20;
Joost38H 5:81d3b53087c0 140
Joost38H 4:fddab1c875a9 141 //pc.printf("ThresholdBR = %f \n", thresholdBR);
Joost38H 4:fddab1c875a9 142 }
Joost38H 5:81d3b53087c0 143 /* Function to sample the EMG of the Left Biceps and get a Threshold value
Joost38H 5:81d3b53087c0 144 from it, which can be used throughout the process */
Joost38H 5:81d3b53087c0 145
Joost38H 4:fddab1c875a9 146 void Threshold_samplingBL() {
Joost38H 4:fddab1c875a9 147 t_thresholdL.start();
Joost38H 4:fddab1c875a9 148 currentTimeTL = t_thresholdL.read();
Joost38H 4:fddab1c875a9 149
Joost38H 4:fddab1c875a9 150 if (currentTimeTL <= 1) {
Joost38H 4:fddab1c875a9 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 4:fddab1c875a9 155
Joost38H 4:fddab1c875a9 156 emgBLsum = emgBLsum + emgBLcomplete;
Joost38H 4:fddab1c875a9 157 }
Joost38H 4:fddab1c875a9 158
Joost38H 4:fddab1c875a9 159 emgBLmeanMVC = emgBLsum/numsamples;
Joost38H 4:fddab1c875a9 160 thresholdBL = emgBLmeanMVC * 0.20;
Joost38H 5:81d3b53087c0 161
Joost38H 4:fddab1c875a9 162 }
Joost38H 5:81d3b53087c0 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 5:81d3b53087c0 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 5:81d3b53087c0 177
Joost38H 1:6aac013b0ba3 178 }
Joost38H 5:81d3b53087c0 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 5:81d3b53087c0 184
Joost38H 5:81d3b53087c0 185 bqChainTR.add(&bqNotchTR).add(&bqHighTR);
Joost38H 5:81d3b53087c0 186 bqChainTL.add(&bqNotchTR).add(&bqHighTL);
Joost38H 1:6aac013b0ba3 187 }
Joost38H 5:81d3b53087c0 188
Joost38H 5:81d3b53087c0 189 // Initial input value for couting the X-values
Joost38H 3:59b504840b95 190 int Xin=0;
Joost38H 5:81d3b53087c0 191 int Xin_new;
Joost38H 0:eb16ed402ffa 192 double huidigetijdX;
Joost38H 5:81d3b53087c0 193
Joost38H 5:81d3b53087c0 194 // Feedback system for counting values of X
Joost38H 5:81d3b53087c0 195 void ledtX(){
Joost38H 5:81d3b53087c0 196 t.reset();
Joost38H 5:81d3b53087c0 197 Xin++;
Joost38H 5:81d3b53087c0 198 pc.printf("Xin is %i\n",Xin);
Joost38H 5:81d3b53087c0 199 led_G=0;
Joost38H 5:81d3b53087c0 200 led_R=1;
Joost38H 5:81d3b53087c0 201 wait(0.2);
Joost38H 5:81d3b53087c0 202 led_G=1;
Joost38H 5:81d3b53087c0 203 led_R=0;
Joost38H 5:81d3b53087c0 204 wait(0.5);
Joost38H 5:81d3b53087c0 205 }
Joost38H 5:81d3b53087c0 206
Joost38H 5:81d3b53087c0 207 // Couting system for values of X
Joost38H 5:81d3b53087c0 208 int tellerX(){
Joost38H 5:81d3b53087c0 209 if (Move_done == true) {
Joost38H 5:81d3b53087c0 210 t.reset();
Joost38H 5:81d3b53087c0 211 led_G=1;
Joost38H 5:81d3b53087c0 212 led_B=1;
Joost38H 5:81d3b53087c0 213 led_R=0;
Joost38H 5:81d3b53087c0 214 while(true){
Joost38H 5:81d3b53087c0 215 //button.fall(ledtX);
Joost38H 5:81d3b53087c0 216 if (emgBRcomplete > thresholdBR) {
Joost38H 5:81d3b53087c0 217 ledtX();
Joost38H 5:81d3b53087c0 218 }
Joost38H 5:81d3b53087c0 219 t.start();
Joost38H 5:81d3b53087c0 220 huidigetijdX=t.read();
Joost38H 5:81d3b53087c0 221 if (huidigetijdX>2){
Joost38H 5:81d3b53087c0 222 led_R=1; //Go to the next program (counting values for Y)
Joost38H 5:81d3b53087c0 223 Xin_new = Xin;
Joost38H 5:81d3b53087c0 224 Xin = 0;
Joost38H 5:81d3b53087c0 225
Joost38H 5:81d3b53087c0 226 return Xin_new;
Joost38H 5:81d3b53087c0 227 }
Joost38H 5:81d3b53087c0 228
Joost38H 5:81d3b53087c0 229 }
Joost38H 5:81d3b53087c0 230
Joost38H 5:81d3b53087c0 231 }
Joost38H 5:81d3b53087c0 232 return 0;
Joost38H 5:81d3b53087c0 233 }
Joost38H 5:81d3b53087c0 234
Joost38H 5:81d3b53087c0 235 // Initial values needed for Y (see comments at X function)
Joost38H 5:81d3b53087c0 236 int Yin=0;
Joost38H 5:81d3b53087c0 237 int Yin_new;
Joost38H 5:81d3b53087c0 238 double huidigetijdY;
Joost38H 5:81d3b53087c0 239
Joost38H 5:81d3b53087c0 240 //Feedback system for couting values of Y
Joost38H 5:81d3b53087c0 241 void ledtY(){
Joost38H 5:81d3b53087c0 242 t.reset();
Joost38H 5:81d3b53087c0 243 Yin++;
Joost38H 5:81d3b53087c0 244 pc.printf("Yin is %i\n",Yin);
Joost38H 5:81d3b53087c0 245 led_G=0;
Joost38H 5:81d3b53087c0 246 led_B=1;
Joost38H 5:81d3b53087c0 247 wait(0.2);
Joost38H 5:81d3b53087c0 248 led_G=1;
Joost38H 5:81d3b53087c0 249 led_B=0;
Joost38H 5:81d3b53087c0 250 wait(0.5);
Joost38H 5:81d3b53087c0 251 }
Joost38H 5:81d3b53087c0 252
Joost38H 5:81d3b53087c0 253 // Couting system for values of Y
Joost38H 5:81d3b53087c0 254 int tellerY(){
Joost38H 5:81d3b53087c0 255 if (Move_done == true) {
Joost38H 5:81d3b53087c0 256 t.reset();
Joost38H 5:81d3b53087c0 257 led_G=1;
Joost38H 5:81d3b53087c0 258 led_B=0;
Joost38H 5:81d3b53087c0 259 led_R=1;
Joost38H 5:81d3b53087c0 260 while(true){
Joost38H 5:81d3b53087c0 261 //button.fall(ledtY);
Joost38H 5:81d3b53087c0 262 if (emgBRcomplete > thresholdBR) {
Joost38H 5:81d3b53087c0 263 ledtY();
Joost38H 5:81d3b53087c0 264 }
Joost38H 5:81d3b53087c0 265 t.start();
Joost38H 5:81d3b53087c0 266 huidigetijdY=t.read();
Joost38H 5:81d3b53087c0 267 if (huidigetijdY>2){
Joost38H 5:81d3b53087c0 268 led_B=1;
Joost38H 5:81d3b53087c0 269 Yin_new = Yin;
Joost38H 5:81d3b53087c0 270 Yin = 0;
Joost38H 5:81d3b53087c0 271
Joost38H 5:81d3b53087c0 272 Move_done = false;
Joost38H 5:81d3b53087c0 273 return Yin_new;
Joost38H 5:81d3b53087c0 274
Joost38H 5:81d3b53087c0 275 }
Joost38H 5:81d3b53087c0 276 }
Joost38H 5:81d3b53087c0 277 }
Joost38H 5:81d3b53087c0 278 return 0; // ga door naar het volgende programma
Joost38H 5:81d3b53087c0 279 }
Joost38H 5:81d3b53087c0 280
Joost38H 5:81d3b53087c0 281
Joost38H 5:81d3b53087c0 282
Joost38H 5:81d3b53087c0 283 // Oude shit voor input waardes geven
Joost38H 5:81d3b53087c0 284 /*---------------------------------------------------------------------------------
Joost38H 0:eb16ed402ffa 285 // Feedback system for counting values of X
Joost38H 0:eb16ed402ffa 286 void ledtX(){
Joost38H 0:eb16ed402ffa 287 t.reset();
Joost38H 0:eb16ed402ffa 288 Xin++;
Joost38H 0:eb16ed402ffa 289 pc.printf("Xin is %i\n",Xin);
Joost38H 0:eb16ed402ffa 290 led_G=0;
Joost38H 0:eb16ed402ffa 291 led_R=1;
Joost38H 2:2c4ee76dc830 292 wait(0.2);
Joost38H 0:eb16ed402ffa 293 led_G=1;
Joost38H 0:eb16ed402ffa 294 led_R=0;
Joost38H 2:2c4ee76dc830 295 wait(0.5);
Joost38H 0:eb16ed402ffa 296 }
Joost38H 0:eb16ed402ffa 297
Joost38H 0:eb16ed402ffa 298
Joost38H 0:eb16ed402ffa 299 // Couting system for values of X
Joost38H 0:eb16ed402ffa 300 int tellerX(){
Joost38H 5:81d3b53087c0 301 led_G=1;
Joost38H 5:81d3b53087c0 302 led_B=1;
Joost38H 5:81d3b53087c0 303 led_R=0;
Joost38H 0:eb16ed402ffa 304 while(true){
Joost38H 5:81d3b53087c0 305 //button.fall(ledtX); //This has to be replaced by EMG
Joost38H 5:81d3b53087c0 306 if (emgBRcomplete > thresholdBR){
Joost38H 5:81d3b53087c0 307 ledtX(); // dit is wat je uiteindelijk wil dat er staat
Joost38H 5:81d3b53087c0 308 }
Joost38H 5:81d3b53087c0 309 t.start();
Joost38H 5:81d3b53087c0 310 huidigetijdX=t.read();
Joost38H 5:81d3b53087c0 311 if (huidigetijdX>2){
Joost38H 5:81d3b53087c0 312 led_R=1; //Go to the next program (couting values for Y)
Joost38H 3:59b504840b95 313 if (emgBRcomplete > thresholdBR){
Joost38H 5:81d3b53087c0 314 0; // dit is wat je uiteindelijk wil dat er staat
Joost38H 2:2c4ee76dc830 315 }
Joost38H 5:81d3b53087c0 316 return 0;
Joost38H 5:81d3b53087c0 317 }
Joost38H 5:81d3b53087c0 318 }
Joost38H 0:eb16ed402ffa 319 }
Joost38H 0:eb16ed402ffa 320
Joost38H 0:eb16ed402ffa 321 // Initial values needed for Y (see comments at X function)
Joost38H 0:eb16ed402ffa 322 int Yin=0;
Joost38H 0:eb16ed402ffa 323 double huidigetijdY;
Joost38H 0:eb16ed402ffa 324
Joost38H 0:eb16ed402ffa 325 //Feedback system for couting values of Y
Joost38H 0:eb16ed402ffa 326 void ledtY(){
Joost38H 0:eb16ed402ffa 327 t.reset();
Joost38H 0:eb16ed402ffa 328 Yin++;
Joost38H 0:eb16ed402ffa 329 pc.printf("Yin is %i\n",Yin);
Joost38H 0:eb16ed402ffa 330 led_G=0;
Joost38H 0:eb16ed402ffa 331 led_B=1;
Joost38H 2:2c4ee76dc830 332 wait(0.2);
Joost38H 0:eb16ed402ffa 333 led_G=1;
Joost38H 0:eb16ed402ffa 334 led_B=0;
Joost38H 2:2c4ee76dc830 335 wait(0.5);
Joost38H 0:eb16ed402ffa 336 }
Joost38H 0:eb16ed402ffa 337
Joost38H 0:eb16ed402ffa 338 // Couting system for values of Y
Joost38H 0:eb16ed402ffa 339 int tellerY(){
Joost38H 0:eb16ed402ffa 340 t.reset();
Joost38H 0:eb16ed402ffa 341 led_G=1;
Joost38H 0:eb16ed402ffa 342 led_B=0;
Joost38H 2:2c4ee76dc830 343 led_R=1;
Joost38H 0:eb16ed402ffa 344 while(true){
Joost38H 1:6aac013b0ba3 345 //button.fall(ledtY); //See comments at X
Joost38H 1:6aac013b0ba3 346 if (emgBRcomplete > thresholdBR){
Joost38H 1:6aac013b0ba3 347 ledtY(); // dit is wat je uiteindelijk wil dat er staat
Joost38H 5:81d3b53087c0 348 }
Joost38H 0:eb16ed402ffa 349 t.start();
Joost38H 0:eb16ed402ffa 350 huidigetijdY=t.read();
Joost38H 0:eb16ed402ffa 351 if (huidigetijdY>2){
Joost38H 0:eb16ed402ffa 352 led_B=1;
Joost38H 5:81d3b53087c0 353 if (emgBRcomplete > thresholdBR){
Joost38H 5:81d3b53087c0 354 0; // dit is wat je uiteindelijk wil dat er staat
Joost38H 5:81d3b53087c0 355 }
Joost38H 5:81d3b53087c0 356
Joost38H 3:59b504840b95 357 //button.fall(0); // Wat is deze?
Joost38H 5:81d3b53087c0 358 return 0; // ga door naar het volgende programma
Joost38H 0:eb16ed402ffa 359 }
Joost38H 0:eb16ed402ffa 360 }
Joost38H 0:eb16ed402ffa 361 }
Joost38H 0:eb16ed402ffa 362
Joost38H 5:81d3b53087c0 363 */
Joost38H 3:59b504840b95 364
Joost38H 5:81d3b53087c0 365
Joost38H 0:eb16ed402ffa 366 // Declaring all variables needed for calculating rope lengths,
Joost38H 5:81d3b53087c0 367 double Pox = 0;
Joost38H 5:81d3b53087c0 368 double Poy = 0;
Joost38H 5:81d3b53087c0 369 double Pbx = 0;
Joost38H 5:81d3b53087c0 370 double Pby = 887;
Joost38H 5:81d3b53087c0 371 double Prx = 768;
Joost38H 5:81d3b53087c0 372 double Pry = 443;
Joost38H 5:81d3b53087c0 373 double Pex=121;
Joost38H 5:81d3b53087c0 374 double Pey=308;
Joost38H 5:81d3b53087c0 375 double diamtrklosje=20;
Joost38H 5:81d3b53087c0 376 double pi=3.14159265359;
Joost38H 5:81d3b53087c0 377 double omtrekklosje=diamtrklosje*pi;
Joost38H 5:81d3b53087c0 378 double Lou;
Joost38H 5:81d3b53087c0 379 double Lbu;
Joost38H 5:81d3b53087c0 380 double Lru;
Joost38H 5:81d3b53087c0 381 double dLod;
Joost38H 5:81d3b53087c0 382 double dLbd;
Joost38H 5:81d3b53087c0 383 double dLrd;
Joost38H 5:81d3b53087c0 384
Joost38H 0:eb16ed402ffa 385 // Declaring variables needed for calculating motor counts
Joost38H 5:81d3b53087c0 386 double roto;
Joost38H 5:81d3b53087c0 387 double rotb;
Joost38H 5:81d3b53087c0 388 double rotr;
Joost38H 5:81d3b53087c0 389 double rotzo;
Joost38H 5:81d3b53087c0 390 double rotzb;
Joost38H 5:81d3b53087c0 391 double rotzr;
Joost38H 5:81d3b53087c0 392 double counto;
Joost38H 5:81d3b53087c0 393 double countb;
Joost38H 5:81d3b53087c0 394 double countr;
Joost38H 5:81d3b53087c0 395 double countzo;
Joost38H 5:81d3b53087c0 396 double countzb;
Joost38H 5:81d3b53087c0 397 double countzr;
Joost38H 5:81d3b53087c0 398
Joost38H 5:81d3b53087c0 399 double hcounto;
Joost38H 5:81d3b53087c0 400 double dcounto;
Joost38H 5:81d3b53087c0 401 double hcountb;
Joost38H 5:81d3b53087c0 402 double dcountb;
Joost38H 5:81d3b53087c0 403 double hcountr;
Joost38H 5:81d3b53087c0 404 double dcountr;
Joost38H 5:81d3b53087c0 405
Joost38H 0:eb16ed402ffa 406 // Declaring variables neeeded for calculating motor movements to get to a certain point <- klopt dit?
Joost38H 5:81d3b53087c0 407 double Psx;
Joost38H 5:81d3b53087c0 408 double Psy;
Joost38H 5:81d3b53087c0 409 double Vex;
Joost38H 5:81d3b53087c0 410 double Vey;
Joost38H 5:81d3b53087c0 411 double Kz=0.7; // nadersnelheid instellen
Joost38H 5:81d3b53087c0 412 double modVe;
Joost38H 5:81d3b53087c0 413 double Vmax=20;
Joost38H 5:81d3b53087c0 414 double Pstx;
Joost38H 5:81d3b53087c0 415 double Psty;
Joost38H 5:81d3b53087c0 416 double T=0.02;//seconds
Joost38H 5:81d3b53087c0 417
Joost38H 5:81d3b53087c0 418 double speedfactor1;
Joost38H 5:81d3b53087c0 419 double speedfactor2;
Joost38H 5:81d3b53087c0 420 double speedfactor3;
Joost38H 5:81d3b53087c0 421
Joost38H 5:81d3b53087c0 422
Joost38H 0:eb16ed402ffa 423 //Deel om motor(en) aan te sturen--------------------------------------------
Joost38H 5:81d3b53087c0 424
Joost38H 5:81d3b53087c0 425
Joost38H 5:81d3b53087c0 426
Joost38H 0:eb16ed402ffa 427 void calcdelta1() {
Joost38H 5:81d3b53087c0 428 delta1 = (dcounto - Encoder1.getPulses());
Joost38H 5:81d3b53087c0 429 }
Joost38H 5:81d3b53087c0 430
Joost38H 5:81d3b53087c0 431 void calcdelta2() {
Joost38H 5:81d3b53087c0 432 delta2 = (dcountb - Encoder2.getPulses()); // <------- de reden dat de delta negatief is (jitse)
Joost38H 0:eb16ed402ffa 433 }
Joost38H 5:81d3b53087c0 434
Joost38H 5:81d3b53087c0 435 void calcdelta3() {
Joost38H 5:81d3b53087c0 436 delta3 = (dcountr - Encoder3.getPulses()); // <------- de reden dat de delta negatief is (jitse)
Joost38H 5:81d3b53087c0 437 }
Joost38H 5:81d3b53087c0 438
Joost38H 5:81d3b53087c0 439 double referenceVelocity1;
Joost38H 5:81d3b53087c0 440 double motorValue1;
Joost38H 5:81d3b53087c0 441
Joost38H 5:81d3b53087c0 442 double referenceVelocity2;
Joost38H 5:81d3b53087c0 443 double motorValue2;
Joost38H 0:eb16ed402ffa 444
Joost38H 5:81d3b53087c0 445 double referenceVelocity3;
Joost38H 5:81d3b53087c0 446 double motorValue3;
Joost38H 5:81d3b53087c0 447
Joost38H 5:81d3b53087c0 448 Ticker controlmotor1; // één ticker van maken?
Joost38H 5:81d3b53087c0 449 Ticker calculatedelta1;
Joost38H 5:81d3b53087c0 450 Ticker printdata1; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
Joost38H 5:81d3b53087c0 451
Joost38H 5:81d3b53087c0 452 Ticker controlmotor2; // één ticker van maken?
Joost38H 5:81d3b53087c0 453 Ticker calculatedelta2;
Joost38H 5:81d3b53087c0 454 Ticker printdata2; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
Joost38H 0:eb16ed402ffa 455
Joost38H 5:81d3b53087c0 456 Ticker controlmotor3; // één ticker van maken?
Joost38H 5:81d3b53087c0 457 Ticker calculatedelta3;
Joost38H 5:81d3b53087c0 458 Ticker printdata3; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
Joost38H 5:81d3b53087c0 459
Joost38H 0:eb16ed402ffa 460 double GetReferenceVelocity1()
Joost38H 0:eb16ed402ffa 461 {
Joost38H 0:eb16ed402ffa 462 // Returns reference velocity in rad/s. Positive value means clockwise rotation.
Joost38H 0:eb16ed402ffa 463 double maxVelocity1=Vex*25+Vey*25; // max 8.4 in rad/s of course!
Joost38H 5:81d3b53087c0 464 referenceVelocity1 = (-1)*speedfactor1 * maxVelocity1;
Joost38H 0:eb16ed402ffa 465
Joost38H 5:81d3b53087c0 466 if (dcounto < (10))
Joost38H 5:81d3b53087c0 467 { speedfactor1 = 0.01;
Joost38H 5:81d3b53087c0 468 if (dcounto > (-10))
Joost38H 5:81d3b53087c0 469 { printf("kleiner111111111");
Joost38H 5:81d3b53087c0 470 speedfactor1=0;
Joost38H 5:81d3b53087c0 471 }
Joost38H 0:eb16ed402ffa 472 }
Joost38H 5:81d3b53087c0 473 else if (dcounto > (-10))
Joost38H 5:81d3b53087c0 474 { speedfactor1 = -0.01;
Joost38H 5:81d3b53087c0 475 if (dcounto < (10))
Joost38H 5:81d3b53087c0 476 { printf("groter");
Joost38H 5:81d3b53087c0 477 speedfactor1=0;
Joost38H 5:81d3b53087c0 478 }
Joost38H 0:eb16ed402ffa 479 }
Joost38H 5:81d3b53087c0 480 else
Joost38H 5:81d3b53087c0 481 { speedfactor1 = 0;
Joost38H 5:81d3b53087c0 482 pc.printf("speedfactor nul;");
Joost38H 0:eb16ed402ffa 483 }
Joost38H 0:eb16ed402ffa 484
Joost38H 0:eb16ed402ffa 485 return referenceVelocity1;
Joost38H 0:eb16ed402ffa 486 }
Joost38H 5:81d3b53087c0 487
Joost38H 0:eb16ed402ffa 488 double GetReferenceVelocity2()
Joost38H 0:eb16ed402ffa 489 {
Joost38H 0:eb16ed402ffa 490 // Returns reference velocity in rad/s. Positive value means clockwise rotation.
Joost38H 0:eb16ed402ffa 491 double maxVelocity2=Vex*25+Vey*25; // max 8.4 in rad/s of course!
Joost38H 5:81d3b53087c0 492 referenceVelocity2 = (-1)*speedfactor2 * maxVelocity2;
Joost38H 0:eb16ed402ffa 493
Joost38H 5:81d3b53087c0 494 if (Encoder2.getPulses() < (dcountb+10))
Joost38H 5:81d3b53087c0 495 { speedfactor2 = -0.01;
Joost38H 5:81d3b53087c0 496 if (Encoder2.getPulses() > (dcountb-10))
Joost38H 5:81d3b53087c0 497 { //printf("kleiner22222222222");
Joost38H 5:81d3b53087c0 498 speedfactor2=0;
Joost38H 5:81d3b53087c0 499 }
Joost38H 0:eb16ed402ffa 500 }
Joost38H 5:81d3b53087c0 501 else if (Encoder2.getPulses() > (dcountb-10))
Joost38H 5:81d3b53087c0 502 { speedfactor2 = 0.01;
Joost38H 5:81d3b53087c0 503 if (Encoder2.getPulses() < (dcountb+10))
Joost38H 5:81d3b53087c0 504 { //printf("groter");
Joost38H 5:81d3b53087c0 505 speedfactor2=0;
Joost38H 5:81d3b53087c0 506 }
Joost38H 0:eb16ed402ffa 507 }
Joost38H 5:81d3b53087c0 508 else
Joost38H 5:81d3b53087c0 509 { speedfactor2 = 0;
Joost38H 5:81d3b53087c0 510 //pc.printf("speedfactor nul;");
Joost38H 0:eb16ed402ffa 511 }
Joost38H 0:eb16ed402ffa 512
Joost38H 0:eb16ed402ffa 513 return referenceVelocity2;
Joost38H 5:81d3b53087c0 514 }
Joost38H 0:eb16ed402ffa 515
Joost38H 5:81d3b53087c0 516 double GetReferenceVelocity3()
Joost38H 5:81d3b53087c0 517 {
Joost38H 5:81d3b53087c0 518 // Returns reference velocity in rad/s. Positive value means clockwise rotation.
Joost38H 5:81d3b53087c0 519 double maxVelocity3=Vex*25+Vey*25; // max 8.4 in rad/s of course!
Joost38H 5:81d3b53087c0 520 referenceVelocity3 = (-1)*speedfactor3 * maxVelocity3;
Joost38H 5:81d3b53087c0 521
Joost38H 5:81d3b53087c0 522 if (Encoder3.getPulses() < (dcountr+10))
Joost38H 5:81d3b53087c0 523 { speedfactor3 = -0.01;
Joost38H 5:81d3b53087c0 524 if (Encoder3.getPulses() > (dcountr-10))
Joost38H 5:81d3b53087c0 525 { //printf("kleiner22222222222");
Joost38H 5:81d3b53087c0 526 speedfactor3=0;
Joost38H 5:81d3b53087c0 527 }
Joost38H 5:81d3b53087c0 528 }
Joost38H 5:81d3b53087c0 529 else if (Encoder3.getPulses() > (dcountr-10))
Joost38H 5:81d3b53087c0 530 { speedfactor3 = 0.01;
Joost38H 5:81d3b53087c0 531 if (Encoder3.getPulses() < (dcountr+10))
Joost38H 5:81d3b53087c0 532 { //printf("groter");
Joost38H 5:81d3b53087c0 533 speedfactor3=0;
Joost38H 5:81d3b53087c0 534 }
Joost38H 5:81d3b53087c0 535 }
Joost38H 5:81d3b53087c0 536 else
Joost38H 5:81d3b53087c0 537 { speedfactor3 = 0;
Joost38H 5:81d3b53087c0 538 //pc.printf("speedfactor nul;");
Joost38H 5:81d3b53087c0 539 }
Joost38H 5:81d3b53087c0 540
Joost38H 5:81d3b53087c0 541 return referenceVelocity3;
Joost38H 5:81d3b53087c0 542 }
Joost38H 5:81d3b53087c0 543
Joost38H 0:eb16ed402ffa 544 void SetMotor1(double motorValue1)
Joost38H 0:eb16ed402ffa 545 {
Joost38H 0:eb16ed402ffa 546 // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
Joost38H 0:eb16ed402ffa 547 // motor rotating clockwise. motorValues outside range are truncated to within range
Joost38H 0:eb16ed402ffa 548 if (motorValue1 >=0) motor1DirectionPin=1;
Joost38H 0:eb16ed402ffa 549 else motor1DirectionPin=0;
Joost38H 0:eb16ed402ffa 550 if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
Joost38H 0:eb16ed402ffa 551 else motor1MagnitudePin = fabs(motorValue1);
Joost38H 0:eb16ed402ffa 552
Joost38H 0:eb16ed402ffa 553 }
Joost38H 5:81d3b53087c0 554
Joost38H 0:eb16ed402ffa 555 void SetMotor2(double motorValue2)
Joost38H 0:eb16ed402ffa 556 {
Joost38H 0:eb16ed402ffa 557 // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
Joost38H 0:eb16ed402ffa 558 // motor rotating clockwise. motorValues outside range are truncated to within range
Joost38H 0:eb16ed402ffa 559 if (motorValue2 >=0) motor2DirectionPin=1;
Joost38H 0:eb16ed402ffa 560 else motor2DirectionPin=0;
Joost38H 0:eb16ed402ffa 561 if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
Joost38H 0:eb16ed402ffa 562 else motor2MagnitudePin = fabs(motorValue2);
Joost38H 0:eb16ed402ffa 563
Joost38H 0:eb16ed402ffa 564 }
Joost38H 0:eb16ed402ffa 565
Joost38H 5:81d3b53087c0 566 void SetMotor3(double motorValue3)
Joost38H 5:81d3b53087c0 567 {
Joost38H 5:81d3b53087c0 568 // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
Joost38H 5:81d3b53087c0 569 // motor rotating clockwise. motorValues outside range are truncated to within range
Joost38H 5:81d3b53087c0 570 if (motorValue3 >=0) motor3DirectionPin=1;
Joost38H 5:81d3b53087c0 571 else motor3DirectionPin=0;
Joost38H 5:81d3b53087c0 572 if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
Joost38H 5:81d3b53087c0 573 else motor3MagnitudePin = fabs(motorValue3);
Joost38H 5:81d3b53087c0 574
Joost38H 5:81d3b53087c0 575 }
Joost38H 5:81d3b53087c0 576
Joost38H 0:eb16ed402ffa 577 double FeedForwardControl1(double referenceVelocity1)
Joost38H 0:eb16ed402ffa 578 {
Joost38H 0:eb16ed402ffa 579 // very simple linear feed-forward control
Joost38H 0:eb16ed402ffa 580 const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
Joost38H 0:eb16ed402ffa 581 double motorValue1 = referenceVelocity1 / MotorGain;
Joost38H 0:eb16ed402ffa 582 return motorValue1;
Joost38H 0:eb16ed402ffa 583 }
Joost38H 5:81d3b53087c0 584
Joost38H 0:eb16ed402ffa 585 double FeedForwardControl2(double referenceVelocity2)
Joost38H 0:eb16ed402ffa 586 {
Joost38H 0:eb16ed402ffa 587 // very simple linear feed-forward control
Joost38H 0:eb16ed402ffa 588 const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
Joost38H 0:eb16ed402ffa 589 double motorValue2 = referenceVelocity2 / MotorGain;
Joost38H 0:eb16ed402ffa 590 return motorValue2;
Joost38H 0:eb16ed402ffa 591 }
Joost38H 5:81d3b53087c0 592
Joost38H 5:81d3b53087c0 593 double FeedForwardControl3(double referenceVelocity3)
Joost38H 5:81d3b53087c0 594 {
Joost38H 5:81d3b53087c0 595 // very simple linear feed-forward control
Joost38H 5:81d3b53087c0 596 const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
Joost38H 5:81d3b53087c0 597 double motorValue3 = referenceVelocity3 / MotorGain;
Joost38H 5:81d3b53087c0 598 return motorValue3;
Joost38H 5:81d3b53087c0 599 }
Joost38H 0:eb16ed402ffa 600
Joost38H 0:eb16ed402ffa 601 void MeasureAndControl1()
Joost38H 0:eb16ed402ffa 602 {
Joost38H 0:eb16ed402ffa 603 // This function measures the potmeter position, extracts a reference velocity from it,
Joost38H 0:eb16ed402ffa 604 // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
Joost38H 0:eb16ed402ffa 605 double referenceVelocity1 = GetReferenceVelocity1();
Joost38H 0:eb16ed402ffa 606 double motorValue1 = FeedForwardControl1(referenceVelocity1);
Joost38H 0:eb16ed402ffa 607 SetMotor1(motorValue1);
Joost38H 0:eb16ed402ffa 608 }
Joost38H 5:81d3b53087c0 609
Joost38H 0:eb16ed402ffa 610 void MeasureAndControl2()
Joost38H 0:eb16ed402ffa 611 {
Joost38H 0:eb16ed402ffa 612 // This function measures the potmeter position, extracts a reference velocity from it,
Joost38H 0:eb16ed402ffa 613 // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
Joost38H 0:eb16ed402ffa 614 double referenceVelocity2 = GetReferenceVelocity2();
Joost38H 0:eb16ed402ffa 615 double motorValue2 = FeedForwardControl2(referenceVelocity2);
Joost38H 0:eb16ed402ffa 616 SetMotor2(motorValue2);
Joost38H 0:eb16ed402ffa 617 }
Joost38H 0:eb16ed402ffa 618
Joost38H 5:81d3b53087c0 619 void MeasureAndControl3()
Joost38H 5:81d3b53087c0 620 {
Joost38H 5:81d3b53087c0 621 // This function measures the potmeter position, extracts a reference velocity from it,
Joost38H 5:81d3b53087c0 622 // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
Joost38H 5:81d3b53087c0 623 double referenceVelocity3 = GetReferenceVelocity3();
Joost38H 5:81d3b53087c0 624 double motorValue3 = FeedForwardControl3(referenceVelocity3);
Joost38H 5:81d3b53087c0 625 SetMotor3(motorValue3);
Joost38H 5:81d3b53087c0 626 }
Joost38H 5:81d3b53087c0 627
Joost38H 0:eb16ed402ffa 628 void readdata1()
Joost38H 0:eb16ed402ffa 629 { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
Joost38H 5:81d3b53087c0 630 //pc.printf("Pulses_M1 = %i \r\n",Encoder1.getPulses());
Joost38H 0:eb16ed402ffa 631 //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
Joost38H 5:81d3b53087c0 632 //pc.printf("Delta_M1 = %i \r\n",delta1);
Joost38H 0:eb16ed402ffa 633 }
Joost38H 0:eb16ed402ffa 634
Joost38H 0:eb16ed402ffa 635 void readdata2()
Joost38H 0:eb16ed402ffa 636 { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
Joost38H 5:81d3b53087c0 637 //pc.printf("Pulses_M2 = %i \r\n",Encoder2.getPulses());
Joost38H 0:eb16ed402ffa 638 //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
Joost38H 5:81d3b53087c0 639 //pc.printf("Delta_M2 = %i \r\n",delta2);
Joost38H 0:eb16ed402ffa 640 }
Joost38H 5:81d3b53087c0 641
Joost38H 5:81d3b53087c0 642 void readdata3()
Joost38H 5:81d3b53087c0 643 { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
Joost38H 5:81d3b53087c0 644 //pc.printf("Pulses_M2 = %i \r\n",Encoder3.getPulses());
Joost38H 5:81d3b53087c0 645 //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
Joost38H 5:81d3b53087c0 646 //pc.printf("Delta_M2 = %i \r\n",delta3);
Joost38H 5:81d3b53087c0 647 }
Joost38H 5:81d3b53087c0 648
Joost38H 0:eb16ed402ffa 649 // einde deel motor------------------------------------------------------------------------------------
Joost38H 5:81d3b53087c0 650
Joost38H 0:eb16ed402ffa 651 Ticker loop;
Joost38H 5:81d3b53087c0 652
Joost38H 0:eb16ed402ffa 653 /*Calculates ropelengths that are needed to get to new positions, based on the
Joost38H 0:eb16ed402ffa 654 set coordinates and the position of the poles */
Joost38H 0:eb16ed402ffa 655 double touwlengtes(){
Joost38H 0:eb16ed402ffa 656 Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));
Joost38H 0:eb16ed402ffa 657 Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2));
Joost38H 0:eb16ed402ffa 658 Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
Joost38H 0:eb16ed402ffa 659 return 0;
Joost38H 0:eb16ed402ffa 660 }
Joost38H 5:81d3b53087c0 661
Joost38H 0:eb16ed402ffa 662 /* Calculates rotations (and associated counts) of the motor to get to the
Joost38H 0:eb16ed402ffa 663 desired new position*/
Joost38H 0:eb16ed402ffa 664 double turns(){
Joost38H 0:eb16ed402ffa 665
Joost38H 0:eb16ed402ffa 666 roto=Lou/omtrekklosje;
Joost38H 0:eb16ed402ffa 667 rotb=Lbu/omtrekklosje;
Joost38H 0:eb16ed402ffa 668 rotr=Lru/omtrekklosje;
Joost38H 0:eb16ed402ffa 669 counto=roto*4200;
Joost38H 5:81d3b53087c0 670 dcounto=counto-hcounto;
Joost38H 5:81d3b53087c0 671 pc.printf("dcounto = %f \n\r",dcounto);
Joost38H 0:eb16ed402ffa 672 countb=rotb*4200;
Joost38H 5:81d3b53087c0 673 dcountb=countb-hcountb;
Joost38H 5:81d3b53087c0 674 pc.printf("dcountb = %f \n\r",dcountb);
Joost38H 0:eb16ed402ffa 675 countr=rotr*4200;
Joost38H 5:81d3b53087c0 676 dcountr=countr-hcountr;
Joost38H 5:81d3b53087c0 677
Joost38H 0:eb16ed402ffa 678 return 0;
Joost38H 0:eb16ed402ffa 679 }
Joost38H 5:81d3b53087c0 680
Joost38H 0:eb16ed402ffa 681 // Waar komen Pstx en Psty vandaan en waar staan ze voor? En is dit maar voor een paal?
Joost38H 0:eb16ed402ffa 682 double Pst(){
Joost38H 0:eb16ed402ffa 683 Pstx=Pex+Vex*T;
Joost38H 0:eb16ed402ffa 684 Psty=Pey+Vey*T;
Joost38H 0:eb16ed402ffa 685 touwlengtes();
Joost38H 0:eb16ed402ffa 686 Pex=Pstx;
Joost38H 0:eb16ed402ffa 687 Pey=Psty;
Joost38H 5:81d3b53087c0 688 pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
Joost38H 0:eb16ed402ffa 689 //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru);
Joost38H 0:eb16ed402ffa 690 turns();
Joost38H 0:eb16ed402ffa 691 //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr);
Joost38H 0:eb16ed402ffa 692 pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
Joost38H 0:eb16ed402ffa 693 /*float R;
Joost38H 0:eb16ed402ffa 694 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 695 pc.printf("\n\r R=%f",R);*/
Joost38H 0:eb16ed402ffa 696 return 0;
Joost38H 0:eb16ed402ffa 697 }
Joost38H 5:81d3b53087c0 698
Joost38H 0:eb16ed402ffa 699 //Calculating desired end position based on the EMG input <- Waarom maar voor een paal?
Joost38H 0:eb16ed402ffa 700 double Ps(){
Joost38H 5:81d3b53087c0 701 if (Move_done==true);
Joost38H 3:59b504840b95 702 Psx=(Xin_new)*30+121;
Joost38H 3:59b504840b95 703 Psy=(Yin_new)*30+308;
Joost38H 5:81d3b53087c0 704 pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
Joost38H 5:81d3b53087c0 705 hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje);
Joost38H 5:81d3b53087c0 706 hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje);
Joost38H 5:81d3b53087c0 707 hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje);
Joost38H 0:eb16ed402ffa 708 return 0;
Joost38H 0:eb16ed402ffa 709 }
Joost38H 5:81d3b53087c0 710
Joost38H 0:eb16ed402ffa 711 // Rekent dit de snelheid uit waarmee de motoren bewegen?
Joost38H 0:eb16ed402ffa 712 void Ve(){
Joost38H 0:eb16ed402ffa 713 Vex=Kz*(Psx-Pex);
Joost38H 0:eb16ed402ffa 714 Vey=Kz*(Psy-Pey);
Joost38H 0:eb16ed402ffa 715 modVe=sqrt(pow(Vex,2)+pow(Vey,2));
Joost38H 0:eb16ed402ffa 716 if(modVe>Vmax){
Joost38H 0:eb16ed402ffa 717 Vex=(Vex/modVe)*Vmax;
Joost38H 0:eb16ed402ffa 718 Vey=(Vey/modVe)*Vmax;
Joost38H 0:eb16ed402ffa 719 }
Joost38H 0:eb16ed402ffa 720 Pst();
Joost38H 5:81d3b53087c0 721 //pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
Joost38H 0:eb16ed402ffa 722 if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
Joost38H 3:59b504840b95 723 Move_done=true;
Joost38H 0:eb16ed402ffa 724 loop.detach();
Joost38H 0:eb16ed402ffa 725 }
Joost38H 0:eb16ed402ffa 726 }
Joost38H 5:81d3b53087c0 727
Joost38H 0:eb16ed402ffa 728 // Calculating the desired position, so that the motors can go here
Joost38H 0:eb16ed402ffa 729 int calculator(){
Joost38H 0:eb16ed402ffa 730 Ps();
Joost38H 0:eb16ed402ffa 731 loop.attach(&Ve,0.02);
Joost38H 0:eb16ed402ffa 732 return 0;
Joost38H 0:eb16ed402ffa 733 }
Joost38H 5:81d3b53087c0 734
Joost38H 0:eb16ed402ffa 735 // Function which makes it possible to lower the end-effector to pick up a piece
Joost38H 0:eb16ed402ffa 736 void zakker(){
Joost38H 0:eb16ed402ffa 737 while(1){
Joost38H 0:eb16ed402ffa 738 wait(1);
Joost38H 3:59b504840b95 739 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 740 dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken
Joost38H 0:eb16ed402ffa 741 dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
Joost38H 0:eb16ed402ffa 742 dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
Joost38H 0:eb16ed402ffa 743 rotzo=dLod/omtrekklosje;
Joost38H 0:eb16ed402ffa 744 rotzb=dLbd/omtrekklosje;
Joost38H 0:eb16ed402ffa 745 rotzr=dLrd/omtrekklosje;
Joost38H 0:eb16ed402ffa 746 countzo=rotzo*4200;
Joost38H 0:eb16ed402ffa 747 countzb=rotzb*4200;
Joost38H 0:eb16ed402ffa 748 countzr=rotzr*4200;
Joost38H 0:eb16ed402ffa 749
Joost38H 5:81d3b53087c0 750 //pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat
Joost38H 0:eb16ed402ffa 751 }
Joost38H 0:eb16ed402ffa 752 }
Joost38H 0:eb16ed402ffa 753 }
Joost38H 5:81d3b53087c0 754
Joost38H 0:eb16ed402ffa 755 int main()
Joost38H 0:eb16ed402ffa 756 {
Joost38H 0:eb16ed402ffa 757 pc.baud(115200);
Joost38H 1:6aac013b0ba3 758 getbqChain();
Joost38H 4:fddab1c875a9 759 threshold_timerR.attach(&Threshold_samplingBR, 0.002);
Joost38H 4:fddab1c875a9 760 threshold_timerL.attach(&Threshold_samplingBL, 0.002);
Joost38H 2:2c4ee76dc830 761 while(true){
Joost38H 2:2c4ee76dc830 762 sample_timer.attach(&EMG_sample, 0.002);
Joost38H 2:2c4ee76dc830 763 wait(2.5f);
Joost38H 5:81d3b53087c0 764
Joost38H 2:2c4ee76dc830 765 tellerX();
Joost38H 2:2c4ee76dc830 766 tellerY();
Joost38H 2:2c4ee76dc830 767 calculator();
Joost38H 2:2c4ee76dc830 768 controlmotor1.attach(&MeasureAndControl1, 0.01);
Joost38H 2:2c4ee76dc830 769 calculatedelta1.attach(&calcdelta1, 0.01);
Joost38H 5:81d3b53087c0 770 printdata1.attach(&readdata1, 0.5);
Joost38H 5:81d3b53087c0 771 //controlmotor2.attach(&MeasureAndControl2, 0.01);
Joost38H 5:81d3b53087c0 772 //calculatedelta2.attach(&calcdelta2, 0.01);
Joost38H 3:59b504840b95 773 //printdata2.attach(&readdata2, 0.5);
Joost38H 5:81d3b53087c0 774 //controlmotor3.attach(&MeasureAndControl3, 0.01);
Joost38H 5:81d3b53087c0 775 //calculatedelta3.attach(&calcdelta3, 0.01);
Joost38H 5:81d3b53087c0 776 //printdata3.attach(&readdata3, 0.5);
Joost38H 2:2c4ee76dc830 777 //zakker();
Joost38H 5:81d3b53087c0 778 wait(5.0f);
Joost38H 2:2c4ee76dc830 779 }
Joost38H 5:81d3b53087c0 780
Joost38H 5:81d3b53087c0 781 }
Joost38H 5:81d3b53087c0 782