Thijs Lubberman / Mbed 2 deprecated MotorArchitecture1-11

juijiu

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture1-11 by Wouter Schuttert

main.cpp@3:055eb9f256fc, 2018-10-26 (annotated)

Committer:
WouterJS
Date:
Fri Oct 26 11:46:25 2018 +0000
Revision:
3:055eb9f256fc
Parent:
2:fa90eaa14f99
Child:
4:34ad002cb646
for alibus;

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