Thijs Lubberman / Mbed 2 deprecated MotorArchitecture1-11

juijiu

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture1-11 by Wouter Schuttert

main.cpp@4:34ad002cb646, 2018-10-29 (annotated)

Committer:
WouterJS
Date:
Mon Oct 29 09:55:04 2018 +0000
Revision:
4:34ad002cb646
Parent:
3:055eb9f256fc
Child:
5:892531e4e015
k

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