Thijs Lubberman / Mbed 2 deprecated MotorArchitecture1-11

juijiu

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture1-11 by Wouter Schuttert

main.cpp@2:fa90eaa14f99, 2018-10-26 (annotated)

Committer:
WouterJS
Date:
Fri Oct 26 10:59:24 2018 +0000
Revision:
2:fa90eaa14f99
Parent:
1:a9c933f1dc71
Child:
3:055eb9f256fc
ss

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WouterJS 0:3710031b2621 1 #include "mbed.h"
WouterJS 0:3710031b2621 2 #include "BiQuad.h"
WouterJS 0:3710031b2621 3 #include "HIDScope.h"
WouterJS 1:a9c933f1dc71 4 #include <stdio.h>
WouterJS 1:a9c933f1dc71 5 #include <math.h>
WouterJS 1:a9c933f1dc71 6 #include <iostream>
WouterJS 1:a9c933f1dc71 7 #include "QEI.h"
WouterJS 1:a9c933f1dc71 8
WouterJS 1:a9c933f1dc71 9
WouterJS 1:a9c933f1dc71 10 Serial pc(USBTX,USBRX);
WouterJS 1:a9c933f1dc71 11 Timer timer;
WouterJS 1:a9c933f1dc71 12 float Ts = 0.002;
WouterJS 1:a9c933f1dc71 13 int sensor_sensitivity = 32;
WouterJS 1:a9c933f1dc71 14 int gear_ratio = 131;
WouterJS 1:a9c933f1dc71 15 float full_ratio = gear_ratio*sensor_sensitivity*4;
WouterJS 1:a9c933f1dc71 16
WouterJS 1:a9c933f1dc71 17 QEI Encoder1(D10,D11,NC,sensor_sensitivity); //First one is B, Second one is A
WouterJS 1:a9c933f1dc71 18 QEI Encoder2(D12,D13,NC,sensor_sensitivity); //
WouterJS 0:3710031b2621 19
WouterJS 1:a9c933f1dc71 20 int counts_m1 = 0;
WouterJS 1:a9c933f1dc71 21 int counts_m2 = 0;
WouterJS 1:a9c933f1dc71 22 int counts_m1_prev = 0;
WouterJS 1:a9c933f1dc71 23 int counts_m2_prev = 0;
WouterJS 1:a9c933f1dc71 24 float deg_m1 = 0;
WouterJS 1:a9c933f1dc71 25 float deg_m2 = 0;
WouterJS 1:a9c933f1dc71 26
WouterJS 1:a9c933f1dc71 27 //Vector2d twist(0,0);
WouterJS 1:a9c933f1dc71 28 //Matrix2f jacobian(0, 0, 0, 0), inv_jacobian(0, 0, 0, 0);
WouterJS 0:3710031b2621 29
WouterJS 0:3710031b2621 30 DigitalOut motor1_direction(D4);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
WouterJS 0:3710031b2621 31 PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1
WouterJS 1:a9c933f1dc71 32 PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
WouterJS 0:3710031b2621 33 DigitalOut motor2_direction(D7);// draairichting motor 2 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
WouterJS 1:a9c933f1dc71 34
WouterJS 1:a9c933f1dc71 35 enum States {failure, waiting, calib_motor, calib_emg, operational, demo};
WouterJS 0:3710031b2621 36 enum Operations {rest, forward, backward, up, down};
WouterJS 0:3710031b2621 37
WouterJS 1:a9c933f1dc71 38 States current_state = calib_motor;
WouterJS 0:3710031b2621 39 Operations movement = rest;
WouterJS 0:3710031b2621 40
WouterJS 1:a9c933f1dc71 41 float max1 = 0; //initial threshold value for emg signals, changes during calibration left arm
WouterJS 1:a9c933f1dc71 42 float max2 = 0; // right arm
WouterJS 1:a9c933f1dc71 43 float threshold1;
WouterJS 1:a9c933f1dc71 44 float threshold2;
WouterJS 1:a9c933f1dc71 45 float thresholdtime = 1.0; // time waiting before switching modes
WouterJS 1:a9c933f1dc71 46
WouterJS 1:a9c933f1dc71 47 Ticker loop_timer;
WouterJS 0:3710031b2621 48 Ticker sample_timer;
WouterJS 1:a9c933f1dc71 49 Ticker sample_timer2;
WouterJS 2:fa90eaa14f99 50 HIDScope scope(5);
WouterJS 0:3710031b2621 51
WouterJS 0:3710031b2621 52 AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes
WouterJS 0:3710031b2621 53 AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength
WouterJS 0:3710031b2621 54
WouterJS 1:a9c933f1dc71 55 volatile float emg1_input;
WouterJS 1:a9c933f1dc71 56 volatile float emg2_input;
WouterJS 1:a9c933f1dc71 57
WouterJS 0:3710031b2621 58 volatile float raw_filteredsignal1;//the first filtered emg signal 1
WouterJS 0:3710031b2621 59 volatile float raw_filteredsignal2;//the first filtered emg signal 2
WouterJS 0:3710031b2621 60
WouterJS 0:3710031b2621 61 volatile float filteredsignal1;//the first filtered emg signal 1
WouterJS 0:3710031b2621 62 volatile float filteredsignal2;//the first filtered emg signal 2
WouterJS 0:3710031b2621 63
WouterJS 0:3710031b2621 64 bool state_changed = false;
WouterJS 2:fa90eaa14f99 65
WouterJS 0:3710031b2621 66 void filterall()
WouterJS 0:3710031b2621 67 {
WouterJS 0:3710031b2621 68 //Highpass Biquad 5 Hz
WouterJS 1:a9c933f1dc71 69 static BiQuad HighPass1(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354);
WouterJS 2:fa90eaa14f99 70 float high1 = HighPass1.step(emg1_input);
WouterJS 1:a9c933f1dc71 71 static BiQuad HighPass2(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354);
WouterJS 1:a9c933f1dc71 72 float high2 = HighPass2.step(emg2_input);
WouterJS 0:3710031b2621 73 // Rectify the signal(absolute value)
WouterJS 2:fa90eaa14f99 74 float abs1 = fabs(high1);
WouterJS 0:3710031b2621 75 float abs2 = fabs(high2);
WouterJS 0:3710031b2621 76 //Lowpass Biquad 10 Hz
WouterJS 1:a9c933f1dc71 77 static BiQuad LowPass1(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906);
WouterJS 2:fa90eaa14f99 78 float low1 = LowPass1.step(abs1);
WouterJS 1:a9c933f1dc71 79 static BiQuad LowPass2(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906);
WouterJS 1:a9c933f1dc71 80 float low2 = LowPass2.step(abs2);
WouterJS 0:3710031b2621 81
WouterJS 0:3710031b2621 82 raw_filteredsignal1 = low1;
WouterJS 0:3710031b2621 83 raw_filteredsignal2 = low2;
WouterJS 0:3710031b2621 84
WouterJS 1:a9c933f1dc71 85
WouterJS 0:3710031b2621 86 }
WouterJS 0:3710031b2621 87
WouterJS 1:a9c933f1dc71 88 void measureall(){ // changes all variables according in sync with the rest of the code
WouterJS 1:a9c933f1dc71 89
WouterJS 1:a9c933f1dc71 90 emg1_input = raw_emg1_input.read();
WouterJS 1:a9c933f1dc71 91 emg2_input = raw_emg2_input.read();
WouterJS 1:a9c933f1dc71 92
WouterJS 1:a9c933f1dc71 93 filteredsignal1 = raw_filteredsignal1;
WouterJS 1:a9c933f1dc71 94 filteredsignal2 = raw_filteredsignal2;
WouterJS 1:a9c933f1dc71 95
WouterJS 1:a9c933f1dc71 96 //Reading of motor
WouterJS 1:a9c933f1dc71 97
WouterJS 1:a9c933f1dc71 98 counts_m1 = Encoder1.getPulses() - counts_m1_prev;
WouterJS 1:a9c933f1dc71 99 counts_m2 = Encoder1.getPulses() - counts_m2_prev;
WouterJS 1:a9c933f1dc71 100 deg_m1 = deg_m1 + counts_m1*(360/(full_ratio));
WouterJS 1:a9c933f1dc71 101 deg_m2 = deg_m2 + counts_m2*(360/(full_ratio));
WouterJS 1:a9c933f1dc71 102 counts_m1_prev = Encoder1.getPulses();
WouterJS 1:a9c933f1dc71 103 counts_m2_prev = Encoder2.getPulses();
WouterJS 1:a9c933f1dc71 104
WouterJS 1:a9c933f1dc71 105
WouterJS 1:a9c933f1dc71 106 }
WouterJS 1:a9c933f1dc71 107
WouterJS 0:3710031b2621 108 void scopedata()
WouterJS 0:3710031b2621 109 {
WouterJS 0:3710031b2621 110 scope.set(0,emg1_input); //
WouterJS 2:fa90eaa14f99 111 scope.set(1,emg1_input); //
WouterJS 2:fa90eaa14f99 112 scope.set(2,emg1_input); //
WouterJS 2:fa90eaa14f99 113 scope.set(3,emg1_input);//
WouterJS 1:a9c933f1dc71 114 scope.set(4,filteredsignal1);
WouterJS 0:3710031b2621 115 scope.send(); // send info to HIDScope server
WouterJS 0:3710031b2621 116 }
WouterJS 0:3710031b2621 117
WouterJS 1:a9c933f1dc71 118
WouterJS 1:a9c933f1dc71 119 //////////////////// MOVEMENT STATES
WouterJS 1:a9c933f1dc71 120 void do_forward(){
WouterJS 1:a9c933f1dc71 121
WouterJS 1:a9c933f1dc71 122 //twist << 1, 0;
WouterJS 1:a9c933f1dc71 123 //Vector2d twistf(0,0);
WouterJS 1:a9c933f1dc71 124 //twistf << 1, 0;
WouterJS 1:a9c933f1dc71 125 if (filteredsignal2 > threshold2){
WouterJS 1:a9c933f1dc71 126 //double abs_sig = (filteredsignal2 - (0.5*max2))/(0.5*max2);
WouterJS 1:a9c933f1dc71 127
WouterJS 1:a9c933f1dc71 128 //twist = twistf * abs_sig;
WouterJS 1:a9c933f1dc71 129
WouterJS 1:a9c933f1dc71 130 }
WouterJS 1:a9c933f1dc71 131
WouterJS 1:a9c933f1dc71 132
WouterJS 1:a9c933f1dc71 133 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 134 {
WouterJS 1:a9c933f1dc71 135 movement = backward;
WouterJS 1:a9c933f1dc71 136 timer.reset();
WouterJS 1:a9c933f1dc71 137 }
WouterJS 1:a9c933f1dc71 138 }
WouterJS 1:a9c933f1dc71 139
WouterJS 1:a9c933f1dc71 140 void do_backward(){
WouterJS 1:a9c933f1dc71 141
WouterJS 1:a9c933f1dc71 142
WouterJS 1:a9c933f1dc71 143
WouterJS 1:a9c933f1dc71 144 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 145 {
WouterJS 1:a9c933f1dc71 146 movement = up;
WouterJS 1:a9c933f1dc71 147 timer.reset();
WouterJS 1:a9c933f1dc71 148 }
WouterJS 1:a9c933f1dc71 149 }
WouterJS 1:a9c933f1dc71 150
WouterJS 1:a9c933f1dc71 151 void do_up(){
WouterJS 1:a9c933f1dc71 152
WouterJS 1:a9c933f1dc71 153 //Code for moving up
WouterJS 1:a9c933f1dc71 154
WouterJS 1:a9c933f1dc71 155 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 156 {
WouterJS 1:a9c933f1dc71 157 movement = down;
WouterJS 1:a9c933f1dc71 158 timer.reset();
WouterJS 1:a9c933f1dc71 159 }
WouterJS 1:a9c933f1dc71 160 }
WouterJS 1:a9c933f1dc71 161 void do_down(){
WouterJS 1:a9c933f1dc71 162
WouterJS 1:a9c933f1dc71 163 //Code for moving down
WouterJS 1:a9c933f1dc71 164
WouterJS 1:a9c933f1dc71 165 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 166 {
WouterJS 1:a9c933f1dc71 167 movement = rest;
WouterJS 1:a9c933f1dc71 168 timer.reset();
WouterJS 1:a9c933f1dc71 169
WouterJS 1:a9c933f1dc71 170 }
WouterJS 1:a9c933f1dc71 171 }
WouterJS 1:a9c933f1dc71 172 void do_wait(){
WouterJS 1:a9c933f1dc71 173
WouterJS 1:a9c933f1dc71 174 if ( filteredsignal2 > threshold2) {//
WouterJS 1:a9c933f1dc71 175 current_state = waiting;
WouterJS 1:a9c933f1dc71 176 state_changed = true;
WouterJS 1:a9c933f1dc71 177 }
WouterJS 1:a9c933f1dc71 178 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 179 {
WouterJS 1:a9c933f1dc71 180 movement = forward;
WouterJS 1:a9c933f1dc71 181 timer.reset();
WouterJS 1:a9c933f1dc71 182 }
WouterJS 1:a9c933f1dc71 183 }
WouterJS 1:a9c933f1dc71 184 ///////////END MOVEMENT STATES/////////////////////////
WouterJS 1:a9c933f1dc71 185 ///////////ROBOT ARM STATES ///////////////////////////
WouterJS 1:a9c933f1dc71 186
WouterJS 1:a9c933f1dc71 187 void do_state_failure(){
WouterJS 1:a9c933f1dc71 188
WouterJS 1:a9c933f1dc71 189 }
WouterJS 1:a9c933f1dc71 190 bool on = true;
WouterJS 1:a9c933f1dc71 191 void do_state_calib_motor(){
WouterJS 1:a9c933f1dc71 192 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 193 state_changed = false;
WouterJS 1:a9c933f1dc71 194 }
WouterJS 1:a9c933f1dc71 195
WouterJS 1:a9c933f1dc71 196
WouterJS 1:a9c933f1dc71 197 if(on){
WouterJS 1:a9c933f1dc71 198 timer.reset();
WouterJS 2:fa90eaa14f99 199
WouterJS 1:a9c933f1dc71 200 on = false;
WouterJS 1:a9c933f1dc71 201 }
WouterJS 1:a9c933f1dc71 202
WouterJS 1:a9c933f1dc71 203 int deriv1 = fabs((counts_m1 - counts_m1_prev)/Ts);
WouterJS 1:a9c933f1dc71 204 int deriv2 = fabs((counts_m2 - counts_m2_prev)/Ts);
WouterJS 1:a9c933f1dc71 205
WouterJS 1:a9c933f1dc71 206
WouterJS 1:a9c933f1dc71 207 if ( timer.read() > 5 && deriv1 < 1 && deriv2 < 1) {
WouterJS 1:a9c933f1dc71 208 motor1_speed_control = 0;
WouterJS 1:a9c933f1dc71 209 motor2_speed_control = 0;
WouterJS 1:a9c933f1dc71 210 current_state = calib_emg;
WouterJS 1:a9c933f1dc71 211 timer.reset();
WouterJS 1:a9c933f1dc71 212 state_changed = true;
WouterJS 1:a9c933f1dc71 213 }
WouterJS 1:a9c933f1dc71 214 }
WouterJS 1:a9c933f1dc71 215 void do_state_calib_emg(){
WouterJS 1:a9c933f1dc71 216 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 217 state_changed = false;
WouterJS 1:a9c933f1dc71 218 }
WouterJS 1:a9c933f1dc71 219
WouterJS 1:a9c933f1dc71 220 if(filteredsignal1 > max1){//calibrate to a new maximum
WouterJS 1:a9c933f1dc71 221 max1 = filteredsignal1;
WouterJS 1:a9c933f1dc71 222 threshold1 = 0.5*max1;
WouterJS 1:a9c933f1dc71 223 }
WouterJS 1:a9c933f1dc71 224 if(filteredsignal2 > max2){//calibrate to a new maximum
WouterJS 1:a9c933f1dc71 225 max2 = filteredsignal2;
WouterJS 1:a9c933f1dc71 226 threshold2 = 0.5 * max2;
WouterJS 1:a9c933f1dc71 227 }
WouterJS 1:a9c933f1dc71 228
WouterJS 1:a9c933f1dc71 229 if (timer.read() > 10 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 230 current_state = operational;
WouterJS 1:a9c933f1dc71 231 timer.reset();
WouterJS 1:a9c933f1dc71 232 state_changed = true;
WouterJS 1:a9c933f1dc71 233 }
WouterJS 1:a9c933f1dc71 234 }
WouterJS 1:a9c933f1dc71 235
WouterJS 1:a9c933f1dc71 236 void do_state_operational(){
WouterJS 1:a9c933f1dc71 237 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 238 state_changed = false;
WouterJS 1:a9c933f1dc71 239 }
WouterJS 1:a9c933f1dc71 240
WouterJS 1:a9c933f1dc71 241 switch(movement) {// a separate function for what happens in each state
WouterJS 1:a9c933f1dc71 242 case rest:
WouterJS 1:a9c933f1dc71 243 do_wait();
WouterJS 1:a9c933f1dc71 244 break;
WouterJS 1:a9c933f1dc71 245 case forward:
WouterJS 1:a9c933f1dc71 246 do_forward();
WouterJS 1:a9c933f1dc71 247
WouterJS 1:a9c933f1dc71 248 break;
WouterJS 1:a9c933f1dc71 249 case backward:
WouterJS 1:a9c933f1dc71 250 do_backward();
WouterJS 1:a9c933f1dc71 251
WouterJS 1:a9c933f1dc71 252 break;
WouterJS 1:a9c933f1dc71 253 case up:
WouterJS 1:a9c933f1dc71 254 do_up();
WouterJS 1:a9c933f1dc71 255 break;
WouterJS 1:a9c933f1dc71 256 case down:
WouterJS 1:a9c933f1dc71 257 do_down();
WouterJS 1:a9c933f1dc71 258 break;
WouterJS 1:a9c933f1dc71 259 }
WouterJS 1:a9c933f1dc71 260 if (movement == rest && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 261 current_state = waiting;
WouterJS 1:a9c933f1dc71 262 state_changed = true;
WouterJS 1:a9c933f1dc71 263 }
WouterJS 1:a9c933f1dc71 264
WouterJS 1:a9c933f1dc71 265 }
WouterJS 1:a9c933f1dc71 266
WouterJS 1:a9c933f1dc71 267 void do_state_waiting(){
WouterJS 1:a9c933f1dc71 268 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 269 state_changed = false;
WouterJS 1:a9c933f1dc71 270 }
WouterJS 1:a9c933f1dc71 271
WouterJS 1:a9c933f1dc71 272 if (filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 273 current_state = operational;
WouterJS 1:a9c933f1dc71 274 state_changed = true;
WouterJS 1:a9c933f1dc71 275 }
WouterJS 1:a9c933f1dc71 276 }
WouterJS 1:a9c933f1dc71 277 //////////////// END ROBOT ARM STATES //////////////////////////////
WouterJS 1:a9c933f1dc71 278
WouterJS 1:a9c933f1dc71 279 void motor_controller(){
WouterJS 1:a9c933f1dc71 280 float q1;
WouterJS 1:a9c933f1dc71 281 float q2;
WouterJS 1:a9c933f1dc71 282 //q1 defined
WouterJS 1:a9c933f1dc71 283 //q2 defined
WouterJS 1:a9c933f1dc71 284
WouterJS 1:a9c933f1dc71 285 //float L0 = 0.1;
WouterJS 1:a9c933f1dc71 286 //float L1 = 0.1;
WouterJS 1:a9c933f1dc71 287 //float L2 = 0.4;
WouterJS 1:a9c933f1dc71 288
WouterJS 1:a9c933f1dc71 289 //jacobian << L1, L2*sin(q1)+L1,-L0,-L0 - L2*cos(q1);
WouterJS 1:a9c933f1dc71 290
WouterJS 1:a9c933f1dc71 291 //inv_jacobian = jacobian.inverse();
WouterJS 1:a9c933f1dc71 292 //Vector2d reference_vector = inv_jacobian*twist;
WouterJS 1:a9c933f1dc71 293 //float ref_v1 = reference_vector(0);
WouterJS 1:a9c933f1dc71 294 //float ref_v2 = reference_vector(1);
WouterJS 1:a9c933f1dc71 295
WouterJS 1:a9c933f1dc71 296 //float ref_q1 = 0;
WouterJS 1:a9c933f1dc71 297 //ref_q1 = ref_p1 + 0.002*ref_v1;
WouterJS 1:a9c933f1dc71 298 // float ref_q2 = 0;
WouterJS 1:a9c933f1dc71 299 //ref_q2 = ref_p2 + 0.002*ref_v2;
WouterJS 1:a9c933f1dc71 300
WouterJS 1:a9c933f1dc71 301
WouterJS 1:a9c933f1dc71 302
WouterJS 1:a9c933f1dc71 303 }
WouterJS 1:a9c933f1dc71 304
WouterJS 1:a9c933f1dc71 305 void loop_function() { //Main loop function
WouterJS 0:3710031b2621 306 measureall();
WouterJS 0:3710031b2621 307 switch(current_state) {
WouterJS 0:3710031b2621 308 case failure:
WouterJS 0:3710031b2621 309 do_state_failure(); // a separate function for what happens in each state
WouterJS 0:3710031b2621 310 break;
WouterJS 1:a9c933f1dc71 311 case calib_motor:
WouterJS 1:a9c933f1dc71 312 do_state_calib_motor();
WouterJS 1:a9c933f1dc71 313 break ;
WouterJS 0:3710031b2621 314 case calib_emg:
WouterJS 0:3710031b2621 315 do_state_calib_emg();
WouterJS 0:3710031b2621 316 break;
WouterJS 0:3710031b2621 317 case operational:
WouterJS 0:3710031b2621 318 do_state_operational();
WouterJS 0:3710031b2621 319 break;
WouterJS 1:a9c933f1dc71 320 case waiting:
WouterJS 1:a9c933f1dc71 321 do_state_waiting();
WouterJS 0:3710031b2621 322 break;
WouterJS 0:3710031b2621 323 }
WouterJS 0:3710031b2621 324 motor_controller();
WouterJS 1:a9c933f1dc71 325 // Outputs data to the computer
WouterJS 0:3710031b2621 326 }
WouterJS 0:3710031b2621 327
WouterJS 0:3710031b2621 328
WouterJS 0:3710031b2621 329 int main()
WouterJS 1:a9c933f1dc71 330 {
WouterJS 1:a9c933f1dc71 331 motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
WouterJS 2:fa90eaa14f99 332
WouterJS 2:fa90eaa14f99 333 //timer.start();
WouterJS 2:fa90eaa14f99 334 //loop_timer.attach(&loop_function, Ts);
WouterJS 2:fa90eaa14f99 335 //sample_timer.attach(&scopedata, Ts);
WouterJS 2:fa90eaa14f99 336 //sample_timer2.attach(&filterall, Ts);
WouterJS 1:a9c933f1dc71 337
WouterJS 2:fa90eaa14f99 338 while (true) {
WouterJS 2:fa90eaa14f99 339 motor1_direction = 1; //set motor 1 to run counterclockwise (positive) direction for calibration
WouterJS 2:fa90eaa14f99 340 motor1_speed_control = 0.55;//to make sure the motor will not run at too high velocity
WouterJS 2:fa90eaa14f99 341 motor2_direction = 0; // set motor 2 to run clockwise (negative) direction
WouterJS 2:fa90eaa14f99 342 motor2_speed_control = 0.55;
WouterJS 0:3710031b2621 343 }
WouterJS 0:3710031b2621 344 }