Thijs Lubberman / Mbed 2 deprecated MotorArchitecture1-11

juijiu

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture1-11 by Wouter Schuttert

main.cpp@9:4f594927cff3, 2018-11-01 (annotated)

Committer:
WouterJS
Date:
Thu Nov 01 08:30:56 2018 +0000
Revision:
9:4f594927cff3
Parent:
8:c7d21f5f87d8
Child:
10:91173ed1e841
Another 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
thijslubberman 7:db050a878cff 16 DigitalIn but1(D2);
thijslubberman 7:db050a878cff 17
WouterJS 1:a9c933f1dc71 18 QEI Encoder1(D10,D11,NC,sensor_sensitivity); //First one is B, Second one is A
WouterJS 1:a9c933f1dc71 19 QEI Encoder2(D12,D13,NC,sensor_sensitivity); //
thijslubberman 7:db050a878cff 20
WouterJS 4:34ad002cb646 21 DigitalOut led_red(LED_RED);
thijslubberman 7:db050a878cff 22 DigitalOut led_green(LED_GREEN);
thijslubberman 7:db050a878cff 23 DigitalOut led_blue(LED_BLUE);
thijslubberman 7:db050a878cff 24
WouterJS 1:a9c933f1dc71 25 int counts_m1 = 0;
WouterJS 1:a9c933f1dc71 26 int counts_m2 = 0;
WouterJS 1:a9c933f1dc71 27 int counts_m1_prev = 0;
WouterJS 1:a9c933f1dc71 28 int counts_m2_prev = 0;
WouterJS 1:a9c933f1dc71 29 float deg_m1 = 0;
WouterJS 1:a9c933f1dc71 30 float deg_m2 = 0;
WouterJS 1:a9c933f1dc71 31
WouterJS 4:34ad002cb646 32
WouterJS 0:3710031b2621 33
WouterJS 5:892531e4e015 34 DigitalOut motor1_direction(D4);// draairichting motor 1 (0 is CCW )
WouterJS 0:3710031b2621 35 PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1
WouterJS 1:a9c933f1dc71 36 PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
WouterJS 5:892531e4e015 37 DigitalOut motor2_direction(D7);// draairichting motor 2 (0 is CCW )
WouterJS 1:a9c933f1dc71 38
WouterJS 9:4f594927cff3 39
thijslubberman 7:db050a878cff 40 float int_u1 = 0;
thijslubberman 7:db050a878cff 41 float int_u2 = 0;
WouterJS 4:34ad002cb646 42 float u1 = 0;
WouterJS 4:34ad002cb646 43 float u2 = 0;
WouterJS 4:34ad002cb646 44
WouterJS 4:34ad002cb646 45 float ref_q1 = 0;
WouterJS 4:34ad002cb646 46 float ref_q2 = 0;
WouterJS 4:34ad002cb646 47 float L0 = 0.1;
WouterJS 4:34ad002cb646 48 float L1 = 0.1;
WouterJS 4:34ad002cb646 49 float L2 = 0.4;
WouterJS 4:34ad002cb646 50
WouterJS 4:34ad002cb646 51 float ref_v1;
WouterJS 4:34ad002cb646 52 float ref_v2;
thijslubberman 8:c7d21f5f87d8 53 float teraterm1;
thijslubberman 8:c7d21f5f87d8 54 float teraterm2;
thijslubberman 8:c7d21f5f87d8 55 float error1;
thijslubberman 8:c7d21f5f87d8 56 float error2;
thijslubberman 8:c7d21f5f87d8 57 float filtered_d_error1;
thijslubberman 8:c7d21f5f87d8 58 float filtered_d_error2;
WouterJS 4:34ad002cb646 59
WouterJS 4:34ad002cb646 60 enum States {failure, waiting, calib_motor, homing ,calib_emg, operational, demo};
WouterJS 0:3710031b2621 61 enum Operations {rest, forward, backward, up, down};
WouterJS 0:3710031b2621 62
WouterJS 1:a9c933f1dc71 63 States current_state = calib_motor;
thijslubberman 7:db050a878cff 64 Operations movement = forward;
WouterJS 0:3710031b2621 65
WouterJS 1:a9c933f1dc71 66 float max1 = 0; //initial threshold value for emg signals, changes during calibration left arm
WouterJS 1:a9c933f1dc71 67 float max2 = 0; // right arm
WouterJS 1:a9c933f1dc71 68 float threshold1;
WouterJS 1:a9c933f1dc71 69 float threshold2;
WouterJS 1:a9c933f1dc71 70 float thresholdtime = 1.0; // time waiting before switching modes
WouterJS 1:a9c933f1dc71 71
WouterJS 1:a9c933f1dc71 72 Ticker loop_timer;
WouterJS 0:3710031b2621 73 Ticker sample_timer;
WouterJS 1:a9c933f1dc71 74 Ticker sample_timer2;
thijslubberman 7:db050a878cff 75 //HIDScope scope(3);
WouterJS 0:3710031b2621 76
WouterJS 0:3710031b2621 77 AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes
WouterJS 0:3710031b2621 78 AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength
WouterJS 0:3710031b2621 79
WouterJS 1:a9c933f1dc71 80 volatile float emg1_input;
WouterJS 1:a9c933f1dc71 81 volatile float emg2_input;
WouterJS 1:a9c933f1dc71 82
WouterJS 0:3710031b2621 83 volatile float raw_filteredsignal1;//the first filtered emg signal 1
WouterJS 0:3710031b2621 84 volatile float raw_filteredsignal2;//the first filtered emg signal 2
WouterJS 0:3710031b2621 85
WouterJS 0:3710031b2621 86 volatile float filteredsignal1;//the first filtered emg signal 1
WouterJS 0:3710031b2621 87 volatile float filteredsignal2;//the first filtered emg signal 2
WouterJS 0:3710031b2621 88
WouterJS 0:3710031b2621 89 bool state_changed = false;
thijslubberman 7:db050a878cff 90 static BiQuad Notch1(0.9714498065192796,-1.5718388053127037,0.9714498065192796,-1.5718388053127037,0.9428996130385592);
thijslubberman 7:db050a878cff 91 static BiQuad Notch2(0.9714498065192796,-1.5718388053127037,0.9714498065192796,-1.5718388053127037,0.9428996130385592);
thijslubberman 7:db050a878cff 92 static BiQuad HighPass1(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354);
thijslubberman 7:db050a878cff 93 static BiQuad HighPass2(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354);
thijslubberman 7:db050a878cff 94 static BiQuad LowPass1(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906);
thijslubberman 7:db050a878cff 95 static BiQuad LowPass2(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906);
WouterJS 9:4f594927cff3 96 static BiQuad LowpassFilter1(0.0640,0.1279,0.0640,-1.1683,0.4241);
WouterJS 9:4f594927cff3 97 static BiQuad LowpassFilter2(0.0640,0.1279,0.0640,-1.1683,0.4241);
WouterJS 0:3710031b2621 98 void filterall()
WouterJS 0:3710031b2621 99 {
thijslubberman 7:db050a878cff 100 //Notch 50 Hz BW 6 Hz
thijslubberman 7:db050a878cff 101 float notch1 = Notch1.step(emg1_input);
thijslubberman 7:db050a878cff 102 float notch2 = Notch2.step(emg2_input);
WouterJS 0:3710031b2621 103 //Highpass Biquad 5 Hz
thijslubberman 7:db050a878cff 104 float high1 = HighPass1.step(notch1);
thijslubberman 7:db050a878cff 105 float high2 = HighPass2.step(notch2);
WouterJS 0:3710031b2621 106 // Rectify the signal(absolute value)
thijslubberman 7:db050a878cff 107 float abs1 = fabs(high1);
thijslubberman 7:db050a878cff 108 float abs2 = fabs(high2);
WouterJS 0:3710031b2621 109 //Lowpass Biquad 10 Hz
thijslubberman 7:db050a878cff 110 float low1 = LowPass1.step(abs1);
thijslubberman 7:db050a878cff 111 float low2 = LowPass2.step(abs2);
WouterJS 0:3710031b2621 112
WouterJS 0:3710031b2621 113 raw_filteredsignal1 = low1;
WouterJS 0:3710031b2621 114 raw_filteredsignal2 = low2;
WouterJS 0:3710031b2621 115
WouterJS 1:a9c933f1dc71 116
WouterJS 0:3710031b2621 117 }
WouterJS 0:3710031b2621 118
WouterJS 1:a9c933f1dc71 119 void measureall(){ // changes all variables according in sync with the rest of the code
WouterJS 1:a9c933f1dc71 120
WouterJS 1:a9c933f1dc71 121 emg1_input = raw_emg1_input.read();
WouterJS 1:a9c933f1dc71 122 emg2_input = raw_emg2_input.read();
WouterJS 1:a9c933f1dc71 123
WouterJS 1:a9c933f1dc71 124 filteredsignal1 = raw_filteredsignal1;
WouterJS 1:a9c933f1dc71 125 filteredsignal2 = raw_filteredsignal2;
WouterJS 1:a9c933f1dc71 126
WouterJS 1:a9c933f1dc71 127 //Reading of motor
WouterJS 1:a9c933f1dc71 128
WouterJS 1:a9c933f1dc71 129 counts_m1 = Encoder1.getPulses() - counts_m1_prev;
WouterJS 6:b526cf83fd4f 130 counts_m2 = Encoder2.getPulses() - counts_m2_prev;
WouterJS 1:a9c933f1dc71 131 deg_m1 = deg_m1 + counts_m1*(360/(full_ratio));
WouterJS 1:a9c933f1dc71 132 deg_m2 = deg_m2 + counts_m2*(360/(full_ratio));
WouterJS 1:a9c933f1dc71 133 counts_m1_prev = Encoder1.getPulses();
WouterJS 5:892531e4e015 134 counts_m2_prev = Encoder2.getPulses();
WouterJS 1:a9c933f1dc71 135
WouterJS 1:a9c933f1dc71 136 }
WouterJS 1:a9c933f1dc71 137
WouterJS 0:3710031b2621 138 void scopedata()
WouterJS 0:3710031b2621 139 {
thijslubberman 7:db050a878cff 140 //scope.set(0,filteredsignal1); //
thijslubberman 7:db050a878cff 141 //scope.set(1,filteredsignal2); //
thijslubberman 7:db050a878cff 142 //scope.set(2,ref_q1); //
thijslubberman 7:db050a878cff 143 // scope.set(3,movement);//
thijslubberman 7:db050a878cff 144 // scope.set(4,ref_q1);
WouterJS 4:34ad002cb646 145 //scope.send(); // send info to HIDScope server
WouterJS 0:3710031b2621 146 }
WouterJS 0:3710031b2621 147
WouterJS 1:a9c933f1dc71 148
WouterJS 1:a9c933f1dc71 149 //////////////////// MOVEMENT STATES
thijslubberman 7:db050a878cff 150 float twist[2] = {0,0};
thijslubberman 7:db050a878cff 151 float twistf[2] = {0,0};
thijslubberman 7:db050a878cff 152 float abs_sig;
thijslubberman 7:db050a878cff 153
thijslubberman 8:c7d21f5f87d8 154 int gain = 3;
thijslubberman 7:db050a878cff 155
WouterJS 1:a9c933f1dc71 156 void do_forward(){
WouterJS 1:a9c933f1dc71 157
thijslubberman 7:db050a878cff 158
thijslubberman 8:c7d21f5f87d8 159 twistf[0] = 1;
thijslubberman 8:c7d21f5f87d8 160 twistf[1] = 0;
thijslubberman 7:db050a878cff 161
WouterJS 9:4f594927cff3 162 if (but1 == false){
WouterJS 9:4f594927cff3 163 //abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2);
WouterJS 9:4f594927cff3 164 abs_sig = 0.4;
WouterJS 1:a9c933f1dc71 165
WouterJS 1:a9c933f1dc71 166 }
thijslubberman 7:db050a878cff 167 else
thijslubberman 7:db050a878cff 168 {
thijslubberman 7:db050a878cff 169 abs_sig = 0;
thijslubberman 7:db050a878cff 170 }
WouterJS 9:4f594927cff3 171
WouterJS 1:a9c933f1dc71 172
WouterJS 9:4f594927cff3 173 twist[0] = twistf[0] * abs_sig ;
WouterJS 9:4f594927cff3 174 twist[1] = twistf[1] * abs_sig ;
thijslubberman 7:db050a878cff 175
thijslubberman 7:db050a878cff 176 motor1_speed_control = fabs(u1);
WouterJS 1:a9c933f1dc71 177
thijslubberman 7:db050a878cff 178 if(u1 > 0){
thijslubberman 7:db050a878cff 179 motor1_direction = 1;}
thijslubberman 7:db050a878cff 180 if(u1 < 0){
thijslubberman 7:db050a878cff 181 motor1_direction = 0;}
thijslubberman 7:db050a878cff 182
thijslubberman 7:db050a878cff 183 motor2_speed_control = fabs(u2);
thijslubberman 7:db050a878cff 184
thijslubberman 7:db050a878cff 185 if(u2 > 0){
thijslubberman 7:db050a878cff 186 motor2_direction = 1;}
thijslubberman 7:db050a878cff 187 if(u2 < 0){
thijslubberman 7:db050a878cff 188 motor2_direction = 0;}
thijslubberman 7:db050a878cff 189
thijslubberman 7:db050a878cff 190 //if( timer.read() > thresholdtime && filteredsignal1 > threshold1*100)
thijslubberman 7:db050a878cff 191 // {
thijslubberman 7:db050a878cff 192 // movement = backward;
thijslubberman 7:db050a878cff 193 // timer.reset();
thijslubberman 7:db050a878cff 194 // }
WouterJS 1:a9c933f1dc71 195 }
WouterJS 1:a9c933f1dc71 196
WouterJS 1:a9c933f1dc71 197 void do_backward(){
WouterJS 1:a9c933f1dc71 198
WouterJS 1:a9c933f1dc71 199
WouterJS 1:a9c933f1dc71 200
WouterJS 1:a9c933f1dc71 201 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 202 {
WouterJS 1:a9c933f1dc71 203 movement = up;
WouterJS 1:a9c933f1dc71 204 timer.reset();
WouterJS 1:a9c933f1dc71 205 }
WouterJS 1:a9c933f1dc71 206 }
WouterJS 1:a9c933f1dc71 207
WouterJS 1:a9c933f1dc71 208 void do_up(){
WouterJS 1:a9c933f1dc71 209
WouterJS 1:a9c933f1dc71 210 //Code for moving up
WouterJS 1:a9c933f1dc71 211
WouterJS 1:a9c933f1dc71 212 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 213 {
WouterJS 1:a9c933f1dc71 214 movement = down;
WouterJS 1:a9c933f1dc71 215 timer.reset();
WouterJS 1:a9c933f1dc71 216 }
WouterJS 1:a9c933f1dc71 217 }
WouterJS 1:a9c933f1dc71 218 void do_down(){
WouterJS 1:a9c933f1dc71 219
WouterJS 1:a9c933f1dc71 220 //Code for moving down
WouterJS 1:a9c933f1dc71 221
WouterJS 1:a9c933f1dc71 222 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 223 {
WouterJS 1:a9c933f1dc71 224 movement = rest;
WouterJS 1:a9c933f1dc71 225 timer.reset();
WouterJS 1:a9c933f1dc71 226
WouterJS 1:a9c933f1dc71 227 }
WouterJS 1:a9c933f1dc71 228 }
WouterJS 1:a9c933f1dc71 229 void do_wait(){
WouterJS 1:a9c933f1dc71 230
WouterJS 1:a9c933f1dc71 231 if ( filteredsignal2 > threshold2) {//
WouterJS 1:a9c933f1dc71 232 current_state = waiting;
WouterJS 1:a9c933f1dc71 233 state_changed = true;
WouterJS 1:a9c933f1dc71 234 }
WouterJS 1:a9c933f1dc71 235 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 236 {
WouterJS 1:a9c933f1dc71 237 movement = forward;
WouterJS 1:a9c933f1dc71 238 timer.reset();
WouterJS 1:a9c933f1dc71 239 }
WouterJS 1:a9c933f1dc71 240 }
WouterJS 1:a9c933f1dc71 241 ///////////END MOVEMENT STATES/////////////////////////
WouterJS 1:a9c933f1dc71 242 ///////////ROBOT ARM STATES ///////////////////////////
WouterJS 9:4f594927cff3 243 float kp1 = 0.1;
WouterJS 9:4f594927cff3 244 float kp2 = 0.1;
WouterJS 9:4f594927cff3 245 float ki1 = 0.05;
WouterJS 9:4f594927cff3 246 float ki2 = 0.05;
WouterJS 9:4f594927cff3 247 float kd1 = 0.005;
WouterJS 9:4f594927cff3 248 float kd2 = 0.005;
WouterJS 9:4f594927cff3 249 float olderror1;
WouterJS 9:4f594927cff3 250 float olderror2;
WouterJS 9:4f594927cff3 251 float d_error1;
WouterJS 9:4f594927cff3 252 float d_error2;
WouterJS 1:a9c933f1dc71 253 void do_state_failure(){
WouterJS 1:a9c933f1dc71 254
WouterJS 1:a9c933f1dc71 255 }
WouterJS 9:4f594927cff3 256
WouterJS 4:34ad002cb646 257 int count1 = 0;
WouterJS 4:34ad002cb646 258 int count2 = 0;
WouterJS 9:4f594927cff3 259
WouterJS 1:a9c933f1dc71 260 void do_state_calib_motor(){
WouterJS 1:a9c933f1dc71 261 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 262 state_changed = false;
WouterJS 1:a9c933f1dc71 263 }
thijslubberman 7:db050a878cff 264 led_red = 0;
WouterJS 1:a9c933f1dc71 265
WouterJS 1:a9c933f1dc71 266
WouterJS 4:34ad002cb646 267
WouterJS 4:34ad002cb646 268 int deriv1 = deg_m1 - count1;
WouterJS 4:34ad002cb646 269 int deriv2 = deg_m2 - count2;
WouterJS 5:892531e4e015 270
WouterJS 4:34ad002cb646 271 count1 = deg_m1;
WouterJS 4:34ad002cb646 272 count2 = deg_m2;
WouterJS 4:34ad002cb646 273
WouterJS 4:34ad002cb646 274 if ( timer.read() > 3 && deriv1 < 0.5 && deriv2 < 0.5) {
WouterJS 4:34ad002cb646 275 motor1_speed_control = 0;
WouterJS 4:34ad002cb646 276 motor2_speed_control = 0;
WouterJS 4:34ad002cb646 277 current_state = homing;
thijslubberman 7:db050a878cff 278
WouterJS 4:34ad002cb646 279 state_changed = true;
thijslubberman 7:db050a878cff 280 wait(1);
thijslubberman 7:db050a878cff 281 deg_m1 = 57.4;
thijslubberman 7:db050a878cff 282 deg_m2 = 72.93;
thijslubberman 7:db050a878cff 283 led_red = 1;
thijslubberman 7:db050a878cff 284 led_green = 0;
WouterJS 4:34ad002cb646 285 }
WouterJS 1:a9c933f1dc71 286 }
WouterJS 1:a9c933f1dc71 287
WouterJS 5:892531e4e015 288 float wu1;
WouterJS 5:892531e4e015 289 float wu2;
thijslubberman 7:db050a878cff 290
WouterJS 4:34ad002cb646 291 void do_state_homing(){
WouterJS 4:34ad002cb646 292 if (state_changed==true) {
thijslubberman 7:db050a878cff 293 timer.reset();
WouterJS 4:34ad002cb646 294 state_changed = false;
WouterJS 4:34ad002cb646 295 }
WouterJS 9:4f594927cff3 296 float kp11 = 0.1;
WouterJS 9:4f594927cff3 297 float kp12 = 0.1;
WouterJS 9:4f594927cff3 298
WouterJS 4:34ad002cb646 299 float werror1 = deg_m1 - 0;
WouterJS 4:34ad002cb646 300 float werror2 = deg_m2 - 0;
WouterJS 1:a9c933f1dc71 301
WouterJS 5:892531e4e015 302 if(werror1 > 5){
WouterJS 5:892531e4e015 303 wu1 = 1; }
WouterJS 5:892531e4e015 304 if(werror1 < -5){
WouterJS 5:892531e4e015 305 wu1 = -1; }
WouterJS 5:892531e4e015 306 else{
WouterJS 9:4f594927cff3 307 wu1 = kp11*werror1; //+ (u1 + werror1*0.002)*ki1;
WouterJS 5:892531e4e015 308 }
WouterJS 1:a9c933f1dc71 309
WouterJS 5:892531e4e015 310 if(werror2 > 5){
WouterJS 5:892531e4e015 311 wu2 = 1;}
WouterJS 5:892531e4e015 312 if(werror2 < -5){
WouterJS 5:892531e4e015 313 wu2 = -1;}
WouterJS 5:892531e4e015 314 else{
WouterJS 9:4f594927cff3 315 wu2 = kp12*werror2; //+ (u2 + werror2*0.002)*ki2);
WouterJS 5:892531e4e015 316 }
WouterJS 5:892531e4e015 317
thijslubberman 7:db050a878cff 318 motor1_speed_control = fabs(wu1)/8;
WouterJS 5:892531e4e015 319
WouterJS 4:34ad002cb646 320 if(wu1 > 0){
thijslubberman 7:db050a878cff 321 motor1_direction = 1;}
WouterJS 4:34ad002cb646 322 if(wu1< 0){
thijslubberman 7:db050a878cff 323 motor1_direction = 0;}
WouterJS 4:34ad002cb646 324
thijslubberman 7:db050a878cff 325 motor2_speed_control = fabs(wu2)/8;
WouterJS 4:34ad002cb646 326
WouterJS 4:34ad002cb646 327 if(wu2 > 0){
thijslubberman 7:db050a878cff 328 motor2_direction = 1;}
WouterJS 5:892531e4e015 329 if(wu2 < 0){
thijslubberman 7:db050a878cff 330 motor2_direction = 0;}
thijslubberman 7:db050a878cff 331
thijslubberman 7:db050a878cff 332 if ( timer.read() > 4) {
thijslubberman 7:db050a878cff 333 motor1_speed_control = 0;
WouterJS 9:4f594927cff3 334 motor2_speed_control = 0;
WouterJS 4:34ad002cb646 335
WouterJS 9:4f594927cff3 336 ref_q1 = 0;
WouterJS 9:4f594927cff3 337 ref_q2 = 0;
thijslubberman 7:db050a878cff 338 deg_m1 = 0;
thijslubberman 7:db050a878cff 339 deg_m2 = 0;
WouterJS 9:4f594927cff3 340 d_error1 = 0;
WouterJS 9:4f594927cff3 341 d_error2 = 0;
thijslubberman 7:db050a878cff 342 u1 = 0;
thijslubberman 7:db050a878cff 343 u2 = 0;
thijslubberman 7:db050a878cff 344 int_u1 = 0;
thijslubberman 7:db050a878cff 345 int_u2 = 0;
WouterJS 9:4f594927cff3 346 wait(1);
thijslubberman 7:db050a878cff 347 current_state = calib_emg;
thijslubberman 7:db050a878cff 348 timer.reset();
thijslubberman 7:db050a878cff 349 state_changed = true;
thijslubberman 7:db050a878cff 350 led_green = 1;
thijslubberman 7:db050a878cff 351 led_blue = 0;
thijslubberman 7:db050a878cff 352
thijslubberman 7:db050a878cff 353 }
WouterJS 4:34ad002cb646 354
WouterJS 4:34ad002cb646 355
WouterJS 1:a9c933f1dc71 356 }
WouterJS 1:a9c933f1dc71 357 void do_state_calib_emg(){
WouterJS 1:a9c933f1dc71 358 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 359 state_changed = false;
WouterJS 1:a9c933f1dc71 360 }
WouterJS 1:a9c933f1dc71 361
WouterJS 1:a9c933f1dc71 362 if(filteredsignal1 > max1){//calibrate to a new maximum
WouterJS 1:a9c933f1dc71 363 max1 = filteredsignal1;
thijslubberman 7:db050a878cff 364 threshold1 = 0.5 * max1;
WouterJS 1:a9c933f1dc71 365 }
WouterJS 1:a9c933f1dc71 366 if(filteredsignal2 > max2){//calibrate to a new maximum
WouterJS 1:a9c933f1dc71 367 max2 = filteredsignal2;
WouterJS 1:a9c933f1dc71 368 threshold2 = 0.5 * max2;
WouterJS 1:a9c933f1dc71 369 }
WouterJS 1:a9c933f1dc71 370
thijslubberman 7:db050a878cff 371 if (timer.read() > 5 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 372 current_state = operational;
WouterJS 1:a9c933f1dc71 373 timer.reset();
WouterJS 1:a9c933f1dc71 374 state_changed = true;
thijslubberman 7:db050a878cff 375 led_green = 0;
thijslubberman 7:db050a878cff 376 led_red =0;
WouterJS 1:a9c933f1dc71 377 }
WouterJS 1:a9c933f1dc71 378 }
WouterJS 1:a9c933f1dc71 379
WouterJS 1:a9c933f1dc71 380 void do_state_operational(){
WouterJS 1:a9c933f1dc71 381 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 382 state_changed = false;
WouterJS 1:a9c933f1dc71 383 }
WouterJS 1:a9c933f1dc71 384
WouterJS 1:a9c933f1dc71 385 switch(movement) {// a separate function for what happens in each state
WouterJS 1:a9c933f1dc71 386 case rest:
WouterJS 1:a9c933f1dc71 387 do_wait();
WouterJS 1:a9c933f1dc71 388 break;
WouterJS 1:a9c933f1dc71 389 case forward:
WouterJS 1:a9c933f1dc71 390 do_forward();
WouterJS 1:a9c933f1dc71 391
WouterJS 1:a9c933f1dc71 392 break;
WouterJS 1:a9c933f1dc71 393 case backward:
WouterJS 1:a9c933f1dc71 394 do_backward();
WouterJS 1:a9c933f1dc71 395
WouterJS 1:a9c933f1dc71 396 break;
WouterJS 1:a9c933f1dc71 397 case up:
WouterJS 1:a9c933f1dc71 398 do_up();
WouterJS 1:a9c933f1dc71 399 break;
WouterJS 1:a9c933f1dc71 400 case down:
WouterJS 1:a9c933f1dc71 401 do_down();
WouterJS 1:a9c933f1dc71 402 break;
WouterJS 1:a9c933f1dc71 403 }
WouterJS 1:a9c933f1dc71 404 if (movement == rest && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 405 current_state = waiting;
WouterJS 1:a9c933f1dc71 406 state_changed = true;
WouterJS 1:a9c933f1dc71 407 }
WouterJS 1:a9c933f1dc71 408
WouterJS 1:a9c933f1dc71 409 }
WouterJS 1:a9c933f1dc71 410
WouterJS 1:a9c933f1dc71 411 void do_state_waiting(){
WouterJS 1:a9c933f1dc71 412 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 413 state_changed = false;
WouterJS 1:a9c933f1dc71 414 }
WouterJS 1:a9c933f1dc71 415
WouterJS 1:a9c933f1dc71 416 if (filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 417 current_state = operational;
WouterJS 1:a9c933f1dc71 418 state_changed = true;
WouterJS 1:a9c933f1dc71 419 }
WouterJS 1:a9c933f1dc71 420 }
WouterJS 1:a9c933f1dc71 421 //////////////// END ROBOT ARM STATES //////////////////////////////
WouterJS 4:34ad002cb646 422
WouterJS 4:34ad002cb646 423
thijslubberman 7:db050a878cff 424 void motor_controller(){
thijslubberman 7:db050a878cff 425
WouterJS 4:34ad002cb646 426 float jacobian[4];
WouterJS 4:34ad002cb646 427 float inv_jacobian[4];
WouterJS 4:34ad002cb646 428
WouterJS 4:34ad002cb646 429 jacobian[0] = L1;
WouterJS 9:4f594927cff3 430 jacobian[1] = (L2*cos(deg_m2))+L1;
WouterJS 4:34ad002cb646 431 jacobian[2] = -L0;
WouterJS 9:4f594927cff3 432 jacobian[3] = -L0 - (L2*sin(deg_m2));
WouterJS 1:a9c933f1dc71 433
WouterJS 9:4f594927cff3 434 float det = 1/((jacobian[0]*jacobian[3])-(jacobian[1]*jacobian[2]));
WouterJS 1:a9c933f1dc71 435
WouterJS 4:34ad002cb646 436 inv_jacobian[0] = det*jacobian[3];
WouterJS 4:34ad002cb646 437 inv_jacobian[1] = -det*jacobian[1];
WouterJS 4:34ad002cb646 438 inv_jacobian[2] = -det*jacobian[2];
WouterJS 4:34ad002cb646 439 inv_jacobian[3] = det*jacobian[0];
WouterJS 4:34ad002cb646 440
WouterJS 4:34ad002cb646 441
thijslubberman 7:db050a878cff 442 ref_v1 = inv_jacobian[0]*twist[0]+inv_jacobian[1]*twist[1];
thijslubberman 7:db050a878cff 443 ref_v2 = inv_jacobian[2]*twist[0]+inv_jacobian[3]*twist[1];
WouterJS 4:34ad002cb646 444
thijslubberman 7:db050a878cff 445
WouterJS 4:34ad002cb646 446
thijslubberman 7:db050a878cff 447 ref_q1 = ref_q1 + 0.002*ref_v1;
thijslubberman 7:db050a878cff 448 ref_q2 = ref_q2 + 0.002*ref_v2;
thijslubberman 7:db050a878cff 449
thijslubberman 8:c7d21f5f87d8 450 error1 = deg_m1 - ref_q1;
thijslubberman 8:c7d21f5f87d8 451 error2 = deg_m2 - ref_q2;
WouterJS 9:4f594927cff3 452
WouterJS 9:4f594927cff3 453
thijslubberman 8:c7d21f5f87d8 454
thijslubberman 8:c7d21f5f87d8 455
thijslubberman 8:c7d21f5f87d8 456
WouterJS 1:a9c933f1dc71 457
WouterJS 9:4f594927cff3 458 // if(error1 > 5){
WouterJS 9:4f594927cff3 459 // u1 = 1; }
WouterJS 9:4f594927cff3 460 //if(error1 < -5){
WouterJS 9:4f594927cff3 461 // u1 = -1; }
WouterJS 9:4f594927cff3 462 //else{
thijslubberman 8:c7d21f5f87d8 463 d_error1 = (error1 - olderror1)/Ts;
WouterJS 9:4f594927cff3 464 filtered_d_error1 = LowpassFilter1.step(d_error1);
thijslubberman 7:db050a878cff 465 int_u1 = int_u1 + error1 * Ts;
thijslubberman 8:c7d21f5f87d8 466 u1 = kp1*error1 + int_u1*ki1 + kd1*filtered_d_error1;
thijslubberman 8:c7d21f5f87d8 467 teraterm1 = u1; // to see how big u1 and u2 actually get, they should lie between -1 and 1 for the right gains.
thijslubberman 8:c7d21f5f87d8 468 if(u1>1){
thijslubberman 8:c7d21f5f87d8 469 u1 =1;
WouterJS 9:4f594927cff3 470 }
thijslubberman 8:c7d21f5f87d8 471 if(u1<-1){
thijslubberman 8:c7d21f5f87d8 472 u1 = -1;
thijslubberman 8:c7d21f5f87d8 473 }
thijslubberman 8:c7d21f5f87d8 474
WouterJS 9:4f594927cff3 475 //}
thijslubberman 8:c7d21f5f87d8 476 olderror1 = error1;
WouterJS 1:a9c933f1dc71 477
WouterJS 9:4f594927cff3 478 //if(error2 > 5){
WouterJS 9:4f594927cff3 479 // u2 = 1;}
WouterJS 9:4f594927cff3 480 // if(error2 < -5){
WouterJS 9:4f594927cff3 481 // u2 = -1;}
WouterJS 9:4f594927cff3 482 //else{
thijslubberman 8:c7d21f5f87d8 483 d_error2 = (error2 - olderror2)/Ts;
WouterJS 9:4f594927cff3 484 filtered_d_error2 = LowpassFilter2.step(d_error2);
thijslubberman 7:db050a878cff 485 int_u2 = int_u2 + error2 * Ts;
thijslubberman 8:c7d21f5f87d8 486 u2 = kp2*error2 + int_u2*ki2 + kd2*filtered_d_error2;
thijslubberman 8:c7d21f5f87d8 487 teraterm2 = u2;
thijslubberman 8:c7d21f5f87d8 488 if(u2>1){
thijslubberman 8:c7d21f5f87d8 489 u2 =1;
thijslubberman 8:c7d21f5f87d8 490 }
thijslubberman 8:c7d21f5f87d8 491 if(u2<-1){
thijslubberman 8:c7d21f5f87d8 492 u2 = -1;
thijslubberman 8:c7d21f5f87d8 493 }
WouterJS 9:4f594927cff3 494 // }
WouterJS 9:4f594927cff3 495 olderror2 = error2;
WouterJS 9:4f594927cff3 496
thijslubberman 7:db050a878cff 497
WouterJS 1:a9c933f1dc71 498 }
WouterJS 1:a9c933f1dc71 499
WouterJS 1:a9c933f1dc71 500 void loop_function() { //Main loop function
WouterJS 0:3710031b2621 501 measureall();
WouterJS 0:3710031b2621 502 switch(current_state) {
WouterJS 0:3710031b2621 503 case failure:
WouterJS 0:3710031b2621 504 do_state_failure(); // a separate function for what happens in each state
WouterJS 0:3710031b2621 505 break;
WouterJS 1:a9c933f1dc71 506 case calib_motor:
WouterJS 1:a9c933f1dc71 507 do_state_calib_motor();
WouterJS 1:a9c933f1dc71 508 break ;
WouterJS 4:34ad002cb646 509 case homing:
WouterJS 4:34ad002cb646 510 do_state_homing();
WouterJS 4:34ad002cb646 511 break;
WouterJS 0:3710031b2621 512 case calib_emg:
WouterJS 0:3710031b2621 513 do_state_calib_emg();
WouterJS 0:3710031b2621 514 break;
WouterJS 0:3710031b2621 515 case operational:
WouterJS 0:3710031b2621 516 do_state_operational();
WouterJS 0:3710031b2621 517 break;
WouterJS 1:a9c933f1dc71 518 case waiting:
WouterJS 1:a9c933f1dc71 519 do_state_waiting();
WouterJS 0:3710031b2621 520 break;
WouterJS 0:3710031b2621 521 }
WouterJS 0:3710031b2621 522 motor_controller();
WouterJS 1:a9c933f1dc71 523 // Outputs data to the computer
thijslubberman 7:db050a878cff 524
WouterJS 0:3710031b2621 525 }
WouterJS 0:3710031b2621 526
WouterJS 0:3710031b2621 527
WouterJS 0:3710031b2621 528 int main()
WouterJS 1:a9c933f1dc71 529 {
WouterJS 4:34ad002cb646 530 // motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
WouterJS 3:055eb9f256fc 531
WouterJS 5:892531e4e015 532 motor1_direction = 0; //set motor 1 to run clockwisedirection for calibration
thijslubberman 7:db050a878cff 533 motor1_speed_control = 0.15;//to make sure the motor will not run at too high velocity
WouterJS 5:892531e4e015 534 motor2_direction = 0; // set motor 2 to run clockwise direction
thijslubberman 7:db050a878cff 535 motor2_speed_control = 0.15;
WouterJS 4:34ad002cb646 536
thijslubberman 7:db050a878cff 537 led_red = 1;
thijslubberman 7:db050a878cff 538 led_green = 1;
thijslubberman 7:db050a878cff 539 led_blue = 1;
WouterJS 3:055eb9f256fc 540 timer.start();
WouterJS 3:055eb9f256fc 541 loop_timer.attach(&loop_function, Ts);
WouterJS 3:055eb9f256fc 542 sample_timer.attach(&scopedata, Ts);
WouterJS 3:055eb9f256fc 543 sample_timer2.attach(&filterall, Ts);
WouterJS 9:4f594927cff3 544
WouterJS 9:4f594927cff3 545 pc.baud(115200);
WouterJS 2:fa90eaa14f99 546 while (true) {
WouterJS 9:4f594927cff3 547 printf("%f %f %f %f %f %f \n\r",ref_q1,ref_q2,error1,error2,deg_m1, deg_m2);
WouterJS 0:3710031b2621 548 }
WouterJS 0:3710031b2621 549 }