Jitse Giesen / Mbed 2 deprecated master

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   biquadFilter mbed MODSERIAL

main.cpp@2:bf1c9d7afabd, 2017-10-26 (annotated)

Committer:
Jitse_Giesen
Date:
Thu Oct 26 13:05:51 2017 +0000
Revision:
2:bf1c9d7afabd
Parent:
1:9e871647a074
Child:
3:edfc159b024e
EMG en motor samengevoegd

Who changed what in which revision?

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