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@12:2249fcee09e6, 2017-11-02 (annotated)

Committer:
Jitse_Giesen
Date:
Thu Nov 02 19:56:49 2017 +0000
Revision:
12:2249fcee09e6
Parent:
11:1f57061dadfa
Child:
13:3a17bdfbbba1
werkende shit met emg, alle waits zijn goed en beargumenteerd. PS als je dit leest heb je iets verneukt met coderen, dummy

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 12:2249fcee09e6 603 wait(2.5f); // om te zorgen dat je niet direct na de calibratie input moet geven en om te zorgen dat de zakker de tijd heeft voor nieuwe input vragen
Jitse_Giesen 11:1f57061dadfa 604
Jitse_Giesen 0:51a6e38a4d4a 605 tellerX();
Jitse_Giesen 0:51a6e38a4d4a 606 tellerY();
Jitse_Giesen 0:51a6e38a4d4a 607 calculator();
Jitse_Giesen 4:9e18e041f7a0 608 controlmotor1.attach(&MotorController1, 0.01);
Jitse_Giesen 4:9e18e041f7a0 609 controlmotor2.attach(&MotorController2, 0.01);
Jitse_Giesen 4:9e18e041f7a0 610 controlmotor3.attach(&MotorController3, 0.01);
Jitse_Giesen 11:1f57061dadfa 611 wait(4.0f); // om te zorgen dat je niet meteen tijdens t zakken nieuwe input moet geven
Jitse_Giesen 0:51a6e38a4d4a 612 }
Jitse_Giesen 4:9e18e041f7a0 613
Jitse_Giesen 0:51a6e38a4d4a 614 }