EMG and motor script together, Not fully working yet,
Dependencies: Encoder QEI biquadFilter mbed
main.cpp@5:81d3b53087c0, 2017-10-27 (annotated)
- Committer:
- Joost38H
- Date:
- Fri Oct 27 08:44:21 2017 +0000
- Revision:
- 5:81d3b53087c0
- Parent:
- 4:fddab1c875a9
new mastercode, with code for three motors
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 | 5:81d3b53087c0 | 215 | //button.fall(ledtX); |
Joost38H | 5:81d3b53087c0 | 216 | if (emgBRcomplete > thresholdBR) { |
Joost38H | 5:81d3b53087c0 | 217 | ledtX(); |
Joost38H | 5:81d3b53087c0 | 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 | 5:81d3b53087c0 | 261 | //button.fall(ledtY); |
Joost38H | 5:81d3b53087c0 | 262 | if (emgBRcomplete > thresholdBR) { |
Joost38H | 5:81d3b53087c0 | 263 | ledtY(); |
Joost38H | 5:81d3b53087c0 | 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 | 5:81d3b53087c0 | 373 | double Pex=121; |
Joost38H | 5:81d3b53087c0 | 374 | double Pey=308; |
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 | 5:81d3b53087c0 | 398 | |
Joost38H | 5:81d3b53087c0 | 399 | double hcounto; |
Joost38H | 5:81d3b53087c0 | 400 | double dcounto; |
Joost38H | 5:81d3b53087c0 | 401 | double hcountb; |
Joost38H | 5:81d3b53087c0 | 402 | double dcountb; |
Joost38H | 5:81d3b53087c0 | 403 | double hcountr; |
Joost38H | 5:81d3b53087c0 | 404 | double dcountr; |
Joost38H | 5:81d3b53087c0 | 405 | |
Joost38H | 0:eb16ed402ffa | 406 | // Declaring variables neeeded for calculating motor movements to get to a certain point <- klopt dit? |
Joost38H | 5:81d3b53087c0 | 407 | double Psx; |
Joost38H | 5:81d3b53087c0 | 408 | double Psy; |
Joost38H | 5:81d3b53087c0 | 409 | double Vex; |
Joost38H | 5:81d3b53087c0 | 410 | double Vey; |
Joost38H | 5:81d3b53087c0 | 411 | double Kz=0.7; // nadersnelheid instellen |
Joost38H | 5:81d3b53087c0 | 412 | double modVe; |
Joost38H | 5:81d3b53087c0 | 413 | double Vmax=20; |
Joost38H | 5:81d3b53087c0 | 414 | double Pstx; |
Joost38H | 5:81d3b53087c0 | 415 | double Psty; |
Joost38H | 5:81d3b53087c0 | 416 | double T=0.02;//seconds |
Joost38H | 5:81d3b53087c0 | 417 | |
Joost38H | 5:81d3b53087c0 | 418 | double speedfactor1; |
Joost38H | 5:81d3b53087c0 | 419 | double speedfactor2; |
Joost38H | 5:81d3b53087c0 | 420 | double speedfactor3; |
Joost38H | 5:81d3b53087c0 | 421 | |
Joost38H | 5:81d3b53087c0 | 422 | |
Joost38H | 0:eb16ed402ffa | 423 | //Deel om motor(en) aan te sturen-------------------------------------------- |
Joost38H | 5:81d3b53087c0 | 424 | |
Joost38H | 5:81d3b53087c0 | 425 | |
Joost38H | 5:81d3b53087c0 | 426 | |
Joost38H | 0:eb16ed402ffa | 427 | void calcdelta1() { |
Joost38H | 5:81d3b53087c0 | 428 | delta1 = (dcounto - Encoder1.getPulses()); |
Joost38H | 5:81d3b53087c0 | 429 | } |
Joost38H | 5:81d3b53087c0 | 430 | |
Joost38H | 5:81d3b53087c0 | 431 | void calcdelta2() { |
Joost38H | 5:81d3b53087c0 | 432 | delta2 = (dcountb - Encoder2.getPulses()); // <------- de reden dat de delta negatief is (jitse) |
Joost38H | 0:eb16ed402ffa | 433 | } |
Joost38H | 5:81d3b53087c0 | 434 | |
Joost38H | 5:81d3b53087c0 | 435 | void calcdelta3() { |
Joost38H | 5:81d3b53087c0 | 436 | delta3 = (dcountr - Encoder3.getPulses()); // <------- de reden dat de delta negatief is (jitse) |
Joost38H | 5:81d3b53087c0 | 437 | } |
Joost38H | 5:81d3b53087c0 | 438 | |
Joost38H | 5:81d3b53087c0 | 439 | double referenceVelocity1; |
Joost38H | 5:81d3b53087c0 | 440 | double motorValue1; |
Joost38H | 5:81d3b53087c0 | 441 | |
Joost38H | 5:81d3b53087c0 | 442 | double referenceVelocity2; |
Joost38H | 5:81d3b53087c0 | 443 | double motorValue2; |
Joost38H | 0:eb16ed402ffa | 444 | |
Joost38H | 5:81d3b53087c0 | 445 | double referenceVelocity3; |
Joost38H | 5:81d3b53087c0 | 446 | double motorValue3; |
Joost38H | 5:81d3b53087c0 | 447 | |
Joost38H | 5:81d3b53087c0 | 448 | Ticker controlmotor1; // één ticker van maken? |
Joost38H | 5:81d3b53087c0 | 449 | Ticker calculatedelta1; |
Joost38H | 5:81d3b53087c0 | 450 | Ticker printdata1; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes |
Joost38H | 5:81d3b53087c0 | 451 | |
Joost38H | 5:81d3b53087c0 | 452 | Ticker controlmotor2; // één ticker van maken? |
Joost38H | 5:81d3b53087c0 | 453 | Ticker calculatedelta2; |
Joost38H | 5:81d3b53087c0 | 454 | Ticker printdata2; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes |
Joost38H | 0:eb16ed402ffa | 455 | |
Joost38H | 5:81d3b53087c0 | 456 | Ticker controlmotor3; // één ticker van maken? |
Joost38H | 5:81d3b53087c0 | 457 | Ticker calculatedelta3; |
Joost38H | 5:81d3b53087c0 | 458 | Ticker printdata3; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes |
Joost38H | 5:81d3b53087c0 | 459 | |
Joost38H | 0:eb16ed402ffa | 460 | double GetReferenceVelocity1() |
Joost38H | 0:eb16ed402ffa | 461 | { |
Joost38H | 0:eb16ed402ffa | 462 | // Returns reference velocity in rad/s. Positive value means clockwise rotation. |
Joost38H | 0:eb16ed402ffa | 463 | double maxVelocity1=Vex*25+Vey*25; // max 8.4 in rad/s of course! |
Joost38H | 5:81d3b53087c0 | 464 | referenceVelocity1 = (-1)*speedfactor1 * maxVelocity1; |
Joost38H | 0:eb16ed402ffa | 465 | |
Joost38H | 5:81d3b53087c0 | 466 | if (dcounto < (10)) |
Joost38H | 5:81d3b53087c0 | 467 | { speedfactor1 = 0.01; |
Joost38H | 5:81d3b53087c0 | 468 | if (dcounto > (-10)) |
Joost38H | 5:81d3b53087c0 | 469 | { printf("kleiner111111111"); |
Joost38H | 5:81d3b53087c0 | 470 | speedfactor1=0; |
Joost38H | 5:81d3b53087c0 | 471 | } |
Joost38H | 0:eb16ed402ffa | 472 | } |
Joost38H | 5:81d3b53087c0 | 473 | else if (dcounto > (-10)) |
Joost38H | 5:81d3b53087c0 | 474 | { speedfactor1 = -0.01; |
Joost38H | 5:81d3b53087c0 | 475 | if (dcounto < (10)) |
Joost38H | 5:81d3b53087c0 | 476 | { printf("groter"); |
Joost38H | 5:81d3b53087c0 | 477 | speedfactor1=0; |
Joost38H | 5:81d3b53087c0 | 478 | } |
Joost38H | 0:eb16ed402ffa | 479 | } |
Joost38H | 5:81d3b53087c0 | 480 | else |
Joost38H | 5:81d3b53087c0 | 481 | { speedfactor1 = 0; |
Joost38H | 5:81d3b53087c0 | 482 | pc.printf("speedfactor nul;"); |
Joost38H | 0:eb16ed402ffa | 483 | } |
Joost38H | 0:eb16ed402ffa | 484 | |
Joost38H | 0:eb16ed402ffa | 485 | return referenceVelocity1; |
Joost38H | 0:eb16ed402ffa | 486 | } |
Joost38H | 5:81d3b53087c0 | 487 | |
Joost38H | 0:eb16ed402ffa | 488 | double GetReferenceVelocity2() |
Joost38H | 0:eb16ed402ffa | 489 | { |
Joost38H | 0:eb16ed402ffa | 490 | // Returns reference velocity in rad/s. Positive value means clockwise rotation. |
Joost38H | 0:eb16ed402ffa | 491 | double maxVelocity2=Vex*25+Vey*25; // max 8.4 in rad/s of course! |
Joost38H | 5:81d3b53087c0 | 492 | referenceVelocity2 = (-1)*speedfactor2 * maxVelocity2; |
Joost38H | 0:eb16ed402ffa | 493 | |
Joost38H | 5:81d3b53087c0 | 494 | if (Encoder2.getPulses() < (dcountb+10)) |
Joost38H | 5:81d3b53087c0 | 495 | { speedfactor2 = -0.01; |
Joost38H | 5:81d3b53087c0 | 496 | if (Encoder2.getPulses() > (dcountb-10)) |
Joost38H | 5:81d3b53087c0 | 497 | { //printf("kleiner22222222222"); |
Joost38H | 5:81d3b53087c0 | 498 | speedfactor2=0; |
Joost38H | 5:81d3b53087c0 | 499 | } |
Joost38H | 0:eb16ed402ffa | 500 | } |
Joost38H | 5:81d3b53087c0 | 501 | else if (Encoder2.getPulses() > (dcountb-10)) |
Joost38H | 5:81d3b53087c0 | 502 | { speedfactor2 = 0.01; |
Joost38H | 5:81d3b53087c0 | 503 | if (Encoder2.getPulses() < (dcountb+10)) |
Joost38H | 5:81d3b53087c0 | 504 | { //printf("groter"); |
Joost38H | 5:81d3b53087c0 | 505 | speedfactor2=0; |
Joost38H | 5:81d3b53087c0 | 506 | } |
Joost38H | 0:eb16ed402ffa | 507 | } |
Joost38H | 5:81d3b53087c0 | 508 | else |
Joost38H | 5:81d3b53087c0 | 509 | { speedfactor2 = 0; |
Joost38H | 5:81d3b53087c0 | 510 | //pc.printf("speedfactor nul;"); |
Joost38H | 0:eb16ed402ffa | 511 | } |
Joost38H | 0:eb16ed402ffa | 512 | |
Joost38H | 0:eb16ed402ffa | 513 | return referenceVelocity2; |
Joost38H | 5:81d3b53087c0 | 514 | } |
Joost38H | 0:eb16ed402ffa | 515 | |
Joost38H | 5:81d3b53087c0 | 516 | double GetReferenceVelocity3() |
Joost38H | 5:81d3b53087c0 | 517 | { |
Joost38H | 5:81d3b53087c0 | 518 | // Returns reference velocity in rad/s. Positive value means clockwise rotation. |
Joost38H | 5:81d3b53087c0 | 519 | double maxVelocity3=Vex*25+Vey*25; // max 8.4 in rad/s of course! |
Joost38H | 5:81d3b53087c0 | 520 | referenceVelocity3 = (-1)*speedfactor3 * maxVelocity3; |
Joost38H | 5:81d3b53087c0 | 521 | |
Joost38H | 5:81d3b53087c0 | 522 | if (Encoder3.getPulses() < (dcountr+10)) |
Joost38H | 5:81d3b53087c0 | 523 | { speedfactor3 = -0.01; |
Joost38H | 5:81d3b53087c0 | 524 | if (Encoder3.getPulses() > (dcountr-10)) |
Joost38H | 5:81d3b53087c0 | 525 | { //printf("kleiner22222222222"); |
Joost38H | 5:81d3b53087c0 | 526 | speedfactor3=0; |
Joost38H | 5:81d3b53087c0 | 527 | } |
Joost38H | 5:81d3b53087c0 | 528 | } |
Joost38H | 5:81d3b53087c0 | 529 | else if (Encoder3.getPulses() > (dcountr-10)) |
Joost38H | 5:81d3b53087c0 | 530 | { speedfactor3 = 0.01; |
Joost38H | 5:81d3b53087c0 | 531 | if (Encoder3.getPulses() < (dcountr+10)) |
Joost38H | 5:81d3b53087c0 | 532 | { //printf("groter"); |
Joost38H | 5:81d3b53087c0 | 533 | speedfactor3=0; |
Joost38H | 5:81d3b53087c0 | 534 | } |
Joost38H | 5:81d3b53087c0 | 535 | } |
Joost38H | 5:81d3b53087c0 | 536 | else |
Joost38H | 5:81d3b53087c0 | 537 | { speedfactor3 = 0; |
Joost38H | 5:81d3b53087c0 | 538 | //pc.printf("speedfactor nul;"); |
Joost38H | 5:81d3b53087c0 | 539 | } |
Joost38H | 5:81d3b53087c0 | 540 | |
Joost38H | 5:81d3b53087c0 | 541 | return referenceVelocity3; |
Joost38H | 5:81d3b53087c0 | 542 | } |
Joost38H | 5:81d3b53087c0 | 543 | |
Joost38H | 0:eb16ed402ffa | 544 | void SetMotor1(double motorValue1) |
Joost38H | 0:eb16ed402ffa | 545 | { |
Joost38H | 0:eb16ed402ffa | 546 | // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes |
Joost38H | 0:eb16ed402ffa | 547 | // motor rotating clockwise. motorValues outside range are truncated to within range |
Joost38H | 0:eb16ed402ffa | 548 | if (motorValue1 >=0) motor1DirectionPin=1; |
Joost38H | 0:eb16ed402ffa | 549 | else motor1DirectionPin=0; |
Joost38H | 0:eb16ed402ffa | 550 | if (fabs(motorValue1)>1) motor1MagnitudePin = 1; |
Joost38H | 0:eb16ed402ffa | 551 | else motor1MagnitudePin = fabs(motorValue1); |
Joost38H | 0:eb16ed402ffa | 552 | |
Joost38H | 0:eb16ed402ffa | 553 | } |
Joost38H | 5:81d3b53087c0 | 554 | |
Joost38H | 0:eb16ed402ffa | 555 | void SetMotor2(double motorValue2) |
Joost38H | 0:eb16ed402ffa | 556 | { |
Joost38H | 0:eb16ed402ffa | 557 | // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes |
Joost38H | 0:eb16ed402ffa | 558 | // motor rotating clockwise. motorValues outside range are truncated to within range |
Joost38H | 0:eb16ed402ffa | 559 | if (motorValue2 >=0) motor2DirectionPin=1; |
Joost38H | 0:eb16ed402ffa | 560 | else motor2DirectionPin=0; |
Joost38H | 0:eb16ed402ffa | 561 | if (fabs(motorValue2)>1) motor2MagnitudePin = 1; |
Joost38H | 0:eb16ed402ffa | 562 | else motor2MagnitudePin = fabs(motorValue2); |
Joost38H | 0:eb16ed402ffa | 563 | |
Joost38H | 0:eb16ed402ffa | 564 | } |
Joost38H | 0:eb16ed402ffa | 565 | |
Joost38H | 5:81d3b53087c0 | 566 | void SetMotor3(double motorValue3) |
Joost38H | 5:81d3b53087c0 | 567 | { |
Joost38H | 5:81d3b53087c0 | 568 | // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes |
Joost38H | 5:81d3b53087c0 | 569 | // motor rotating clockwise. motorValues outside range are truncated to within range |
Joost38H | 5:81d3b53087c0 | 570 | if (motorValue3 >=0) motor3DirectionPin=1; |
Joost38H | 5:81d3b53087c0 | 571 | else motor3DirectionPin=0; |
Joost38H | 5:81d3b53087c0 | 572 | if (fabs(motorValue3)>1) motor3MagnitudePin = 1; |
Joost38H | 5:81d3b53087c0 | 573 | else motor3MagnitudePin = fabs(motorValue3); |
Joost38H | 5:81d3b53087c0 | 574 | |
Joost38H | 5:81d3b53087c0 | 575 | } |
Joost38H | 5:81d3b53087c0 | 576 | |
Joost38H | 0:eb16ed402ffa | 577 | double FeedForwardControl1(double referenceVelocity1) |
Joost38H | 0:eb16ed402ffa | 578 | { |
Joost38H | 0:eb16ed402ffa | 579 | // very simple linear feed-forward control |
Joost38H | 0:eb16ed402ffa | 580 | const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4 |
Joost38H | 0:eb16ed402ffa | 581 | double motorValue1 = referenceVelocity1 / MotorGain; |
Joost38H | 0:eb16ed402ffa | 582 | return motorValue1; |
Joost38H | 0:eb16ed402ffa | 583 | } |
Joost38H | 5:81d3b53087c0 | 584 | |
Joost38H | 0:eb16ed402ffa | 585 | double FeedForwardControl2(double referenceVelocity2) |
Joost38H | 0:eb16ed402ffa | 586 | { |
Joost38H | 0:eb16ed402ffa | 587 | // very simple linear feed-forward control |
Joost38H | 0:eb16ed402ffa | 588 | const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4 |
Joost38H | 0:eb16ed402ffa | 589 | double motorValue2 = referenceVelocity2 / MotorGain; |
Joost38H | 0:eb16ed402ffa | 590 | return motorValue2; |
Joost38H | 0:eb16ed402ffa | 591 | } |
Joost38H | 5:81d3b53087c0 | 592 | |
Joost38H | 5:81d3b53087c0 | 593 | double FeedForwardControl3(double referenceVelocity3) |
Joost38H | 5:81d3b53087c0 | 594 | { |
Joost38H | 5:81d3b53087c0 | 595 | // very simple linear feed-forward control |
Joost38H | 5:81d3b53087c0 | 596 | const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4 |
Joost38H | 5:81d3b53087c0 | 597 | double motorValue3 = referenceVelocity3 / MotorGain; |
Joost38H | 5:81d3b53087c0 | 598 | return motorValue3; |
Joost38H | 5:81d3b53087c0 | 599 | } |
Joost38H | 0:eb16ed402ffa | 600 | |
Joost38H | 0:eb16ed402ffa | 601 | void MeasureAndControl1() |
Joost38H | 0:eb16ed402ffa | 602 | { |
Joost38H | 0:eb16ed402ffa | 603 | // This function measures the potmeter position, extracts a reference velocity from it, |
Joost38H | 0:eb16ed402ffa | 604 | // and controls the motor with a simple FeedForward controller. Call this from a Ticker. |
Joost38H | 0:eb16ed402ffa | 605 | double referenceVelocity1 = GetReferenceVelocity1(); |
Joost38H | 0:eb16ed402ffa | 606 | double motorValue1 = FeedForwardControl1(referenceVelocity1); |
Joost38H | 0:eb16ed402ffa | 607 | SetMotor1(motorValue1); |
Joost38H | 0:eb16ed402ffa | 608 | } |
Joost38H | 5:81d3b53087c0 | 609 | |
Joost38H | 0:eb16ed402ffa | 610 | void MeasureAndControl2() |
Joost38H | 0:eb16ed402ffa | 611 | { |
Joost38H | 0:eb16ed402ffa | 612 | // This function measures the potmeter position, extracts a reference velocity from it, |
Joost38H | 0:eb16ed402ffa | 613 | // and controls the motor with a simple FeedForward controller. Call this from a Ticker. |
Joost38H | 0:eb16ed402ffa | 614 | double referenceVelocity2 = GetReferenceVelocity2(); |
Joost38H | 0:eb16ed402ffa | 615 | double motorValue2 = FeedForwardControl2(referenceVelocity2); |
Joost38H | 0:eb16ed402ffa | 616 | SetMotor2(motorValue2); |
Joost38H | 0:eb16ed402ffa | 617 | } |
Joost38H | 0:eb16ed402ffa | 618 | |
Joost38H | 5:81d3b53087c0 | 619 | void MeasureAndControl3() |
Joost38H | 5:81d3b53087c0 | 620 | { |
Joost38H | 5:81d3b53087c0 | 621 | // This function measures the potmeter position, extracts a reference velocity from it, |
Joost38H | 5:81d3b53087c0 | 622 | // and controls the motor with a simple FeedForward controller. Call this from a Ticker. |
Joost38H | 5:81d3b53087c0 | 623 | double referenceVelocity3 = GetReferenceVelocity3(); |
Joost38H | 5:81d3b53087c0 | 624 | double motorValue3 = FeedForwardControl3(referenceVelocity3); |
Joost38H | 5:81d3b53087c0 | 625 | SetMotor3(motorValue3); |
Joost38H | 5:81d3b53087c0 | 626 | } |
Joost38H | 5:81d3b53087c0 | 627 | |
Joost38H | 0:eb16ed402ffa | 628 | void readdata1() |
Joost38H | 0:eb16ed402ffa | 629 | { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState()); |
Joost38H | 5:81d3b53087c0 | 630 | //pc.printf("Pulses_M1 = %i \r\n",Encoder1.getPulses()); |
Joost38H | 0:eb16ed402ffa | 631 | //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions()); |
Joost38H | 5:81d3b53087c0 | 632 | //pc.printf("Delta_M1 = %i \r\n",delta1); |
Joost38H | 0:eb16ed402ffa | 633 | } |
Joost38H | 0:eb16ed402ffa | 634 | |
Joost38H | 0:eb16ed402ffa | 635 | void readdata2() |
Joost38H | 0:eb16ed402ffa | 636 | { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState()); |
Joost38H | 5:81d3b53087c0 | 637 | //pc.printf("Pulses_M2 = %i \r\n",Encoder2.getPulses()); |
Joost38H | 0:eb16ed402ffa | 638 | //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions()); |
Joost38H | 5:81d3b53087c0 | 639 | //pc.printf("Delta_M2 = %i \r\n",delta2); |
Joost38H | 0:eb16ed402ffa | 640 | } |
Joost38H | 5:81d3b53087c0 | 641 | |
Joost38H | 5:81d3b53087c0 | 642 | void readdata3() |
Joost38H | 5:81d3b53087c0 | 643 | { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState()); |
Joost38H | 5:81d3b53087c0 | 644 | //pc.printf("Pulses_M2 = %i \r\n",Encoder3.getPulses()); |
Joost38H | 5:81d3b53087c0 | 645 | //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions()); |
Joost38H | 5:81d3b53087c0 | 646 | //pc.printf("Delta_M2 = %i \r\n",delta3); |
Joost38H | 5:81d3b53087c0 | 647 | } |
Joost38H | 5:81d3b53087c0 | 648 | |
Joost38H | 0:eb16ed402ffa | 649 | // einde deel motor------------------------------------------------------------------------------------ |
Joost38H | 5:81d3b53087c0 | 650 | |
Joost38H | 0:eb16ed402ffa | 651 | Ticker loop; |
Joost38H | 5:81d3b53087c0 | 652 | |
Joost38H | 0:eb16ed402ffa | 653 | /*Calculates ropelengths that are needed to get to new positions, based on the |
Joost38H | 0:eb16ed402ffa | 654 | set coordinates and the position of the poles */ |
Joost38H | 0:eb16ed402ffa | 655 | double touwlengtes(){ |
Joost38H | 0:eb16ed402ffa | 656 | Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2)); |
Joost38H | 0:eb16ed402ffa | 657 | Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2)); |
Joost38H | 0:eb16ed402ffa | 658 | Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2)); |
Joost38H | 0:eb16ed402ffa | 659 | return 0; |
Joost38H | 0:eb16ed402ffa | 660 | } |
Joost38H | 5:81d3b53087c0 | 661 | |
Joost38H | 0:eb16ed402ffa | 662 | /* Calculates rotations (and associated counts) of the motor to get to the |
Joost38H | 0:eb16ed402ffa | 663 | desired new position*/ |
Joost38H | 0:eb16ed402ffa | 664 | double turns(){ |
Joost38H | 0:eb16ed402ffa | 665 | |
Joost38H | 0:eb16ed402ffa | 666 | roto=Lou/omtrekklosje; |
Joost38H | 0:eb16ed402ffa | 667 | rotb=Lbu/omtrekklosje; |
Joost38H | 0:eb16ed402ffa | 668 | rotr=Lru/omtrekklosje; |
Joost38H | 0:eb16ed402ffa | 669 | counto=roto*4200; |
Joost38H | 5:81d3b53087c0 | 670 | dcounto=counto-hcounto; |
Joost38H | 5:81d3b53087c0 | 671 | pc.printf("dcounto = %f \n\r",dcounto); |
Joost38H | 0:eb16ed402ffa | 672 | countb=rotb*4200; |
Joost38H | 5:81d3b53087c0 | 673 | dcountb=countb-hcountb; |
Joost38H | 5:81d3b53087c0 | 674 | pc.printf("dcountb = %f \n\r",dcountb); |
Joost38H | 0:eb16ed402ffa | 675 | countr=rotr*4200; |
Joost38H | 5:81d3b53087c0 | 676 | dcountr=countr-hcountr; |
Joost38H | 5:81d3b53087c0 | 677 | |
Joost38H | 0:eb16ed402ffa | 678 | return 0; |
Joost38H | 0:eb16ed402ffa | 679 | } |
Joost38H | 5:81d3b53087c0 | 680 | |
Joost38H | 0:eb16ed402ffa | 681 | // Waar komen Pstx en Psty vandaan en waar staan ze voor? En is dit maar voor een paal? |
Joost38H | 0:eb16ed402ffa | 682 | double Pst(){ |
Joost38H | 0:eb16ed402ffa | 683 | Pstx=Pex+Vex*T; |
Joost38H | 0:eb16ed402ffa | 684 | Psty=Pey+Vey*T; |
Joost38H | 0:eb16ed402ffa | 685 | touwlengtes(); |
Joost38H | 0:eb16ed402ffa | 686 | Pex=Pstx; |
Joost38H | 0:eb16ed402ffa | 687 | Pey=Psty; |
Joost38H | 5:81d3b53087c0 | 688 | pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty); |
Joost38H | 0:eb16ed402ffa | 689 | //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru); |
Joost38H | 0:eb16ed402ffa | 690 | turns(); |
Joost38H | 0:eb16ed402ffa | 691 | //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr); |
Joost38H | 0:eb16ed402ffa | 692 | pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr); |
Joost38H | 0:eb16ed402ffa | 693 | /*float R; |
Joost38H | 0:eb16ed402ffa | 694 | 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 | 695 | pc.printf("\n\r R=%f",R);*/ |
Joost38H | 0:eb16ed402ffa | 696 | return 0; |
Joost38H | 0:eb16ed402ffa | 697 | } |
Joost38H | 5:81d3b53087c0 | 698 | |
Joost38H | 0:eb16ed402ffa | 699 | //Calculating desired end position based on the EMG input <- Waarom maar voor een paal? |
Joost38H | 0:eb16ed402ffa | 700 | double Ps(){ |
Joost38H | 5:81d3b53087c0 | 701 | if (Move_done==true); |
Joost38H | 3:59b504840b95 | 702 | Psx=(Xin_new)*30+121; |
Joost38H | 3:59b504840b95 | 703 | Psy=(Yin_new)*30+308; |
Joost38H | 5:81d3b53087c0 | 704 | pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy); |
Joost38H | 5:81d3b53087c0 | 705 | hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje); |
Joost38H | 5:81d3b53087c0 | 706 | hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje); |
Joost38H | 5:81d3b53087c0 | 707 | hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje); |
Joost38H | 0:eb16ed402ffa | 708 | return 0; |
Joost38H | 0:eb16ed402ffa | 709 | } |
Joost38H | 5:81d3b53087c0 | 710 | |
Joost38H | 0:eb16ed402ffa | 711 | // Rekent dit de snelheid uit waarmee de motoren bewegen? |
Joost38H | 0:eb16ed402ffa | 712 | void Ve(){ |
Joost38H | 0:eb16ed402ffa | 713 | Vex=Kz*(Psx-Pex); |
Joost38H | 0:eb16ed402ffa | 714 | Vey=Kz*(Psy-Pey); |
Joost38H | 0:eb16ed402ffa | 715 | modVe=sqrt(pow(Vex,2)+pow(Vey,2)); |
Joost38H | 0:eb16ed402ffa | 716 | if(modVe>Vmax){ |
Joost38H | 0:eb16ed402ffa | 717 | Vex=(Vex/modVe)*Vmax; |
Joost38H | 0:eb16ed402ffa | 718 | Vey=(Vey/modVe)*Vmax; |
Joost38H | 0:eb16ed402ffa | 719 | } |
Joost38H | 0:eb16ed402ffa | 720 | Pst(); |
Joost38H | 5:81d3b53087c0 | 721 | //pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey); |
Joost38H | 0:eb16ed402ffa | 722 | if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){ |
Joost38H | 3:59b504840b95 | 723 | Move_done=true; |
Joost38H | 0:eb16ed402ffa | 724 | loop.detach(); |
Joost38H | 0:eb16ed402ffa | 725 | } |
Joost38H | 0:eb16ed402ffa | 726 | } |
Joost38H | 5:81d3b53087c0 | 727 | |
Joost38H | 0:eb16ed402ffa | 728 | // Calculating the desired position, so that the motors can go here |
Joost38H | 0:eb16ed402ffa | 729 | int calculator(){ |
Joost38H | 0:eb16ed402ffa | 730 | Ps(); |
Joost38H | 0:eb16ed402ffa | 731 | loop.attach(&Ve,0.02); |
Joost38H | 0:eb16ed402ffa | 732 | return 0; |
Joost38H | 0:eb16ed402ffa | 733 | } |
Joost38H | 5:81d3b53087c0 | 734 | |
Joost38H | 0:eb16ed402ffa | 735 | // Function which makes it possible to lower the end-effector to pick up a piece |
Joost38H | 0:eb16ed402ffa | 736 | void zakker(){ |
Joost38H | 0:eb16ed402ffa | 737 | while(1){ |
Joost38H | 0:eb16ed402ffa | 738 | wait(1); |
Joost38H | 3:59b504840b95 | 739 | 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 | 740 | dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken |
Joost38H | 0:eb16ed402ffa | 741 | dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu; |
Joost38H | 0:eb16ed402ffa | 742 | dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru; |
Joost38H | 0:eb16ed402ffa | 743 | rotzo=dLod/omtrekklosje; |
Joost38H | 0:eb16ed402ffa | 744 | rotzb=dLbd/omtrekklosje; |
Joost38H | 0:eb16ed402ffa | 745 | rotzr=dLrd/omtrekklosje; |
Joost38H | 0:eb16ed402ffa | 746 | countzo=rotzo*4200; |
Joost38H | 0:eb16ed402ffa | 747 | countzb=rotzb*4200; |
Joost38H | 0:eb16ed402ffa | 748 | countzr=rotzr*4200; |
Joost38H | 0:eb16ed402ffa | 749 | |
Joost38H | 5:81d3b53087c0 | 750 | //pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat |
Joost38H | 0:eb16ed402ffa | 751 | } |
Joost38H | 0:eb16ed402ffa | 752 | } |
Joost38H | 0:eb16ed402ffa | 753 | } |
Joost38H | 5:81d3b53087c0 | 754 | |
Joost38H | 0:eb16ed402ffa | 755 | int main() |
Joost38H | 0:eb16ed402ffa | 756 | { |
Joost38H | 0:eb16ed402ffa | 757 | pc.baud(115200); |
Joost38H | 1:6aac013b0ba3 | 758 | getbqChain(); |
Joost38H | 4:fddab1c875a9 | 759 | threshold_timerR.attach(&Threshold_samplingBR, 0.002); |
Joost38H | 4:fddab1c875a9 | 760 | threshold_timerL.attach(&Threshold_samplingBL, 0.002); |
Joost38H | 2:2c4ee76dc830 | 761 | while(true){ |
Joost38H | 2:2c4ee76dc830 | 762 | sample_timer.attach(&EMG_sample, 0.002); |
Joost38H | 2:2c4ee76dc830 | 763 | wait(2.5f); |
Joost38H | 5:81d3b53087c0 | 764 | |
Joost38H | 2:2c4ee76dc830 | 765 | tellerX(); |
Joost38H | 2:2c4ee76dc830 | 766 | tellerY(); |
Joost38H | 2:2c4ee76dc830 | 767 | calculator(); |
Joost38H | 2:2c4ee76dc830 | 768 | controlmotor1.attach(&MeasureAndControl1, 0.01); |
Joost38H | 2:2c4ee76dc830 | 769 | calculatedelta1.attach(&calcdelta1, 0.01); |
Joost38H | 5:81d3b53087c0 | 770 | printdata1.attach(&readdata1, 0.5); |
Joost38H | 5:81d3b53087c0 | 771 | //controlmotor2.attach(&MeasureAndControl2, 0.01); |
Joost38H | 5:81d3b53087c0 | 772 | //calculatedelta2.attach(&calcdelta2, 0.01); |
Joost38H | 3:59b504840b95 | 773 | //printdata2.attach(&readdata2, 0.5); |
Joost38H | 5:81d3b53087c0 | 774 | //controlmotor3.attach(&MeasureAndControl3, 0.01); |
Joost38H | 5:81d3b53087c0 | 775 | //calculatedelta3.attach(&calcdelta3, 0.01); |
Joost38H | 5:81d3b53087c0 | 776 | //printdata3.attach(&readdata3, 0.5); |
Joost38H | 2:2c4ee76dc830 | 777 | //zakker(); |
Joost38H | 5:81d3b53087c0 | 778 | wait(5.0f); |
Joost38H | 2:2c4ee76dc830 | 779 | } |
Joost38H | 5:81d3b53087c0 | 780 | |
Joost38H | 5:81d3b53087c0 | 781 | } |
Joost38H | 5:81d3b53087c0 | 782 |