Wouter Schuttert / Mbed 2 deprecated MotorArchitecture1-11

Another one

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture2 by Thijs Lubberman

main.cpp@1:a9c933f1dc71, 2018-10-26 (annotated)

Committer:
WouterJS
Date:
Fri Oct 26 10:50:57 2018 +0000
Revision:
1:a9c933f1dc71
Parent:
0:3710031b2621
Child:
2:fa90eaa14f99
update!;

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