Joost Herijgers / Mbed 2 deprecated Code_MotorEMG_

EMG and motor script together, Not fully working yet,.

Dependencies:   Encoder MODSERIAL QEI biquadFilter mbed

Fork of Code_MotorEMG by Joost Herijgers

main.cpp@25:c1eab0cc297e, 2017-11-02 (annotated)

Committer:
Joost38H
Date:
Thu Nov 02 10:24:47 2017 +0000
Revision:
25:c1eab0cc297e
Parent:
24:50235511956c
Child:
26:3e421de274e9
Bewegingen werken, zakker nog niet;

Who changed what in which revision?

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