Remi van Veen / Mbed 2 deprecated cpfromralph1

asdfasdf

Dependencies:   HIDScope MODSERIAL PID QEI biquadFilter mbed

Fork of cpfromralph by Ralph Gerlings

main.cpp@31:d346f9244b4a, 2017-11-01 (annotated)

Committer:
ralphg_92
Date:
Wed Nov 01 11:34:46 2017 +0000
Revision:
31:d346f9244b4a
Parent:
30:2c67abcdb892
Child:
32:a779b1131977
motor emg controlled, 8 cases, not fully balanced

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
ralphg_92 29:09c1567d6148 3 #include "MODSERIAL.h"
relvorelvo 21:136a1ab8163c 4 #include "BiQuad.h"
ralphg_92 31:d346f9244b4a 5 //#include "QEI.h"
vsluiter 0:32bb76391d89 6
vsluiter 4:8b298dfada81 7 //Define objects
ralphg_92 31:d346f9244b4a 8 AnalogIn emg1_in( A0 ); // read out the signal
relvorelvo 23:e5db011bd410 9 AnalogIn emg2_in( A1 );
relvorelvo 23:e5db011bd410 10 AnalogIn emg3_in( A2 );
ralphg_92 27:ca07f895f999 11 AnalogIn emg4_in( A3 );
ralphg_92 31:d346f9244b4a 12 DigitalIn max_reader12( SW2 ); // define button press
ralphg_92 27:ca07f895f999 13 DigitalIn max_reader34( SW3 );
ralphg_92 31:d346f9244b4a 14
ralphg_92 31:d346f9244b4a 15 DigitalOut motor1direction( D4 );
ralphg_92 31:d346f9244b4a 16 PwmOut motor1pwm( D5);
ralphg_92 31:d346f9244b4a 17 PwmOut motor2pwm( D6 );
ralphg_92 31:d346f9244b4a 18 DigitalOut motor2direction( D7 );
ralphg_92 31:d346f9244b4a 19 //QEI Encoder1(D10, D11, NC, 32); // Encoder reminder
ralphg_92 31:d346f9244b4a 20 //QEI Encoder2(D12, D13, NC, 32);
relvorelvo 23:e5db011bd410 21
relvorelvo 23:e5db011bd410 22 Ticker main_timer;
relvorelvo 23:e5db011bd410 23 Ticker max_read1;
relvorelvo 23:e5db011bd410 24 Ticker max_read3;
ralphg_92 31:d346f9244b4a 25 Ticker Motorcontrol;
ralphg_92 27:ca07f895f999 26 HIDScope scope( 4 );
relvorelvo 23:e5db011bd410 27 DigitalOut red(LED_RED);
relvorelvo 23:e5db011bd410 28 DigitalOut blue(LED_BLUE);
relvorelvo 23:e5db011bd410 29 DigitalOut green(LED_GREEN);
relvorelvo 23:e5db011bd410 30 MODSERIAL pc(USBTX, USBRX);
relvorelvo 21:136a1ab8163c 31
relvorelvo 21:136a1ab8163c 32
relvorelvo 21:136a1ab8163c 33 // EMG variables
relvorelvo 23:e5db011bd410 34 //Right Biceps
relvorelvo 23:e5db011bd410 35 double emg1;
relvorelvo 23:e5db011bd410 36 double emg1highfilter;
relvorelvo 23:e5db011bd410 37 double emg1notchfilter;
relvorelvo 23:e5db011bd410 38 double emg1abs;
relvorelvo 23:e5db011bd410 39 double emg1lowfilter;
ralphg_92 27:ca07f895f999 40 double max1;
relvorelvo 24:26659f1de039 41 double maxpart1;
relvorelvo 23:e5db011bd410 42 // Left Biceps
relvorelvo 23:e5db011bd410 43 double emg2;
relvorelvo 23:e5db011bd410 44 double emg2highfilter;
relvorelvo 23:e5db011bd410 45 double emg2notchfilter;
relvorelvo 23:e5db011bd410 46 double emg2abs;
relvorelvo 23:e5db011bd410 47 double emg2lowfilter;
ralphg_92 27:ca07f895f999 48 double max2;
relvorelvo 24:26659f1de039 49 double maxpart2;
ralphg_92 27:ca07f895f999 50 // Left Lower Arm
relvorelvo 23:e5db011bd410 51 double emg3;
relvorelvo 23:e5db011bd410 52 double emg3highfilter;
relvorelvo 23:e5db011bd410 53 double emg3notchfilter;
relvorelvo 23:e5db011bd410 54 double emg3abs;
relvorelvo 23:e5db011bd410 55 double emg3lowfilter;
relvorelvo 23:e5db011bd410 56 double max3;
relvorelvo 23:e5db011bd410 57 double maxpart3;
ralphg_92 27:ca07f895f999 58 // Right Lower Arm
ralphg_92 27:ca07f895f999 59 double emg4;
ralphg_92 27:ca07f895f999 60 double emg4highfilter;
ralphg_92 27:ca07f895f999 61 double emg4notchfilter;
ralphg_92 27:ca07f895f999 62 double emg4abs;
ralphg_92 27:ca07f895f999 63 double emg4lowfilter;
ralphg_92 27:ca07f895f999 64 double max4;
ralphg_92 27:ca07f895f999 65 double maxpart4;
ralphg_92 31:d346f9244b4a 66
ralphg_92 31:d346f9244b4a 67 // Motor variables
ralphg_92 31:d346f9244b4a 68 float referenceVelocity;
ralphg_92 31:d346f9244b4a 69 float potMeterIn;
ralphg_92 31:d346f9244b4a 70 float MV1 = 0;
ralphg_92 31:d346f9244b4a 71 float MV2 = 0;
ralphg_92 29:09c1567d6148 72
relvorelvo 25:07187cf76863 73 // BiQuad Filter Settings
relvorelvo 23:e5db011bd410 74 // Right Biceps
ralphg_92 31:d346f9244b4a 75 BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); // Filter at 10 Hz
ralphg_92 31:d346f9244b4a 76 BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); // Filter at 50 Hz
ralphg_92 31:d346f9244b4a 77 BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); // Filter at 15 Hz
relvorelvo 23:e5db011bd410 78 // Left Biceps
relvorelvo 23:e5db011bd410 79 BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 80 BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 81 BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 82 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 83 BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 84 BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 85 BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
ralphg_92 27:ca07f895f999 86 // Right Lower Arm OR Triceps
ralphg_92 27:ca07f895f999 87 BiQuad filterhigh4(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
ralphg_92 27:ca07f895f999 88 BiQuad filternotch4(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
ralphg_92 27:ca07f895f999 89 BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 90 //
relvorelvo 21:136a1ab8163c 91
ralphg_92 29:09c1567d6148 92 // Finding max values for correct motor switch if the button is pressed
ralphg_92 31:d346f9244b4a 93 // calibration of both biceps
ralphg_92 31:d346f9244b4a 94
ralphg_92 29:09c1567d6148 95 void get_max1(){
ralphg_92 29:09c1567d6148 96 if (max_reader12==0){
ralphg_92 29:09c1567d6148 97 green = !green;
ralphg_92 29:09c1567d6148 98 red = 1;
ralphg_92 29:09c1567d6148 99 blue = 1;
ralphg_92 31:d346f9244b4a 100 for(int n=0;n<2000;n++){ // measure 2000 samples and filter it
ralphg_92 29:09c1567d6148 101
ralphg_92 31:d346f9244b4a 102 emg1 = emg1_in.read(); // read out emg
ralphg_92 31:d346f9244b4a 103 emg1highfilter = filterhigh1.step(emg1); // high pass filtered
ralphg_92 31:d346f9244b4a 104 emg1notchfilter = filternotch1.step(emg1highfilter); // notch filtered
ralphg_92 31:d346f9244b4a 105 emg1abs = fabs(emg1notchfilter); // take the absolute value
ralphg_92 31:d346f9244b4a 106 emg1lowfilter = filterlow1.step(emg1abs); // low pass filtered
ralphg_92 29:09c1567d6148 107
ralphg_92 31:d346f9244b4a 108 emg2 = emg2_in.read(); // read out emg
ralphg_92 31:d346f9244b4a 109 emg2highfilter = filterhigh2.step(emg2); // high pass filtered
ralphg_92 31:d346f9244b4a 110 emg2notchfilter = filternotch2.step(emg2highfilter); // notch filtered
ralphg_92 31:d346f9244b4a 111 emg2abs = fabs(emg2notchfilter); // take the absolute value
ralphg_92 31:d346f9244b4a 112 emg2lowfilter = filterlow2.step(emg2abs); // low pass filtered
ralphg_92 29:09c1567d6148 113
ralphg_92 31:d346f9244b4a 114 if (max1<emg1lowfilter){
ralphg_92 31:d346f9244b4a 115 max1 = emg1lowfilter; // set the max value at the highest measured value
ralphg_92 29:09c1567d6148 116 }
ralphg_92 31:d346f9244b4a 117 if (max2<emg2lowfilter){
ralphg_92 31:d346f9244b4a 118 max2 = emg2lowfilter; // set the max value at the highest measured value
ralphg_92 29:09c1567d6148 119 }
ralphg_92 31:d346f9244b4a 120 wait(0.001f); // measure at 1000Hz
ralphg_92 29:09c1567d6148 121 }
ralphg_92 29:09c1567d6148 122 wait(0.2f);
ralphg_92 29:09c1567d6148 123 green = 1;
ralphg_92 29:09c1567d6148 124 }
ralphg_92 31:d346f9244b4a 125 maxpart1 = 0.11*max1; // set cut off voltage at 13% of max for right biceps
ralphg_92 31:d346f9244b4a 126 maxpart2 = 0.13*max2; // set cut off voltage at 13% of max for left biceps
ralphg_92 29:09c1567d6148 127 }
ralphg_92 29:09c1567d6148 128
ralphg_92 31:d346f9244b4a 129 // calibration of both lower arms
ralphg_92 31:d346f9244b4a 130
relvorelvo 23:e5db011bd410 131 void get_max3(){
ralphg_92 27:ca07f895f999 132 if (max_reader34==0){
relvorelvo 23:e5db011bd410 133 green = 1;
relvorelvo 23:e5db011bd410 134 blue = 1;
relvorelvo 23:e5db011bd410 135 red = !red;
relvorelvo 23:e5db011bd410 136 for(int n=0;n<2000;n++){
relvorelvo 23:e5db011bd410 137
relvorelvo 23:e5db011bd410 138 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 139 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 140 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 141 emg3abs = fabs(emg3notchfilter);
relvorelvo 23:e5db011bd410 142 emg3lowfilter = filterlow3.step(emg3abs);
relvorelvo 23:e5db011bd410 143
ralphg_92 27:ca07f895f999 144 emg4 = emg4_in.read();
ralphg_92 27:ca07f895f999 145 emg4highfilter = filterhigh4.step(emg4);
ralphg_92 27:ca07f895f999 146 emg4notchfilter = filternotch4.step(emg4highfilter);
ralphg_92 27:ca07f895f999 147 emg4abs = fabs(emg4notchfilter);
ralphg_92 27:ca07f895f999 148 emg4lowfilter = filterlow4.step(emg4abs);
ralphg_92 27:ca07f895f999 149
ralphg_92 31:d346f9244b4a 150 if (max3<emg3lowfilter){
ralphg_92 31:d346f9244b4a 151 max3 = emg3lowfilter; // set the max value at the highest measured value
relvorelvo 23:e5db011bd410 152 }
ralphg_92 31:d346f9244b4a 153 if (max4<emg4lowfilter){
ralphg_92 31:d346f9244b4a 154 max4 = emg4lowfilter; // set the max value at the highest measured value
ralphg_92 27:ca07f895f999 155 }
relvorelvo 23:e5db011bd410 156 wait(0.001f);
relvorelvo 23:e5db011bd410 157 }
relvorelvo 23:e5db011bd410 158 wait(0.2f);
relvorelvo 23:e5db011bd410 159 red = 1;
relvorelvo 23:e5db011bd410 160 }
ralphg_92 31:d346f9244b4a 161 maxpart3 = 0.15*max3; // set cut off voltage at 15% of max for left lower arm
ralphg_92 31:d346f9244b4a 162 maxpart4 = 0.15*max4; // set cut off voltage at 15% of max for right lower arm
vsluiter 2:e314bb3b2d99 163 }
vsluiter 0:32bb76391d89 164
relvorelvo 23:e5db011bd410 165 // Filtering & Scope
relvorelvo 23:e5db011bd410 166 void filter() {
relvorelvo 23:e5db011bd410 167 // Right Biceps
relvorelvo 23:e5db011bd410 168 emg1 = emg1_in.read();
relvorelvo 23:e5db011bd410 169 emg1highfilter = filterhigh1.step(emg1);
relvorelvo 23:e5db011bd410 170 emg1notchfilter = filternotch1.step(emg1highfilter);
relvorelvo 23:e5db011bd410 171 emg1abs = fabs(emg1notchfilter);
ralphg_92 31:d346f9244b4a 172 emg1lowfilter = filterlow1.step(emg1abs); // Final Right Biceps values to be sent
relvorelvo 23:e5db011bd410 173 // Left Biceps
relvorelvo 23:e5db011bd410 174 emg2 = emg2_in.read();
relvorelvo 23:e5db011bd410 175 emg2highfilter = filterhigh2.step(emg2);
relvorelvo 23:e5db011bd410 176 emg2notchfilter = filternotch2.step(emg2highfilter);
relvorelvo 23:e5db011bd410 177 emg2abs = fabs(emg2notchfilter);
ralphg_92 31:d346f9244b4a 178 emg2lowfilter = filterlow2.step(emg2abs); // Final Left Biceps values to be sent
relvorelvo 23:e5db011bd410 179 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 180 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 181 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 182 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 183 emg3abs = fabs(emg3notchfilter);
ralphg_92 31:d346f9244b4a 184 emg3lowfilter = filterlow3.step(emg3abs); // Final Lower Arm values to be sent
ralphg_92 27:ca07f895f999 185 // Right Lower Arm OR Triceps
ralphg_92 27:ca07f895f999 186 emg4 = emg4_in.read();
ralphg_92 27:ca07f895f999 187 emg4highfilter = filterhigh4.step(emg4);
ralphg_92 27:ca07f895f999 188 emg4notchfilter = filternotch4.step(emg4highfilter);
ralphg_92 27:ca07f895f999 189 emg4abs = fabs(emg4notchfilter);
ralphg_92 31:d346f9244b4a 190 emg4lowfilter = filterlow4.step(emg4abs); // Final Lower Arm values to be sent
ralphg_92 29:09c1567d6148 191
relvorelvo 23:e5db011bd410 192
ralphg_92 31:d346f9244b4a 193 // Compare measurement to the calibrated value to decide actions
ralphg_92 31:d346f9244b4a 194 // more options could be added if the callibration is well defined enough
ralphg_92 31:d346f9244b4a 195 // This part checks for right biceps contractions only
ralphg_92 31:d346f9244b4a 196 if (maxpart1<emg1lowfilter && maxpart2>emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
ralphg_92 29:09c1567d6148 197 red = 1;
ralphg_92 29:09c1567d6148 198 blue = 1;
ralphg_92 29:09c1567d6148 199 green = 0;
ralphg_92 31:d346f9244b4a 200 MV1 = 0.5;
ralphg_92 31:d346f9244b4a 201 MV2 = 0;
ralphg_92 31:d346f9244b4a 202 }
ralphg_92 31:d346f9244b4a 203 // This part checks for left biceps contractions only
ralphg_92 31:d346f9244b4a 204 else if (maxpart1>emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
ralphg_92 29:09c1567d6148 205 red = 0;
ralphg_92 29:09c1567d6148 206 blue = 1;
ralphg_92 29:09c1567d6148 207 green = 1;
ralphg_92 31:d346f9244b4a 208 MV1 = -0.5;
ralphg_92 31:d346f9244b4a 209 MV2 = 0;
ralphg_92 31:d346f9244b4a 210 }
ralphg_92 31:d346f9244b4a 211 // This part checks for left lower arm contractions only
ralphg_92 31:d346f9244b4a 212 else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4>emg4lowfilter){
relvorelvo 23:e5db011bd410 213 red = 1;
relvorelvo 23:e5db011bd410 214 blue = 0;
relvorelvo 23:e5db011bd410 215 green = 1;
ralphg_92 31:d346f9244b4a 216 MV1 = 0;
ralphg_92 31:d346f9244b4a 217 MV2 = 0.5;
ralphg_92 31:d346f9244b4a 218 }
ralphg_92 31:d346f9244b4a 219 // This part checks for right lower arm contractions only
ralphg_92 31:d346f9244b4a 220 else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3>emg3lowfilter && maxpart4<emg4lowfilter){
ralphg_92 29:09c1567d6148 221 red = 0;
relvorelvo 23:e5db011bd410 222 blue = 1;
relvorelvo 23:e5db011bd410 223 green = 0;
ralphg_92 31:d346f9244b4a 224 MV1 = 0;
ralphg_92 31:d346f9244b4a 225 MV2 = -0.5;
ralphg_92 31:d346f9244b4a 226 }
ralphg_92 31:d346f9244b4a 227 // This part checks for both lower arm contractions only
ralphg_92 31:d346f9244b4a 228 else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4<emg4lowfilter){
ralphg_92 31:d346f9244b4a 229 red = 0;
ralphg_92 31:d346f9244b4a 230 blue = 0;
ralphg_92 31:d346f9244b4a 231 green = 0;
ralphg_92 31:d346f9244b4a 232 MV1 = -0.5;
ralphg_92 31:d346f9244b4a 233 MV2 = -0.5;
ralphg_92 31:d346f9244b4a 234 }
ralphg_92 31:d346f9244b4a 235 // This part checks for both biceps contractions only
ralphg_92 31:d346f9244b4a 236 else if (maxpart1<emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
ralphg_92 31:d346f9244b4a 237 red = 0;
ralphg_92 31:d346f9244b4a 238 blue = 0;
ralphg_92 31:d346f9244b4a 239 green = 0;
ralphg_92 31:d346f9244b4a 240 MV1 = 0.5;
ralphg_92 31:d346f9244b4a 241 MV2 = 0.5;
ralphg_92 31:d346f9244b4a 242 }
ralphg_92 31:d346f9244b4a 243 // This part checks for right lower arm & left biceps contractions only
ralphg_92 31:d346f9244b4a 244 else if (maxpart1>emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4<emg4lowfilter){
ralphg_92 30:2c67abcdb892 245 red = 0;
ralphg_92 30:2c67abcdb892 246 blue = 0;
ralphg_92 30:2c67abcdb892 247 green = 0;
ralphg_92 31:d346f9244b4a 248 MV1 = 0.5;
ralphg_92 31:d346f9244b4a 249 MV2 = -0.5;
ralphg_92 31:d346f9244b4a 250 }
ralphg_92 31:d346f9244b4a 251 // This part checks for left lower arm & right biceps contractions only
ralphg_92 31:d346f9244b4a 252 else if (maxpart1<emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4>emg4lowfilter){
ralphg_92 30:2c67abcdb892 253 red = 0;
ralphg_92 30:2c67abcdb892 254 blue = 0;
ralphg_92 30:2c67abcdb892 255 green = 0;
ralphg_92 31:d346f9244b4a 256 MV1 = -0.5;
ralphg_92 31:d346f9244b4a 257 MV2 = 0.5;
ralphg_92 31:d346f9244b4a 258 }
ralphg_92 31:d346f9244b4a 259 else {
ralphg_92 31:d346f9244b4a 260 red = 1; // Shut down all led colors if no movement is registered
relvorelvo 23:e5db011bd410 261 blue = 1;
relvorelvo 23:e5db011bd410 262 green = 1;
ralphg_92 31:d346f9244b4a 263 MV1 = 0;
ralphg_92 31:d346f9244b4a 264 MV2 = 0;
ralphg_92 29:09c1567d6148 265 //pc.printf( "No contraction registered\n");
ralphg_92 29:09c1567d6148 266 }
ralphg_92 29:09c1567d6148 267
ralphg_92 31:d346f9244b4a 268 // Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
ralphg_92 31:d346f9244b4a 269 scope.set(0, emg1lowfilter ); // plot Right biceps voltage
ralphg_92 31:d346f9244b4a 270 scope.set(1, emg2lowfilter ); // Plot Left biceps voltage
ralphg_92 31:d346f9244b4a 271 scope.set(2, emg3lowfilter ); // Plot Lower Left Arm voltage
ralphg_92 31:d346f9244b4a 272 scope.set(3, emg4lowfilter ); // Plot Lower Right Arm Voltage
ralphg_92 31:d346f9244b4a 273 scope.send(); // send everything to the HID scope
ralphg_92 31:d346f9244b4a 274
relvorelvo 23:e5db011bd410 275 }
tomlankhorst 15:0da764eea774 276
ralphg_92 31:d346f9244b4a 277 // check MV1 to see if motor1 needs to be activated
ralphg_92 31:d346f9244b4a 278 void SetMotor1(float MV1) {
ralphg_92 31:d346f9244b4a 279 motor1pwm = fabs(MV1); // motor speed
ralphg_92 31:d346f9244b4a 280 if (MV1 >=0) {
ralphg_92 31:d346f9244b4a 281 motor1direction = 1; // clockwise rotation
ralphg_92 31:d346f9244b4a 282 }
ralphg_92 31:d346f9244b4a 283 else {
ralphg_92 31:d346f9244b4a 284 motor1direction = 0; // counterclockwise rotation
ralphg_92 31:d346f9244b4a 285 }
ralphg_92 31:d346f9244b4a 286 }
ralphg_92 31:d346f9244b4a 287 // check MV2 if motor1 needs to be activated
ralphg_92 31:d346f9244b4a 288 void SetMotor2(float MV2) {
ralphg_92 31:d346f9244b4a 289 motor2pwm = fabs(MV2); // motor speed
ralphg_92 31:d346f9244b4a 290 if (MV2 >=0) {
ralphg_92 31:d346f9244b4a 291 motor2direction = 1; // clockwise rotation
ralphg_92 31:d346f9244b4a 292 }
ralphg_92 31:d346f9244b4a 293 else {
ralphg_92 31:d346f9244b4a 294 motor2direction = 0; // counterclockwise rotation
ralphg_92 31:d346f9244b4a 295 }
ralphg_92 31:d346f9244b4a 296 }
ralphg_92 31:d346f9244b4a 297
ralphg_92 31:d346f9244b4a 298 void MeasureAndControl(void)
ralphg_92 31:d346f9244b4a 299 {
ralphg_92 31:d346f9244b4a 300 // This function measures the potmeter position, extracts a
ralphg_92 31:d346f9244b4a 301 // reference velocity from it, and controls the motor with
ralphg_92 31:d346f9244b4a 302 // a simple FeedForward controller. Call this from a Ticker.
ralphg_92 31:d346f9244b4a 303 SetMotor1(MV1);
ralphg_92 31:d346f9244b4a 304 SetMotor2(MV2);
ralphg_92 31:d346f9244b4a 305 }
ralphg_92 31:d346f9244b4a 306
ralphg_92 31:d346f9244b4a 307
relvorelvo 23:e5db011bd410 308 int main(){
relvorelvo 22:68ab712b62b2 309
ralphg_92 31:d346f9244b4a 310 main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
ralphg_92 31:d346f9244b4a 311 max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
relvorelvo 23:e5db011bd410 312 max_read3.attach(&get_max3, 2);
ralphg_92 31:d346f9244b4a 313 Motorcontrol.attach(MeasureAndControl,0.5);
tomlankhorst 15:0da764eea774 314 while(1) {}
vsluiter 0:32bb76391d89 315 }