Thijs Lubberman / Mbed 2 deprecated MotorArchitecture1-11

juijiu

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture1-11 by Wouter Schuttert

main.cpp@10:91173ed1e841, 2018-11-01 (annotated)

Committer:
thijslubberman
Date:
Thu Nov 01 16:50:47 2018 +0000
Revision:
10:91173ed1e841
Parent:
9:4f594927cff3
Child:
11:c7e27de26ac0
werkt een beetje

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;
thijslubberman 10:91173ed1e841 15 double pi = 3.141592653589793238462643383279502884;
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
thijslubberman 10:91173ed1e841 156 int sign1 = 0.5;
thijslubberman 10:91173ed1e841 157 int sign2 = 1;
WouterJS 1:a9c933f1dc71 158 void do_forward(){
WouterJS 1:a9c933f1dc71 159
thijslubberman 7:db050a878cff 160
thijslubberman 8:c7d21f5f87d8 161 twistf[0] = 1;
thijslubberman 8:c7d21f5f87d8 162 twistf[1] = 0;
thijslubberman 7:db050a878cff 163
WouterJS 9:4f594927cff3 164 if (but1 == false){
WouterJS 9:4f594927cff3 165 //abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2);
WouterJS 9:4f594927cff3 166 abs_sig = 0.4;
WouterJS 1:a9c933f1dc71 167
WouterJS 1:a9c933f1dc71 168 }
thijslubberman 7:db050a878cff 169 else
thijslubberman 7:db050a878cff 170 {
thijslubberman 7:db050a878cff 171 abs_sig = 0;
thijslubberman 7:db050a878cff 172 }
WouterJS 9:4f594927cff3 173
WouterJS 1:a9c933f1dc71 174
WouterJS 9:4f594927cff3 175 twist[0] = twistf[0] * abs_sig ;
WouterJS 9:4f594927cff3 176 twist[1] = twistf[1] * abs_sig ;
thijslubberman 7:db050a878cff 177
thijslubberman 7:db050a878cff 178 motor1_speed_control = fabs(u1);
WouterJS 1:a9c933f1dc71 179
thijslubberman 7:db050a878cff 180 if(u1 > 0){
thijslubberman 7:db050a878cff 181 motor1_direction = 1;}
thijslubberman 7:db050a878cff 182 if(u1 < 0){
thijslubberman 7:db050a878cff 183 motor1_direction = 0;}
thijslubberman 7:db050a878cff 184
thijslubberman 7:db050a878cff 185 motor2_speed_control = fabs(u2);
thijslubberman 7:db050a878cff 186
thijslubberman 7:db050a878cff 187 if(u2 > 0){
thijslubberman 7:db050a878cff 188 motor2_direction = 1;}
thijslubberman 7:db050a878cff 189 if(u2 < 0){
thijslubberman 7:db050a878cff 190 motor2_direction = 0;}
thijslubberman 7:db050a878cff 191
thijslubberman 7:db050a878cff 192 //if( timer.read() > thresholdtime && filteredsignal1 > threshold1*100)
thijslubberman 7:db050a878cff 193 // {
thijslubberman 7:db050a878cff 194 // movement = backward;
thijslubberman 7:db050a878cff 195 // timer.reset();
thijslubberman 7:db050a878cff 196 // }
WouterJS 1:a9c933f1dc71 197 }
WouterJS 1:a9c933f1dc71 198
WouterJS 1:a9c933f1dc71 199 void do_backward(){
WouterJS 1:a9c933f1dc71 200
WouterJS 1:a9c933f1dc71 201
WouterJS 1:a9c933f1dc71 202
WouterJS 1:a9c933f1dc71 203 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 204 {
WouterJS 1:a9c933f1dc71 205 movement = up;
WouterJS 1:a9c933f1dc71 206 timer.reset();
WouterJS 1:a9c933f1dc71 207 }
WouterJS 1:a9c933f1dc71 208 }
WouterJS 1:a9c933f1dc71 209
WouterJS 1:a9c933f1dc71 210 void do_up(){
WouterJS 1:a9c933f1dc71 211
WouterJS 1:a9c933f1dc71 212 //Code for moving up
WouterJS 1:a9c933f1dc71 213
WouterJS 1:a9c933f1dc71 214 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 215 {
WouterJS 1:a9c933f1dc71 216 movement = down;
WouterJS 1:a9c933f1dc71 217 timer.reset();
WouterJS 1:a9c933f1dc71 218 }
WouterJS 1:a9c933f1dc71 219 }
WouterJS 1:a9c933f1dc71 220 void do_down(){
WouterJS 1:a9c933f1dc71 221
WouterJS 1:a9c933f1dc71 222 //Code for moving down
WouterJS 1:a9c933f1dc71 223
WouterJS 1:a9c933f1dc71 224 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 225 {
WouterJS 1:a9c933f1dc71 226 movement = rest;
WouterJS 1:a9c933f1dc71 227 timer.reset();
WouterJS 1:a9c933f1dc71 228
WouterJS 1:a9c933f1dc71 229 }
WouterJS 1:a9c933f1dc71 230 }
WouterJS 1:a9c933f1dc71 231 void do_wait(){
WouterJS 1:a9c933f1dc71 232
WouterJS 1:a9c933f1dc71 233 if ( filteredsignal2 > threshold2) {//
WouterJS 1:a9c933f1dc71 234 current_state = waiting;
WouterJS 1:a9c933f1dc71 235 state_changed = true;
WouterJS 1:a9c933f1dc71 236 }
WouterJS 1:a9c933f1dc71 237 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 238 {
WouterJS 1:a9c933f1dc71 239 movement = forward;
WouterJS 1:a9c933f1dc71 240 timer.reset();
WouterJS 1:a9c933f1dc71 241 }
WouterJS 1:a9c933f1dc71 242 }
WouterJS 1:a9c933f1dc71 243 ///////////END MOVEMENT STATES/////////////////////////
WouterJS 1:a9c933f1dc71 244 ///////////ROBOT ARM STATES ///////////////////////////
WouterJS 9:4f594927cff3 245 float kp1 = 0.1;
WouterJS 9:4f594927cff3 246 float kp2 = 0.1;
WouterJS 9:4f594927cff3 247 float ki1 = 0.05;
WouterJS 9:4f594927cff3 248 float ki2 = 0.05;
WouterJS 9:4f594927cff3 249 float kd1 = 0.005;
WouterJS 9:4f594927cff3 250 float kd2 = 0.005;
WouterJS 9:4f594927cff3 251 float olderror1;
WouterJS 9:4f594927cff3 252 float olderror2;
WouterJS 9:4f594927cff3 253 float d_error1;
WouterJS 9:4f594927cff3 254 float d_error2;
WouterJS 1:a9c933f1dc71 255 void do_state_failure(){
WouterJS 1:a9c933f1dc71 256
WouterJS 1:a9c933f1dc71 257 }
WouterJS 9:4f594927cff3 258
WouterJS 4:34ad002cb646 259 int count1 = 0;
WouterJS 4:34ad002cb646 260 int count2 = 0;
WouterJS 9:4f594927cff3 261
WouterJS 1:a9c933f1dc71 262 void do_state_calib_motor(){
WouterJS 1:a9c933f1dc71 263 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 264 state_changed = false;
WouterJS 1:a9c933f1dc71 265 }
thijslubberman 7:db050a878cff 266 led_red = 0;
WouterJS 1:a9c933f1dc71 267
WouterJS 1:a9c933f1dc71 268
WouterJS 4:34ad002cb646 269
WouterJS 4:34ad002cb646 270 int deriv1 = deg_m1 - count1;
WouterJS 4:34ad002cb646 271 int deriv2 = deg_m2 - count2;
WouterJS 5:892531e4e015 272
WouterJS 4:34ad002cb646 273 count1 = deg_m1;
WouterJS 4:34ad002cb646 274 count2 = deg_m2;
WouterJS 4:34ad002cb646 275
WouterJS 4:34ad002cb646 276 if ( timer.read() > 3 && deriv1 < 0.5 && deriv2 < 0.5) {
WouterJS 4:34ad002cb646 277 motor1_speed_control = 0;
WouterJS 4:34ad002cb646 278 motor2_speed_control = 0;
WouterJS 4:34ad002cb646 279 current_state = homing;
thijslubberman 7:db050a878cff 280
WouterJS 4:34ad002cb646 281 state_changed = true;
thijslubberman 7:db050a878cff 282 wait(1);
thijslubberman 7:db050a878cff 283 deg_m1 = 57.4;
thijslubberman 7:db050a878cff 284 deg_m2 = 72.93;
thijslubberman 7:db050a878cff 285 led_red = 1;
thijslubberman 7:db050a878cff 286 led_green = 0;
WouterJS 4:34ad002cb646 287 }
WouterJS 1:a9c933f1dc71 288 }
WouterJS 1:a9c933f1dc71 289
WouterJS 5:892531e4e015 290 float wu1;
WouterJS 5:892531e4e015 291 float wu2;
thijslubberman 7:db050a878cff 292
WouterJS 4:34ad002cb646 293 void do_state_homing(){
WouterJS 4:34ad002cb646 294 if (state_changed==true) {
thijslubberman 7:db050a878cff 295 timer.reset();
WouterJS 4:34ad002cb646 296 state_changed = false;
WouterJS 4:34ad002cb646 297 }
WouterJS 9:4f594927cff3 298 float kp11 = 0.1;
WouterJS 9:4f594927cff3 299 float kp12 = 0.1;
WouterJS 9:4f594927cff3 300
WouterJS 4:34ad002cb646 301 float werror1 = deg_m1 - 0;
WouterJS 4:34ad002cb646 302 float werror2 = deg_m2 - 0;
WouterJS 1:a9c933f1dc71 303
WouterJS 5:892531e4e015 304 if(werror1 > 5){
WouterJS 5:892531e4e015 305 wu1 = 1; }
WouterJS 5:892531e4e015 306 if(werror1 < -5){
WouterJS 5:892531e4e015 307 wu1 = -1; }
WouterJS 5:892531e4e015 308 else{
WouterJS 9:4f594927cff3 309 wu1 = kp11*werror1; //+ (u1 + werror1*0.002)*ki1;
WouterJS 5:892531e4e015 310 }
WouterJS 1:a9c933f1dc71 311
WouterJS 5:892531e4e015 312 if(werror2 > 5){
WouterJS 5:892531e4e015 313 wu2 = 1;}
WouterJS 5:892531e4e015 314 if(werror2 < -5){
WouterJS 5:892531e4e015 315 wu2 = -1;}
WouterJS 5:892531e4e015 316 else{
WouterJS 9:4f594927cff3 317 wu2 = kp12*werror2; //+ (u2 + werror2*0.002)*ki2);
WouterJS 5:892531e4e015 318 }
WouterJS 5:892531e4e015 319
thijslubberman 7:db050a878cff 320 motor1_speed_control = fabs(wu1)/8;
WouterJS 5:892531e4e015 321
WouterJS 4:34ad002cb646 322 if(wu1 > 0){
thijslubberman 7:db050a878cff 323 motor1_direction = 1;}
WouterJS 4:34ad002cb646 324 if(wu1< 0){
thijslubberman 7:db050a878cff 325 motor1_direction = 0;}
WouterJS 4:34ad002cb646 326
thijslubberman 7:db050a878cff 327 motor2_speed_control = fabs(wu2)/8;
WouterJS 4:34ad002cb646 328
WouterJS 4:34ad002cb646 329 if(wu2 > 0){
thijslubberman 7:db050a878cff 330 motor2_direction = 1;}
WouterJS 5:892531e4e015 331 if(wu2 < 0){
thijslubberman 7:db050a878cff 332 motor2_direction = 0;}
thijslubberman 7:db050a878cff 333
thijslubberman 7:db050a878cff 334 if ( timer.read() > 4) {
thijslubberman 7:db050a878cff 335 motor1_speed_control = 0;
WouterJS 9:4f594927cff3 336 motor2_speed_control = 0;
WouterJS 4:34ad002cb646 337
WouterJS 9:4f594927cff3 338 ref_q1 = 0;
WouterJS 9:4f594927cff3 339 ref_q2 = 0;
thijslubberman 7:db050a878cff 340 deg_m1 = 0;
thijslubberman 7:db050a878cff 341 deg_m2 = 0;
WouterJS 9:4f594927cff3 342 d_error1 = 0;
WouterJS 9:4f594927cff3 343 d_error2 = 0;
thijslubberman 7:db050a878cff 344 u1 = 0;
thijslubberman 7:db050a878cff 345 u2 = 0;
thijslubberman 7:db050a878cff 346 int_u1 = 0;
thijslubberman 7:db050a878cff 347 int_u2 = 0;
WouterJS 9:4f594927cff3 348 wait(1);
thijslubberman 7:db050a878cff 349 current_state = calib_emg;
thijslubberman 7:db050a878cff 350 timer.reset();
thijslubberman 7:db050a878cff 351 state_changed = true;
thijslubberman 7:db050a878cff 352 led_green = 1;
thijslubberman 7:db050a878cff 353 led_blue = 0;
thijslubberman 7:db050a878cff 354
thijslubberman 7:db050a878cff 355 }
WouterJS 4:34ad002cb646 356
WouterJS 4:34ad002cb646 357
WouterJS 1:a9c933f1dc71 358 }
WouterJS 1:a9c933f1dc71 359 void do_state_calib_emg(){
WouterJS 1:a9c933f1dc71 360 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 361 state_changed = false;
WouterJS 1:a9c933f1dc71 362 }
WouterJS 1:a9c933f1dc71 363
WouterJS 1:a9c933f1dc71 364 if(filteredsignal1 > max1){//calibrate to a new maximum
WouterJS 1:a9c933f1dc71 365 max1 = filteredsignal1;
thijslubberman 7:db050a878cff 366 threshold1 = 0.5 * max1;
WouterJS 1:a9c933f1dc71 367 }
WouterJS 1:a9c933f1dc71 368 if(filteredsignal2 > max2){//calibrate to a new maximum
WouterJS 1:a9c933f1dc71 369 max2 = filteredsignal2;
WouterJS 1:a9c933f1dc71 370 threshold2 = 0.5 * max2;
WouterJS 1:a9c933f1dc71 371 }
WouterJS 1:a9c933f1dc71 372
thijslubberman 10:91173ed1e841 373 if (timer.read() > 1 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 374 current_state = operational;
WouterJS 1:a9c933f1dc71 375 timer.reset();
WouterJS 1:a9c933f1dc71 376 state_changed = true;
thijslubberman 7:db050a878cff 377 led_green = 0;
thijslubberman 7:db050a878cff 378 led_red =0;
WouterJS 1:a9c933f1dc71 379 }
WouterJS 1:a9c933f1dc71 380 }
WouterJS 1:a9c933f1dc71 381
WouterJS 1:a9c933f1dc71 382 void do_state_operational(){
WouterJS 1:a9c933f1dc71 383 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 384 state_changed = false;
WouterJS 1:a9c933f1dc71 385 }
WouterJS 1:a9c933f1dc71 386
WouterJS 1:a9c933f1dc71 387 switch(movement) {// a separate function for what happens in each state
WouterJS 1:a9c933f1dc71 388 case rest:
WouterJS 1:a9c933f1dc71 389 do_wait();
WouterJS 1:a9c933f1dc71 390 break;
WouterJS 1:a9c933f1dc71 391 case forward:
WouterJS 1:a9c933f1dc71 392 do_forward();
WouterJS 1:a9c933f1dc71 393
WouterJS 1:a9c933f1dc71 394 break;
WouterJS 1:a9c933f1dc71 395 case backward:
WouterJS 1:a9c933f1dc71 396 do_backward();
WouterJS 1:a9c933f1dc71 397
WouterJS 1:a9c933f1dc71 398 break;
WouterJS 1:a9c933f1dc71 399 case up:
WouterJS 1:a9c933f1dc71 400 do_up();
WouterJS 1:a9c933f1dc71 401 break;
WouterJS 1:a9c933f1dc71 402 case down:
WouterJS 1:a9c933f1dc71 403 do_down();
WouterJS 1:a9c933f1dc71 404 break;
WouterJS 1:a9c933f1dc71 405 }
WouterJS 1:a9c933f1dc71 406 if (movement == rest && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 407 current_state = waiting;
WouterJS 1:a9c933f1dc71 408 state_changed = true;
WouterJS 1:a9c933f1dc71 409 }
WouterJS 1:a9c933f1dc71 410
WouterJS 1:a9c933f1dc71 411 }
WouterJS 1:a9c933f1dc71 412
WouterJS 1:a9c933f1dc71 413 void do_state_waiting(){
WouterJS 1:a9c933f1dc71 414 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 415 state_changed = false;
WouterJS 1:a9c933f1dc71 416 }
WouterJS 1:a9c933f1dc71 417
WouterJS 1:a9c933f1dc71 418 if (filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 419 current_state = operational;
WouterJS 1:a9c933f1dc71 420 state_changed = true;
WouterJS 1:a9c933f1dc71 421 }
WouterJS 1:a9c933f1dc71 422 }
WouterJS 1:a9c933f1dc71 423 //////////////// END ROBOT ARM STATES //////////////////////////////
WouterJS 4:34ad002cb646 424
WouterJS 4:34ad002cb646 425
thijslubberman 7:db050a878cff 426 void motor_controller(){
thijslubberman 7:db050a878cff 427
WouterJS 4:34ad002cb646 428 float jacobian[4];
WouterJS 4:34ad002cb646 429 float inv_jacobian[4];
WouterJS 4:34ad002cb646 430
WouterJS 4:34ad002cb646 431 jacobian[0] = L1;
thijslubberman 10:91173ed1e841 432 jacobian[1] = (L2*cos(deg_m2/180*pi))+L1;
WouterJS 4:34ad002cb646 433 jacobian[2] = -L0;
thijslubberman 10:91173ed1e841 434 jacobian[3] = -L0 - (L2*sin(deg_m2/180*pi));
WouterJS 1:a9c933f1dc71 435
WouterJS 9:4f594927cff3 436 float det = 1/((jacobian[0]*jacobian[3])-(jacobian[1]*jacobian[2]));
WouterJS 1:a9c933f1dc71 437
WouterJS 4:34ad002cb646 438 inv_jacobian[0] = det*jacobian[3];
WouterJS 4:34ad002cb646 439 inv_jacobian[1] = -det*jacobian[1];
WouterJS 4:34ad002cb646 440 inv_jacobian[2] = -det*jacobian[2];
WouterJS 4:34ad002cb646 441 inv_jacobian[3] = det*jacobian[0];
WouterJS 4:34ad002cb646 442
WouterJS 4:34ad002cb646 443
thijslubberman 7:db050a878cff 444 ref_v1 = inv_jacobian[0]*twist[0]+inv_jacobian[1]*twist[1];
thijslubberman 7:db050a878cff 445 ref_v2 = inv_jacobian[2]*twist[0]+inv_jacobian[3]*twist[1];
WouterJS 4:34ad002cb646 446
thijslubberman 7:db050a878cff 447
WouterJS 4:34ad002cb646 448
thijslubberman 10:91173ed1e841 449 ref_q1 = ref_q1 + 0.002*ref_v1 * sign1;
thijslubberman 10:91173ed1e841 450 ref_q2 = ref_q2 + 0.002*ref_v2 * sign2;
thijslubberman 7:db050a878cff 451
thijslubberman 8:c7d21f5f87d8 452 error1 = deg_m1 - ref_q1;
thijslubberman 8:c7d21f5f87d8 453 error2 = deg_m2 - ref_q2;
WouterJS 9:4f594927cff3 454
WouterJS 9:4f594927cff3 455
thijslubberman 8:c7d21f5f87d8 456
thijslubberman 8:c7d21f5f87d8 457
thijslubberman 8:c7d21f5f87d8 458
WouterJS 1:a9c933f1dc71 459
WouterJS 9:4f594927cff3 460 // if(error1 > 5){
WouterJS 9:4f594927cff3 461 // u1 = 1; }
WouterJS 9:4f594927cff3 462 //if(error1 < -5){
WouterJS 9:4f594927cff3 463 // u1 = -1; }
WouterJS 9:4f594927cff3 464 //else{
thijslubberman 8:c7d21f5f87d8 465 d_error1 = (error1 - olderror1)/Ts;
WouterJS 9:4f594927cff3 466 filtered_d_error1 = LowpassFilter1.step(d_error1);
thijslubberman 7:db050a878cff 467 int_u1 = int_u1 + error1 * Ts;
thijslubberman 8:c7d21f5f87d8 468 u1 = kp1*error1 + int_u1*ki1 + kd1*filtered_d_error1;
thijslubberman 8:c7d21f5f87d8 469 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 470 if(u1>1){
thijslubberman 8:c7d21f5f87d8 471 u1 =1;
WouterJS 9:4f594927cff3 472 }
thijslubberman 8:c7d21f5f87d8 473 if(u1<-1){
thijslubberman 8:c7d21f5f87d8 474 u1 = -1;
thijslubberman 8:c7d21f5f87d8 475 }
thijslubberman 8:c7d21f5f87d8 476
WouterJS 9:4f594927cff3 477 //}
thijslubberman 8:c7d21f5f87d8 478 olderror1 = error1;
WouterJS 1:a9c933f1dc71 479
WouterJS 9:4f594927cff3 480 //if(error2 > 5){
WouterJS 9:4f594927cff3 481 // u2 = 1;}
WouterJS 9:4f594927cff3 482 // if(error2 < -5){
WouterJS 9:4f594927cff3 483 // u2 = -1;}
WouterJS 9:4f594927cff3 484 //else{
thijslubberman 8:c7d21f5f87d8 485 d_error2 = (error2 - olderror2)/Ts;
WouterJS 9:4f594927cff3 486 filtered_d_error2 = LowpassFilter2.step(d_error2);
thijslubberman 7:db050a878cff 487 int_u2 = int_u2 + error2 * Ts;
thijslubberman 8:c7d21f5f87d8 488 u2 = kp2*error2 + int_u2*ki2 + kd2*filtered_d_error2;
thijslubberman 8:c7d21f5f87d8 489 teraterm2 = u2;
thijslubberman 8:c7d21f5f87d8 490 if(u2>1){
thijslubberman 8:c7d21f5f87d8 491 u2 =1;
thijslubberman 8:c7d21f5f87d8 492 }
thijslubberman 8:c7d21f5f87d8 493 if(u2<-1){
thijslubberman 8:c7d21f5f87d8 494 u2 = -1;
thijslubberman 8:c7d21f5f87d8 495 }
WouterJS 9:4f594927cff3 496 // }
WouterJS 9:4f594927cff3 497 olderror2 = error2;
WouterJS 9:4f594927cff3 498
thijslubberman 7:db050a878cff 499
WouterJS 1:a9c933f1dc71 500 }
WouterJS 1:a9c933f1dc71 501
WouterJS 1:a9c933f1dc71 502 void loop_function() { //Main loop function
WouterJS 0:3710031b2621 503 measureall();
WouterJS 0:3710031b2621 504 switch(current_state) {
WouterJS 0:3710031b2621 505 case failure:
WouterJS 0:3710031b2621 506 do_state_failure(); // a separate function for what happens in each state
WouterJS 0:3710031b2621 507 break;
WouterJS 1:a9c933f1dc71 508 case calib_motor:
WouterJS 1:a9c933f1dc71 509 do_state_calib_motor();
WouterJS 1:a9c933f1dc71 510 break ;
WouterJS 4:34ad002cb646 511 case homing:
WouterJS 4:34ad002cb646 512 do_state_homing();
WouterJS 4:34ad002cb646 513 break;
WouterJS 0:3710031b2621 514 case calib_emg:
WouterJS 0:3710031b2621 515 do_state_calib_emg();
WouterJS 0:3710031b2621 516 break;
WouterJS 0:3710031b2621 517 case operational:
WouterJS 0:3710031b2621 518 do_state_operational();
WouterJS 0:3710031b2621 519 break;
WouterJS 1:a9c933f1dc71 520 case waiting:
WouterJS 1:a9c933f1dc71 521 do_state_waiting();
WouterJS 0:3710031b2621 522 break;
WouterJS 0:3710031b2621 523 }
WouterJS 0:3710031b2621 524 motor_controller();
WouterJS 1:a9c933f1dc71 525 // Outputs data to the computer
thijslubberman 7:db050a878cff 526
WouterJS 0:3710031b2621 527 }
WouterJS 0:3710031b2621 528
WouterJS 0:3710031b2621 529
WouterJS 0:3710031b2621 530 int main()
WouterJS 1:a9c933f1dc71 531 {
WouterJS 4:34ad002cb646 532 // motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
WouterJS 3:055eb9f256fc 533
WouterJS 5:892531e4e015 534 motor1_direction = 0; //set motor 1 to run clockwisedirection for calibration
thijslubberman 7:db050a878cff 535 motor1_speed_control = 0.15;//to make sure the motor will not run at too high velocity
WouterJS 5:892531e4e015 536 motor2_direction = 0; // set motor 2 to run clockwise direction
thijslubberman 7:db050a878cff 537 motor2_speed_control = 0.15;
WouterJS 4:34ad002cb646 538
thijslubberman 7:db050a878cff 539 led_red = 1;
thijslubberman 7:db050a878cff 540 led_green = 1;
thijslubberman 7:db050a878cff 541 led_blue = 1;
WouterJS 3:055eb9f256fc 542 timer.start();
WouterJS 3:055eb9f256fc 543 loop_timer.attach(&loop_function, Ts);
WouterJS 3:055eb9f256fc 544 sample_timer.attach(&scopedata, Ts);
WouterJS 3:055eb9f256fc 545 sample_timer2.attach(&filterall, Ts);
WouterJS 9:4f594927cff3 546
WouterJS 9:4f594927cff3 547 pc.baud(115200);
WouterJS 2:fa90eaa14f99 548 while (true) {
WouterJS 9:4f594927cff3 549 printf("%f %f %f %f %f %f \n\r",ref_q1,ref_q2,error1,error2,deg_m1, deg_m2);
WouterJS 0:3710031b2621 550 }
WouterJS 0:3710031b2621 551 }