Thijs Lubberman / Mbed 2 deprecated MotorArchitecture1-11

juijiu

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture1-11 by Wouter Schuttert

main.cpp@6:b526cf83fd4f, 2018-10-30 (annotated)

Committer:
WouterJS
Date:
Tue Oct 30 08:26:42 2018 +0000
Revision:
6:b526cf83fd4f
Parent:
5:892531e4e015
Child:
7:db050a878cff
Next one;

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 5:892531e4e015 28 DigitalOut motor1_direction(D4);// draairichting motor 1 (0 is CCW )
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 5:892531e4e015 31 DigitalOut motor2_direction(D7);// draairichting motor 2 (0 is CCW )
WouterJS 1:a9c933f1dc71 32
WouterJS 5:892531e4e015 33 float kp1 = 0.2;
WouterJS 5:892531e4e015 34 float kp2 = 0.2;
WouterJS 5:892531e4e015 35 float ki1 = 0;
WouterJS 5:892531e4e015 36 float ki2 = 0;
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 5:892531e4e015 64 //HIDScope scope(2);
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 6:b526cf83fd4f 113 counts_m2 = Encoder2.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 5:892531e4e015 117 counts_m2_prev = Encoder2.getPulses();
WouterJS 1:a9c933f1dc71 118
WouterJS 1:a9c933f1dc71 119 }
WouterJS 1:a9c933f1dc71 120
WouterJS 0:3710031b2621 121 void scopedata()
WouterJS 0:3710031b2621 122 {
WouterJS 5:892531e4e015 123 //scope.set(0,deg_m1); //
WouterJS 5:892531e4e015 124 //scope.set(1,deg_m2); //
WouterJS 4:34ad002cb646 125 //scope.set(2,emg1_input); //
WouterJS 4:34ad002cb646 126 //scope.set(3,emg1_input);//
WouterJS 4:34ad002cb646 127 //scope.set(4,filteredsignal1);
WouterJS 4:34ad002cb646 128 //scope.send(); // send info to HIDScope server
WouterJS 0:3710031b2621 129 }
WouterJS 0:3710031b2621 130
WouterJS 1:a9c933f1dc71 131
WouterJS 1:a9c933f1dc71 132 //////////////////// MOVEMENT STATES
WouterJS 1:a9c933f1dc71 133 void do_forward(){
WouterJS 1:a9c933f1dc71 134
WouterJS 4:34ad002cb646 135 //twist1, 0;
WouterJS 1:a9c933f1dc71 136 //Vector2d twistf(0,0);
WouterJS 1:a9c933f1dc71 137 //twistf << 1, 0;
WouterJS 1:a9c933f1dc71 138 if (filteredsignal2 > threshold2){
WouterJS 4:34ad002cb646 139 double abs_sig = (filteredsignal2 - (0.5*max2))/(0.5*max2);
WouterJS 1:a9c933f1dc71 140
WouterJS 1:a9c933f1dc71 141 //twist = twistf * abs_sig;
WouterJS 1:a9c933f1dc71 142
WouterJS 1:a9c933f1dc71 143 }
WouterJS 1:a9c933f1dc71 144
WouterJS 1:a9c933f1dc71 145
WouterJS 1:a9c933f1dc71 146 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 147 {
WouterJS 1:a9c933f1dc71 148 movement = backward;
WouterJS 1:a9c933f1dc71 149 timer.reset();
WouterJS 1:a9c933f1dc71 150 }
WouterJS 1:a9c933f1dc71 151 }
WouterJS 1:a9c933f1dc71 152
WouterJS 1:a9c933f1dc71 153 void do_backward(){
WouterJS 1:a9c933f1dc71 154
WouterJS 1:a9c933f1dc71 155
WouterJS 1:a9c933f1dc71 156
WouterJS 1:a9c933f1dc71 157 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 158 {
WouterJS 1:a9c933f1dc71 159 movement = up;
WouterJS 1:a9c933f1dc71 160 timer.reset();
WouterJS 1:a9c933f1dc71 161 }
WouterJS 1:a9c933f1dc71 162 }
WouterJS 1:a9c933f1dc71 163
WouterJS 1:a9c933f1dc71 164 void do_up(){
WouterJS 1:a9c933f1dc71 165
WouterJS 1:a9c933f1dc71 166 //Code for moving up
WouterJS 1:a9c933f1dc71 167
WouterJS 1:a9c933f1dc71 168 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 169 {
WouterJS 1:a9c933f1dc71 170 movement = down;
WouterJS 1:a9c933f1dc71 171 timer.reset();
WouterJS 1:a9c933f1dc71 172 }
WouterJS 1:a9c933f1dc71 173 }
WouterJS 1:a9c933f1dc71 174 void do_down(){
WouterJS 1:a9c933f1dc71 175
WouterJS 1:a9c933f1dc71 176 //Code for moving down
WouterJS 1:a9c933f1dc71 177
WouterJS 1:a9c933f1dc71 178 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 179 {
WouterJS 1:a9c933f1dc71 180 movement = rest;
WouterJS 1:a9c933f1dc71 181 timer.reset();
WouterJS 1:a9c933f1dc71 182
WouterJS 1:a9c933f1dc71 183 }
WouterJS 1:a9c933f1dc71 184 }
WouterJS 1:a9c933f1dc71 185 void do_wait(){
WouterJS 1:a9c933f1dc71 186
WouterJS 1:a9c933f1dc71 187 if ( filteredsignal2 > threshold2) {//
WouterJS 1:a9c933f1dc71 188 current_state = waiting;
WouterJS 1:a9c933f1dc71 189 state_changed = true;
WouterJS 1:a9c933f1dc71 190 }
WouterJS 1:a9c933f1dc71 191 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 192 {
WouterJS 1:a9c933f1dc71 193 movement = forward;
WouterJS 1:a9c933f1dc71 194 timer.reset();
WouterJS 1:a9c933f1dc71 195 }
WouterJS 1:a9c933f1dc71 196 }
WouterJS 1:a9c933f1dc71 197 ///////////END MOVEMENT STATES/////////////////////////
WouterJS 1:a9c933f1dc71 198 ///////////ROBOT ARM STATES ///////////////////////////
WouterJS 1:a9c933f1dc71 199
WouterJS 1:a9c933f1dc71 200 void do_state_failure(){
WouterJS 1:a9c933f1dc71 201
WouterJS 1:a9c933f1dc71 202 }
WouterJS 4:34ad002cb646 203 int count1 = 0;
WouterJS 4:34ad002cb646 204 int count2 = 0;
WouterJS 1:a9c933f1dc71 205 void do_state_calib_motor(){
WouterJS 1:a9c933f1dc71 206 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 207 state_changed = false;
WouterJS 1:a9c933f1dc71 208 }
WouterJS 1:a9c933f1dc71 209
WouterJS 1:a9c933f1dc71 210
WouterJS 4:34ad002cb646 211
WouterJS 4:34ad002cb646 212 int deriv1 = deg_m1 - count1;
WouterJS 4:34ad002cb646 213 int deriv2 = deg_m2 - count2;
WouterJS 5:892531e4e015 214
WouterJS 4:34ad002cb646 215 count1 = deg_m1;
WouterJS 4:34ad002cb646 216 count2 = deg_m2;
WouterJS 4:34ad002cb646 217
WouterJS 4:34ad002cb646 218 if ( timer.read() > 3 && deriv1 < 0.5 && deriv2 < 0.5) {
WouterJS 4:34ad002cb646 219 motor1_speed_control = 0;
WouterJS 4:34ad002cb646 220 motor2_speed_control = 0;
WouterJS 4:34ad002cb646 221 current_state = homing;
WouterJS 1:a9c933f1dc71 222 timer.reset();
WouterJS 4:34ad002cb646 223 state_changed = true;
WouterJS 5:892531e4e015 224 wait(3);
WouterJS 5:892531e4e015 225 deg_m1 = -2;
WouterJS 5:892531e4e015 226 deg_m2 = -2;
WouterJS 4:34ad002cb646 227 }
WouterJS 1:a9c933f1dc71 228 }
WouterJS 1:a9c933f1dc71 229
WouterJS 5:892531e4e015 230 float wu1;
WouterJS 5:892531e4e015 231 float wu2;
WouterJS 4:34ad002cb646 232 void do_state_homing(){
WouterJS 4:34ad002cb646 233 if (state_changed==true) {
WouterJS 4:34ad002cb646 234 state_changed = false;
WouterJS 4:34ad002cb646 235 }
WouterJS 4:34ad002cb646 236
WouterJS 4:34ad002cb646 237 float werror1 = deg_m1 - 0;
WouterJS 4:34ad002cb646 238 float werror2 = deg_m2 - 0;
WouterJS 1:a9c933f1dc71 239
WouterJS 5:892531e4e015 240 if(werror1 > 5){
WouterJS 5:892531e4e015 241 wu1 = 1; }
WouterJS 5:892531e4e015 242 if(werror1 < -5){
WouterJS 5:892531e4e015 243 wu1 = -1; }
WouterJS 5:892531e4e015 244 else{
WouterJS 5:892531e4e015 245 wu1 = kp1*werror1 + (u1 + werror1*0.002)*ki1;
WouterJS 5:892531e4e015 246 }
WouterJS 1:a9c933f1dc71 247
WouterJS 5:892531e4e015 248 if(werror2 > 5){
WouterJS 5:892531e4e015 249 wu2 = 1;}
WouterJS 5:892531e4e015 250 if(werror2 < -5){
WouterJS 5:892531e4e015 251 wu2 = -1;}
WouterJS 5:892531e4e015 252 else{
WouterJS 5:892531e4e015 253 wu2 = (kp2*werror2 + (u2 + werror2*0.002)*ki2);
WouterJS 5:892531e4e015 254 }
WouterJS 5:892531e4e015 255
WouterJS 5:892531e4e015 256 motor1_speed_control = fabs(wu1)/5 + 0.2;
WouterJS 5:892531e4e015 257
WouterJS 4:34ad002cb646 258 if(wu1 > 0){
WouterJS 4:34ad002cb646 259 motor1_direction = 0;}
WouterJS 4:34ad002cb646 260 if(wu1< 0){
WouterJS 4:34ad002cb646 261 motor1_direction = 1;}
WouterJS 4:34ad002cb646 262
WouterJS 5:892531e4e015 263 motor2_speed_control = fabs(wu2)/5 + 0.2;
WouterJS 4:34ad002cb646 264
WouterJS 4:34ad002cb646 265 if(wu2 > 0){
WouterJS 4:34ad002cb646 266 motor2_direction = 0;}
WouterJS 5:892531e4e015 267 if(wu2 < 0){
WouterJS 4:34ad002cb646 268 motor2_direction = 1;}
WouterJS 4:34ad002cb646 269
WouterJS 4:34ad002cb646 270
WouterJS 4:34ad002cb646 271
WouterJS 1:a9c933f1dc71 272 }
WouterJS 1:a9c933f1dc71 273 void do_state_calib_emg(){
WouterJS 1:a9c933f1dc71 274 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 275 state_changed = false;
WouterJS 1:a9c933f1dc71 276 }
WouterJS 1:a9c933f1dc71 277
WouterJS 1:a9c933f1dc71 278 if(filteredsignal1 > max1){//calibrate to a new maximum
WouterJS 1:a9c933f1dc71 279 max1 = filteredsignal1;
WouterJS 1:a9c933f1dc71 280 threshold1 = 0.5*max1;
WouterJS 1:a9c933f1dc71 281 }
WouterJS 1:a9c933f1dc71 282 if(filteredsignal2 > max2){//calibrate to a new maximum
WouterJS 1:a9c933f1dc71 283 max2 = filteredsignal2;
WouterJS 1:a9c933f1dc71 284 threshold2 = 0.5 * max2;
WouterJS 1:a9c933f1dc71 285 }
WouterJS 1:a9c933f1dc71 286
WouterJS 1:a9c933f1dc71 287 if (timer.read() > 10 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 288 current_state = operational;
WouterJS 1:a9c933f1dc71 289 timer.reset();
WouterJS 1:a9c933f1dc71 290 state_changed = true;
WouterJS 1:a9c933f1dc71 291 }
WouterJS 1:a9c933f1dc71 292 }
WouterJS 1:a9c933f1dc71 293
WouterJS 1:a9c933f1dc71 294 void do_state_operational(){
WouterJS 1:a9c933f1dc71 295 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 296 state_changed = false;
WouterJS 1:a9c933f1dc71 297 }
WouterJS 1:a9c933f1dc71 298
WouterJS 1:a9c933f1dc71 299 switch(movement) {// a separate function for what happens in each state
WouterJS 1:a9c933f1dc71 300 case rest:
WouterJS 1:a9c933f1dc71 301 do_wait();
WouterJS 1:a9c933f1dc71 302 break;
WouterJS 1:a9c933f1dc71 303 case forward:
WouterJS 1:a9c933f1dc71 304 do_forward();
WouterJS 1:a9c933f1dc71 305
WouterJS 1:a9c933f1dc71 306 break;
WouterJS 1:a9c933f1dc71 307 case backward:
WouterJS 1:a9c933f1dc71 308 do_backward();
WouterJS 1:a9c933f1dc71 309
WouterJS 1:a9c933f1dc71 310 break;
WouterJS 1:a9c933f1dc71 311 case up:
WouterJS 1:a9c933f1dc71 312 do_up();
WouterJS 1:a9c933f1dc71 313 break;
WouterJS 1:a9c933f1dc71 314 case down:
WouterJS 1:a9c933f1dc71 315 do_down();
WouterJS 1:a9c933f1dc71 316 break;
WouterJS 1:a9c933f1dc71 317 }
WouterJS 1:a9c933f1dc71 318 if (movement == rest && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 319 current_state = waiting;
WouterJS 1:a9c933f1dc71 320 state_changed = true;
WouterJS 1:a9c933f1dc71 321 }
WouterJS 1:a9c933f1dc71 322
WouterJS 1:a9c933f1dc71 323 }
WouterJS 1:a9c933f1dc71 324
WouterJS 1:a9c933f1dc71 325 void do_state_waiting(){
WouterJS 1:a9c933f1dc71 326 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 327 state_changed = false;
WouterJS 1:a9c933f1dc71 328 }
WouterJS 1:a9c933f1dc71 329
WouterJS 1:a9c933f1dc71 330 if (filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 331 current_state = operational;
WouterJS 1:a9c933f1dc71 332 state_changed = true;
WouterJS 1:a9c933f1dc71 333 }
WouterJS 1:a9c933f1dc71 334 }
WouterJS 1:a9c933f1dc71 335 //////////////// END ROBOT ARM STATES //////////////////////////////
WouterJS 4:34ad002cb646 336
WouterJS 4:34ad002cb646 337
WouterJS 1:a9c933f1dc71 338 void motor_controller(){
WouterJS 4:34ad002cb646 339
WouterJS 5:892531e4e015 340
WouterJS 4:34ad002cb646 341
WouterJS 4:34ad002cb646 342 float jacobian[4];
WouterJS 4:34ad002cb646 343 float inv_jacobian[4];
WouterJS 4:34ad002cb646 344
WouterJS 4:34ad002cb646 345 jacobian[0] = L1;
WouterJS 4:34ad002cb646 346 jacobian[1] = L2*sin(deg_m1)+L1;
WouterJS 4:34ad002cb646 347 jacobian[2] = -L0;
WouterJS 4:34ad002cb646 348 jacobian[3] = -L0 - L2*cos(deg_m1);
WouterJS 1:a9c933f1dc71 349
WouterJS 4:34ad002cb646 350 float det = 1/(jacobian[0]*jacobian[3]-jacobian[1]*jacobian[2]);
WouterJS 1:a9c933f1dc71 351
WouterJS 4:34ad002cb646 352 inv_jacobian[0] = det*jacobian[3];
WouterJS 4:34ad002cb646 353 inv_jacobian[1] = -det*jacobian[1];
WouterJS 4:34ad002cb646 354 inv_jacobian[2] = -det*jacobian[2];
WouterJS 4:34ad002cb646 355 inv_jacobian[3] = det*jacobian[0];
WouterJS 4:34ad002cb646 356
WouterJS 4:34ad002cb646 357
WouterJS 4:34ad002cb646 358 //ref_v1 = jacobian[0]*twist[0]+jacobian[1]*twist[1];
WouterJS 4:34ad002cb646 359 //ref_v2 = jacobian[2]*twist[0]+jacobian[3]*twist[1];
WouterJS 4:34ad002cb646 360
WouterJS 1:a9c933f1dc71 361 //jacobian << L1, L2*sin(q1)+L1,-L0,-L0 - L2*cos(q1);
WouterJS 1:a9c933f1dc71 362
WouterJS 1:a9c933f1dc71 363 //inv_jacobian = jacobian.inverse();
WouterJS 1:a9c933f1dc71 364 //Vector2d reference_vector = inv_jacobian*twist;
WouterJS 1:a9c933f1dc71 365 //float ref_v1 = reference_vector(0);
WouterJS 1:a9c933f1dc71 366 //float ref_v2 = reference_vector(1);
WouterJS 4:34ad002cb646 367
WouterJS 4:34ad002cb646 368
WouterJS 4:34ad002cb646 369 //ref_q1 = ref_q1 + 0.002*ref_v1;
WouterJS 4:34ad002cb646 370 //ref_q2 = ref_q2 + 0.002*ref_v2;
WouterJS 1:a9c933f1dc71 371
WouterJS 4:34ad002cb646 372 //float error1 = deg_m1 - ref_q1;
WouterJS 4:34ad002cb646 373 //float error2 = deg_m2 - ref_q2;
WouterJS 1:a9c933f1dc71 374
WouterJS 4:34ad002cb646 375
WouterJS 4:34ad002cb646 376 //u1 = kp1*error1 + (u1 + error1*0.002)*ki1;
WouterJS 4:34ad002cb646 377 //u2 = kp2*error1 + (u2 + error1*0.002)*ki2;
WouterJS 1:a9c933f1dc71 378
WouterJS 1:a9c933f1dc71 379 }
WouterJS 1:a9c933f1dc71 380
WouterJS 1:a9c933f1dc71 381 void loop_function() { //Main loop function
WouterJS 0:3710031b2621 382 measureall();
WouterJS 0:3710031b2621 383 switch(current_state) {
WouterJS 0:3710031b2621 384 case failure:
WouterJS 0:3710031b2621 385 do_state_failure(); // a separate function for what happens in each state
WouterJS 0:3710031b2621 386 break;
WouterJS 1:a9c933f1dc71 387 case calib_motor:
WouterJS 1:a9c933f1dc71 388 do_state_calib_motor();
WouterJS 1:a9c933f1dc71 389 break ;
WouterJS 4:34ad002cb646 390 case homing:
WouterJS 4:34ad002cb646 391 do_state_homing();
WouterJS 4:34ad002cb646 392 break;
WouterJS 0:3710031b2621 393 case calib_emg:
WouterJS 0:3710031b2621 394 do_state_calib_emg();
WouterJS 0:3710031b2621 395 break;
WouterJS 0:3710031b2621 396 case operational:
WouterJS 0:3710031b2621 397 do_state_operational();
WouterJS 0:3710031b2621 398 break;
WouterJS 1:a9c933f1dc71 399 case waiting:
WouterJS 1:a9c933f1dc71 400 do_state_waiting();
WouterJS 0:3710031b2621 401 break;
WouterJS 0:3710031b2621 402 }
WouterJS 0:3710031b2621 403 motor_controller();
WouterJS 1:a9c933f1dc71 404 // Outputs data to the computer
WouterJS 0:3710031b2621 405 }
WouterJS 0:3710031b2621 406
WouterJS 0:3710031b2621 407
WouterJS 0:3710031b2621 408 int main()
WouterJS 1:a9c933f1dc71 409 {
WouterJS 4:34ad002cb646 410 // motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
WouterJS 3:055eb9f256fc 411
WouterJS 5:892531e4e015 412 motor1_direction = 0; //set motor 1 to run clockwisedirection for calibration
WouterJS 4:34ad002cb646 413 motor1_speed_control = 0.25;//to make sure the motor will not run at too high velocity
WouterJS 5:892531e4e015 414 motor2_direction = 0; // set motor 2 to run clockwise direction
WouterJS 4:34ad002cb646 415 motor2_speed_control = 0.25;
WouterJS 4:34ad002cb646 416
WouterJS 4:34ad002cb646 417 led_red = 0;
WouterJS 3:055eb9f256fc 418 timer.start();
WouterJS 3:055eb9f256fc 419 loop_timer.attach(&loop_function, Ts);
WouterJS 3:055eb9f256fc 420 sample_timer.attach(&scopedata, Ts);
WouterJS 3:055eb9f256fc 421 sample_timer2.attach(&filterall, Ts);
WouterJS 1:a9c933f1dc71 422
WouterJS 2:fa90eaa14f99 423 while (true) {
WouterJS 3:055eb9f256fc 424
WouterJS 0:3710031b2621 425 }
WouterJS 0:3710031b2621 426 }