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