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@10:a8a07e4ce85c, 2017-11-02 (annotated)

Committer:
Jitse_Giesen
Date:
Thu Nov 02 19:16:02 2017 +0000
Revision:
10:a8a07e4ce85c
Parent:
8:8e7c928aadc6
Child:
11:1f57061dadfa
dit werkt met EMG #fissaaaaaaaaaaaaaa

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