Thijs Lubberman / Mbed 2 deprecated MotorArchitecture1-11

juijiu

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture1-11 by Wouter Schuttert

main.cpp@12:c59b25d07bb9, 2018-11-01 (annotated)

Committer:
thijslubberman
Date:
Thu Nov 01 18:24:55 2018 +0000
Revision:
12:c59b25d07bb9
Parent:
11:c7e27de26ac0 
versie avond;

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
thijslubberman 11:c7e27de26ac0 60 float kp1 = 0.1;
thijslubberman 11:c7e27de26ac0 61 float kp2 = 0.1;
thijslubberman 11:c7e27de26ac0 62 float ki1 = 0.05;
thijslubberman 11:c7e27de26ac0 63 float ki2 = 0.05;
thijslubberman 11:c7e27de26ac0 64 float kd1 = 0.005;
thijslubberman 11:c7e27de26ac0 65 float kd2 = 0.005;
thijslubberman 11:c7e27de26ac0 66 float olderror1;
thijslubberman 11:c7e27de26ac0 67 float olderror2;
thijslubberman 11:c7e27de26ac0 68 float d_error1;
thijslubberman 11:c7e27de26ac0 69 float d_error2;
thijslubberman 11:c7e27de26ac0 70
WouterJS 4:34ad002cb646 71 enum States {failure, waiting, calib_motor, homing ,calib_emg, operational, demo};
WouterJS 0:3710031b2621 72 enum Operations {rest, forward, backward, up, down};
WouterJS 0:3710031b2621 73
WouterJS 1:a9c933f1dc71 74 States current_state = calib_motor;
thijslubberman 11:c7e27de26ac0 75 Operations movement = rest;
WouterJS 0:3710031b2621 76
WouterJS 1:a9c933f1dc71 77 float max1 = 0; //initial threshold value for emg signals, changes during calibration left arm
WouterJS 1:a9c933f1dc71 78 float max2 = 0; // right arm
WouterJS 1:a9c933f1dc71 79 float threshold1;
WouterJS 1:a9c933f1dc71 80 float threshold2;
WouterJS 1:a9c933f1dc71 81 float thresholdtime = 1.0; // time waiting before switching modes
WouterJS 1:a9c933f1dc71 82
WouterJS 1:a9c933f1dc71 83 Ticker loop_timer;
WouterJS 0:3710031b2621 84 Ticker sample_timer;
WouterJS 1:a9c933f1dc71 85 Ticker sample_timer2;
thijslubberman 7:db050a878cff 86 //HIDScope scope(3);
WouterJS 0:3710031b2621 87
WouterJS 0:3710031b2621 88 AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes
WouterJS 0:3710031b2621 89 AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength
WouterJS 0:3710031b2621 90
WouterJS 1:a9c933f1dc71 91 volatile float emg1_input;
WouterJS 1:a9c933f1dc71 92 volatile float emg2_input;
WouterJS 1:a9c933f1dc71 93
WouterJS 0:3710031b2621 94 volatile float raw_filteredsignal1;//the first filtered emg signal 1
WouterJS 0:3710031b2621 95 volatile float raw_filteredsignal2;//the first filtered emg signal 2
WouterJS 0:3710031b2621 96
WouterJS 0:3710031b2621 97 volatile float filteredsignal1;//the first filtered emg signal 1
WouterJS 0:3710031b2621 98 volatile float filteredsignal2;//the first filtered emg signal 2
WouterJS 0:3710031b2621 99
WouterJS 0:3710031b2621 100 bool state_changed = false;
thijslubberman 7:db050a878cff 101 static BiQuad Notch1(0.9714498065192796,-1.5718388053127037,0.9714498065192796,-1.5718388053127037,0.9428996130385592);
thijslubberman 7:db050a878cff 102 static BiQuad Notch2(0.9714498065192796,-1.5718388053127037,0.9714498065192796,-1.5718388053127037,0.9428996130385592);
thijslubberman 7:db050a878cff 103 static BiQuad HighPass1(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354);
thijslubberman 7:db050a878cff 104 static BiQuad HighPass2(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354);
thijslubberman 7:db050a878cff 105 static BiQuad LowPass1(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906);
thijslubberman 7:db050a878cff 106 static BiQuad LowPass2(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906);
WouterJS 9:4f594927cff3 107 static BiQuad LowpassFilter1(0.0640,0.1279,0.0640,-1.1683,0.4241);
WouterJS 9:4f594927cff3 108 static BiQuad LowpassFilter2(0.0640,0.1279,0.0640,-1.1683,0.4241);
WouterJS 0:3710031b2621 109 void filterall()
WouterJS 0:3710031b2621 110 {
thijslubberman 7:db050a878cff 111 //Notch 50 Hz BW 6 Hz
thijslubberman 7:db050a878cff 112 float notch1 = Notch1.step(emg1_input);
thijslubberman 7:db050a878cff 113 float notch2 = Notch2.step(emg2_input);
WouterJS 0:3710031b2621 114 //Highpass Biquad 5 Hz
thijslubberman 7:db050a878cff 115 float high1 = HighPass1.step(notch1);
thijslubberman 7:db050a878cff 116 float high2 = HighPass2.step(notch2);
WouterJS 0:3710031b2621 117 // Rectify the signal(absolute value)
thijslubberman 7:db050a878cff 118 float abs1 = fabs(high1);
thijslubberman 7:db050a878cff 119 float abs2 = fabs(high2);
WouterJS 0:3710031b2621 120 //Lowpass Biquad 10 Hz
thijslubberman 7:db050a878cff 121 float low1 = LowPass1.step(abs1);
thijslubberman 7:db050a878cff 122 float low2 = LowPass2.step(abs2);
WouterJS 0:3710031b2621 123
WouterJS 0:3710031b2621 124 raw_filteredsignal1 = low1;
WouterJS 0:3710031b2621 125 raw_filteredsignal2 = low2;
WouterJS 0:3710031b2621 126
WouterJS 1:a9c933f1dc71 127
WouterJS 0:3710031b2621 128 }
WouterJS 0:3710031b2621 129
WouterJS 1:a9c933f1dc71 130 void measureall(){ // changes all variables according in sync with the rest of the code
WouterJS 1:a9c933f1dc71 131
WouterJS 1:a9c933f1dc71 132 emg1_input = raw_emg1_input.read();
WouterJS 1:a9c933f1dc71 133 emg2_input = raw_emg2_input.read();
WouterJS 1:a9c933f1dc71 134
WouterJS 1:a9c933f1dc71 135 filteredsignal1 = raw_filteredsignal1;
WouterJS 1:a9c933f1dc71 136 filteredsignal2 = raw_filteredsignal2;
WouterJS 1:a9c933f1dc71 137
WouterJS 1:a9c933f1dc71 138 //Reading of motor
WouterJS 1:a9c933f1dc71 139
WouterJS 1:a9c933f1dc71 140 counts_m1 = Encoder1.getPulses() - counts_m1_prev;
WouterJS 6:b526cf83fd4f 141 counts_m2 = Encoder2.getPulses() - counts_m2_prev;
WouterJS 1:a9c933f1dc71 142 deg_m1 = deg_m1 + counts_m1*(360/(full_ratio));
WouterJS 1:a9c933f1dc71 143 deg_m2 = deg_m2 + counts_m2*(360/(full_ratio));
WouterJS 1:a9c933f1dc71 144 counts_m1_prev = Encoder1.getPulses();
WouterJS 5:892531e4e015 145 counts_m2_prev = Encoder2.getPulses();
WouterJS 1:a9c933f1dc71 146
WouterJS 1:a9c933f1dc71 147 }
WouterJS 1:a9c933f1dc71 148
WouterJS 0:3710031b2621 149 void scopedata()
WouterJS 0:3710031b2621 150 {
thijslubberman 7:db050a878cff 151 //scope.set(0,filteredsignal1); //
thijslubberman 7:db050a878cff 152 //scope.set(1,filteredsignal2); //
thijslubberman 7:db050a878cff 153 //scope.set(2,ref_q1); //
thijslubberman 7:db050a878cff 154 // scope.set(3,movement);//
thijslubberman 7:db050a878cff 155 // scope.set(4,ref_q1);
WouterJS 4:34ad002cb646 156 //scope.send(); // send info to HIDScope server
WouterJS 0:3710031b2621 157 }
WouterJS 0:3710031b2621 158
WouterJS 1:a9c933f1dc71 159
WouterJS 1:a9c933f1dc71 160 //////////////////// MOVEMENT STATES
thijslubberman 7:db050a878cff 161 float twist[2] = {0,0};
thijslubberman 7:db050a878cff 162 float twistf[2] = {0,0};
thijslubberman 7:db050a878cff 163 float abs_sig;
thijslubberman 7:db050a878cff 164
thijslubberman 8:c7d21f5f87d8 165 int gain = 3;
thijslubberman 7:db050a878cff 166
thijslubberman 10:91173ed1e841 167 int sign1 = 0.5;
thijslubberman 10:91173ed1e841 168 int sign2 = 1;
thijslubberman 11:c7e27de26ac0 169
WouterJS 1:a9c933f1dc71 170 void do_forward(){
WouterJS 1:a9c933f1dc71 171
thijslubberman 11:c7e27de26ac0 172 sign1 = 0.5;
thijslubberman 11:c7e27de26ac0 173 sign2 = 1;
thijslubberman 8:c7d21f5f87d8 174 twistf[0] = 1;
thijslubberman 8:c7d21f5f87d8 175 twistf[1] = 0;
thijslubberman 7:db050a878cff 176
thijslubberman 12:c59b25d07bb9 177 if (filteredsignal2 > (0.4*max2)){
thijslubberman 12:c59b25d07bb9 178 abs_sig = (filteredsignal2 - (0.4*max2))/(0.6*max2)* gain;
WouterJS 1:a9c933f1dc71 179 }
thijslubberman 7:db050a878cff 180 else
thijslubberman 11:c7e27de26ac0 181 { abs_sig = 0;
thijslubberman 11:c7e27de26ac0 182 }
thijslubberman 11:c7e27de26ac0 183 twist[0] = twistf[0] * abs_sig ;
thijslubberman 11:c7e27de26ac0 184 twist[1] = twistf[1] * abs_sig ;
thijslubberman 11:c7e27de26ac0 185
thijslubberman 11:c7e27de26ac0 186 motor1_speed_control = fabs(u1);
thijslubberman 11:c7e27de26ac0 187
thijslubberman 11:c7e27de26ac0 188 if(u1 > 0){
thijslubberman 11:c7e27de26ac0 189 motor1_direction = 1;}
thijslubberman 11:c7e27de26ac0 190 if(u1 < 0){
thijslubberman 11:c7e27de26ac0 191 motor1_direction = 0;}
thijslubberman 11:c7e27de26ac0 192
thijslubberman 11:c7e27de26ac0 193 motor2_speed_control = fabs(u2);
thijslubberman 11:c7e27de26ac0 194
thijslubberman 11:c7e27de26ac0 195 if(u2 > 0){
thijslubberman 11:c7e27de26ac0 196 motor2_direction = 1;}
thijslubberman 11:c7e27de26ac0 197 if(u2 < 0){
thijslubberman 11:c7e27de26ac0 198 motor2_direction = 0;}
thijslubberman 11:c7e27de26ac0 199
thijslubberman 11:c7e27de26ac0 200 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
thijslubberman 11:c7e27de26ac0 201 {
thijslubberman 11:c7e27de26ac0 202 movement = backward;
thijslubberman 11:c7e27de26ac0 203 timer.reset();
thijslubberman 11:c7e27de26ac0 204
thijslubberman 11:c7e27de26ac0 205 d_error1 = 0;
thijslubberman 11:c7e27de26ac0 206 d_error2 = 0;
thijslubberman 11:c7e27de26ac0 207 u1 = 0;
thijslubberman 11:c7e27de26ac0 208 u2 = 0;
thijslubberman 11:c7e27de26ac0 209 int_u1 = 0;
thijslubberman 11:c7e27de26ac0 210 int_u2 = 0;
thijslubberman 11:c7e27de26ac0 211 led_green = 0;
thijslubberman 11:c7e27de26ac0 212 }
thijslubberman 11:c7e27de26ac0 213 }
thijslubberman 11:c7e27de26ac0 214
thijslubberman 11:c7e27de26ac0 215 void do_backward(){
thijslubberman 11:c7e27de26ac0 216
thijslubberman 11:c7e27de26ac0 217 sign1 = -1;
thijslubberman 11:c7e27de26ac0 218 sign2 = 1;
thijslubberman 11:c7e27de26ac0 219 twistf[0] = -1;
thijslubberman 11:c7e27de26ac0 220 twistf[1] = 0;
thijslubberman 11:c7e27de26ac0 221
thijslubberman 12:c59b25d07bb9 222 if (filteredsignal2 > (0.4*max2)){
thijslubberman 12:c59b25d07bb9 223 abs_sig = (filteredsignal2 - (0.4*max2))/(0.6*max2)* gain;
thijslubberman 7:db050a878cff 224 }
thijslubberman 11:c7e27de26ac0 225 else
thijslubberman 11:c7e27de26ac0 226 { abs_sig = 0;
thijslubberman 11:c7e27de26ac0 227 }
WouterJS 9:4f594927cff3 228 twist[0] = twistf[0] * abs_sig ;
WouterJS 9:4f594927cff3 229 twist[1] = twistf[1] * abs_sig ;
thijslubberman 7:db050a878cff 230
thijslubberman 7:db050a878cff 231 motor1_speed_control = fabs(u1);
WouterJS 1:a9c933f1dc71 232
thijslubberman 7:db050a878cff 233 if(u1 > 0){
thijslubberman 7:db050a878cff 234 motor1_direction = 1;}
thijslubberman 7:db050a878cff 235 if(u1 < 0){
thijslubberman 7:db050a878cff 236 motor1_direction = 0;}
thijslubberman 7:db050a878cff 237
thijslubberman 7:db050a878cff 238 motor2_speed_control = fabs(u2);
thijslubberman 7:db050a878cff 239
thijslubberman 7:db050a878cff 240 if(u2 > 0){
thijslubberman 7:db050a878cff 241 motor2_direction = 1;}
thijslubberman 7:db050a878cff 242 if(u2 < 0){
thijslubberman 7:db050a878cff 243 motor2_direction = 0;}
thijslubberman 7:db050a878cff 244
WouterJS 1:a9c933f1dc71 245 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 246 {
thijslubberman 11:c7e27de26ac0 247 movement = up;
thijslubberman 11:c7e27de26ac0 248 timer.reset();
thijslubberman 11:c7e27de26ac0 249
thijslubberman 11:c7e27de26ac0 250 d_error1 = 0;
thijslubberman 11:c7e27de26ac0 251 d_error2 = 0;
thijslubberman 11:c7e27de26ac0 252 u1 = 0;
thijslubberman 11:c7e27de26ac0 253 u2 = 0;
thijslubberman 11:c7e27de26ac0 254 int_u1 = 0;
thijslubberman 11:c7e27de26ac0 255 int_u2 = 0;
thijslubberman 11:c7e27de26ac0 256 led_red = 1;
WouterJS 1:a9c933f1dc71 257 }
WouterJS 1:a9c933f1dc71 258 }
WouterJS 1:a9c933f1dc71 259
WouterJS 1:a9c933f1dc71 260 void do_up(){
WouterJS 1:a9c933f1dc71 261
thijslubberman 11:c7e27de26ac0 262 sign1 = 1;
thijslubberman 11:c7e27de26ac0 263 sign2 = 1;
thijslubberman 11:c7e27de26ac0 264 twistf[0] = 0;
thijslubberman 11:c7e27de26ac0 265 twistf[1] = 1;
thijslubberman 11:c7e27de26ac0 266
thijslubberman 12:c59b25d07bb9 267 if (filteredsignal2 > (0.4*max2)){
thijslubberman 12:c59b25d07bb9 268 abs_sig = (filteredsignal2 - (0.4*max2))/(0.6*max2)* gain;
thijslubberman 11:c7e27de26ac0 269 }
thijslubberman 11:c7e27de26ac0 270 else
thijslubberman 11:c7e27de26ac0 271 { abs_sig = 0;
thijslubberman 11:c7e27de26ac0 272 }
thijslubberman 11:c7e27de26ac0 273 twist[0] = twistf[0] * abs_sig ;
thijslubberman 11:c7e27de26ac0 274 twist[1] = twistf[1] * abs_sig ;
thijslubberman 11:c7e27de26ac0 275
thijslubberman 11:c7e27de26ac0 276 motor1_speed_control = fabs(u1);
thijslubberman 11:c7e27de26ac0 277
thijslubberman 11:c7e27de26ac0 278 if(u1 > 0){
thijslubberman 11:c7e27de26ac0 279 motor1_direction = 1;}
thijslubberman 11:c7e27de26ac0 280 if(u1 < 0){
thijslubberman 11:c7e27de26ac0 281 motor1_direction = 0;}
thijslubberman 11:c7e27de26ac0 282
thijslubberman 11:c7e27de26ac0 283 motor2_speed_control = fabs(u2);
thijslubberman 11:c7e27de26ac0 284
thijslubberman 11:c7e27de26ac0 285 if(u2 > 0){
thijslubberman 11:c7e27de26ac0 286 motor2_direction = 1;}
thijslubberman 11:c7e27de26ac0 287 if(u2 < 0){
thijslubberman 11:c7e27de26ac0 288 motor2_direction = 0;}
WouterJS 1:a9c933f1dc71 289
WouterJS 1:a9c933f1dc71 290 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 291 {
thijslubberman 11:c7e27de26ac0 292 movement = down;
thijslubberman 11:c7e27de26ac0 293 timer.reset();
thijslubberman 11:c7e27de26ac0 294
thijslubberman 11:c7e27de26ac0 295 d_error1 = 0;
thijslubberman 11:c7e27de26ac0 296 d_error2 = 0;
thijslubberman 11:c7e27de26ac0 297 u1 = 0;
thijslubberman 11:c7e27de26ac0 298 u2 = 0;
thijslubberman 11:c7e27de26ac0 299 int_u1 = 0;
thijslubberman 11:c7e27de26ac0 300 int_u2 = 0;
thijslubberman 11:c7e27de26ac0 301 led_green = 1;
thijslubberman 11:c7e27de26ac0 302 led_blue = 0;
WouterJS 1:a9c933f1dc71 303 }
WouterJS 1:a9c933f1dc71 304 }
WouterJS 1:a9c933f1dc71 305 void do_down(){
WouterJS 1:a9c933f1dc71 306
thijslubberman 11:c7e27de26ac0 307 sign1 = 1;
thijslubberman 11:c7e27de26ac0 308 sign2 = -1;
thijslubberman 11:c7e27de26ac0 309 twistf[0] = 0;
thijslubberman 11:c7e27de26ac0 310 twistf[1] = -1;
thijslubberman 11:c7e27de26ac0 311
thijslubberman 12:c59b25d07bb9 312 if (filteredsignal2 > (0.4*max2)){
thijslubberman 12:c59b25d07bb9 313 abs_sig = (filteredsignal2 - (0.4*max2))/(0.6*max2)* gain;
thijslubberman 11:c7e27de26ac0 314 }
thijslubberman 11:c7e27de26ac0 315 else
thijslubberman 11:c7e27de26ac0 316 { abs_sig = 0;
thijslubberman 11:c7e27de26ac0 317 }
thijslubberman 11:c7e27de26ac0 318 twist[0] = twistf[0] * abs_sig ;
thijslubberman 11:c7e27de26ac0 319 twist[1] = twistf[1] * abs_sig ;
thijslubberman 11:c7e27de26ac0 320
thijslubberman 11:c7e27de26ac0 321 motor1_speed_control = fabs(u1);
thijslubberman 11:c7e27de26ac0 322
thijslubberman 11:c7e27de26ac0 323 if(u1 > 0){
thijslubberman 11:c7e27de26ac0 324 motor1_direction = 1;}
thijslubberman 11:c7e27de26ac0 325 if(u1 < 0){
thijslubberman 11:c7e27de26ac0 326 motor1_direction = 0;}
thijslubberman 11:c7e27de26ac0 327
thijslubberman 11:c7e27de26ac0 328 motor2_speed_control = fabs(u2);
thijslubberman 11:c7e27de26ac0 329
thijslubberman 11:c7e27de26ac0 330 if(u2 > 0){
thijslubberman 11:c7e27de26ac0 331 motor2_direction = 1;}
thijslubberman 11:c7e27de26ac0 332 if(u2 < 0){
thijslubberman 11:c7e27de26ac0 333 motor2_direction = 0;}
WouterJS 1:a9c933f1dc71 334
WouterJS 1:a9c933f1dc71 335 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 336 {
thijslubberman 11:c7e27de26ac0 337 movement = rest;
thijslubberman 11:c7e27de26ac0 338 timer.reset();
thijslubberman 11:c7e27de26ac0 339
thijslubberman 11:c7e27de26ac0 340 d_error1 = 0;
thijslubberman 11:c7e27de26ac0 341 d_error2 = 0;
thijslubberman 11:c7e27de26ac0 342 u1 = 0;
thijslubberman 11:c7e27de26ac0 343 u2 = 0;
thijslubberman 11:c7e27de26ac0 344 int_u1 = 0;
thijslubberman 11:c7e27de26ac0 345 int_u2 = 0;
thijslubberman 11:c7e27de26ac0 346 led_red = 0;
thijslubberman 11:c7e27de26ac0 347 led_green = 0;
WouterJS 1:a9c933f1dc71 348
WouterJS 1:a9c933f1dc71 349 }
WouterJS 1:a9c933f1dc71 350 }
WouterJS 1:a9c933f1dc71 351 void do_wait(){
WouterJS 1:a9c933f1dc71 352
thijslubberman 11:c7e27de26ac0 353
WouterJS 1:a9c933f1dc71 354 if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
WouterJS 1:a9c933f1dc71 355 {
WouterJS 1:a9c933f1dc71 356 movement = forward;
WouterJS 1:a9c933f1dc71 357 timer.reset();
thijslubberman 11:c7e27de26ac0 358 led_green = 1;
thijslubberman 11:c7e27de26ac0 359 led_blue = 1;
WouterJS 1:a9c933f1dc71 360 }
WouterJS 1:a9c933f1dc71 361 }
WouterJS 1:a9c933f1dc71 362 ///////////END MOVEMENT STATES/////////////////////////
WouterJS 1:a9c933f1dc71 363 ///////////ROBOT ARM STATES ///////////////////////////
thijslubberman 11:c7e27de26ac0 364
WouterJS 1:a9c933f1dc71 365 void do_state_failure(){
WouterJS 1:a9c933f1dc71 366
WouterJS 1:a9c933f1dc71 367 }
WouterJS 9:4f594927cff3 368
WouterJS 4:34ad002cb646 369 int count1 = 0;
WouterJS 4:34ad002cb646 370 int count2 = 0;
WouterJS 9:4f594927cff3 371
WouterJS 1:a9c933f1dc71 372 void do_state_calib_motor(){
WouterJS 1:a9c933f1dc71 373 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 374 state_changed = false;
WouterJS 1:a9c933f1dc71 375 }
thijslubberman 7:db050a878cff 376 led_red = 0;
WouterJS 1:a9c933f1dc71 377
WouterJS 1:a9c933f1dc71 378
WouterJS 4:34ad002cb646 379
WouterJS 4:34ad002cb646 380 int deriv1 = deg_m1 - count1;
WouterJS 4:34ad002cb646 381 int deriv2 = deg_m2 - count2;
WouterJS 5:892531e4e015 382
WouterJS 4:34ad002cb646 383 count1 = deg_m1;
WouterJS 4:34ad002cb646 384 count2 = deg_m2;
WouterJS 4:34ad002cb646 385
WouterJS 4:34ad002cb646 386 if ( timer.read() > 3 && deriv1 < 0.5 && deriv2 < 0.5) {
WouterJS 4:34ad002cb646 387 motor1_speed_control = 0;
WouterJS 4:34ad002cb646 388 motor2_speed_control = 0;
WouterJS 4:34ad002cb646 389 current_state = homing;
thijslubberman 7:db050a878cff 390
WouterJS 4:34ad002cb646 391 state_changed = true;
thijslubberman 7:db050a878cff 392 wait(1);
thijslubberman 7:db050a878cff 393 deg_m1 = 57.4;
thijslubberman 7:db050a878cff 394 deg_m2 = 72.93;
thijslubberman 7:db050a878cff 395 led_red = 1;
thijslubberman 7:db050a878cff 396 led_green = 0;
WouterJS 4:34ad002cb646 397 }
WouterJS 1:a9c933f1dc71 398 }
WouterJS 1:a9c933f1dc71 399
WouterJS 5:892531e4e015 400 float wu1;
WouterJS 5:892531e4e015 401 float wu2;
thijslubberman 7:db050a878cff 402
WouterJS 4:34ad002cb646 403 void do_state_homing(){
WouterJS 4:34ad002cb646 404 if (state_changed==true) {
thijslubberman 7:db050a878cff 405 timer.reset();
WouterJS 4:34ad002cb646 406 state_changed = false;
WouterJS 4:34ad002cb646 407 }
WouterJS 9:4f594927cff3 408 float kp11 = 0.1;
WouterJS 9:4f594927cff3 409 float kp12 = 0.1;
WouterJS 9:4f594927cff3 410
WouterJS 4:34ad002cb646 411 float werror1 = deg_m1 - 0;
WouterJS 4:34ad002cb646 412 float werror2 = deg_m2 - 0;
WouterJS 1:a9c933f1dc71 413
WouterJS 5:892531e4e015 414 if(werror1 > 5){
WouterJS 5:892531e4e015 415 wu1 = 1; }
WouterJS 5:892531e4e015 416 if(werror1 < -5){
WouterJS 5:892531e4e015 417 wu1 = -1; }
WouterJS 5:892531e4e015 418 else{
WouterJS 9:4f594927cff3 419 wu1 = kp11*werror1; //+ (u1 + werror1*0.002)*ki1;
WouterJS 5:892531e4e015 420 }
WouterJS 1:a9c933f1dc71 421
WouterJS 5:892531e4e015 422 if(werror2 > 5){
WouterJS 5:892531e4e015 423 wu2 = 1;}
WouterJS 5:892531e4e015 424 if(werror2 < -5){
WouterJS 5:892531e4e015 425 wu2 = -1;}
WouterJS 5:892531e4e015 426 else{
WouterJS 9:4f594927cff3 427 wu2 = kp12*werror2; //+ (u2 + werror2*0.002)*ki2);
WouterJS 5:892531e4e015 428 }
WouterJS 5:892531e4e015 429
thijslubberman 11:c7e27de26ac0 430 motor1_speed_control = fabs(wu1)/16;
WouterJS 5:892531e4e015 431
WouterJS 4:34ad002cb646 432 if(wu1 > 0){
thijslubberman 7:db050a878cff 433 motor1_direction = 1;}
WouterJS 4:34ad002cb646 434 if(wu1< 0){
thijslubberman 7:db050a878cff 435 motor1_direction = 0;}
WouterJS 4:34ad002cb646 436
thijslubberman 11:c7e27de26ac0 437 motor2_speed_control = fabs(wu2)/16;
WouterJS 4:34ad002cb646 438
WouterJS 4:34ad002cb646 439 if(wu2 > 0){
thijslubberman 7:db050a878cff 440 motor2_direction = 1;}
WouterJS 5:892531e4e015 441 if(wu2 < 0){
thijslubberman 7:db050a878cff 442 motor2_direction = 0;}
thijslubberman 7:db050a878cff 443
thijslubberman 7:db050a878cff 444 if ( timer.read() > 4) {
thijslubberman 7:db050a878cff 445 motor1_speed_control = 0;
WouterJS 9:4f594927cff3 446 motor2_speed_control = 0;
WouterJS 4:34ad002cb646 447
WouterJS 9:4f594927cff3 448 ref_q1 = 0;
WouterJS 9:4f594927cff3 449 ref_q2 = 0;
thijslubberman 7:db050a878cff 450 deg_m1 = 0;
thijslubberman 7:db050a878cff 451 deg_m2 = 0;
WouterJS 9:4f594927cff3 452 d_error1 = 0;
WouterJS 9:4f594927cff3 453 d_error2 = 0;
thijslubberman 7:db050a878cff 454 u1 = 0;
thijslubberman 7:db050a878cff 455 u2 = 0;
thijslubberman 7:db050a878cff 456 int_u1 = 0;
thijslubberman 7:db050a878cff 457 int_u2 = 0;
WouterJS 9:4f594927cff3 458 wait(1);
thijslubberman 7:db050a878cff 459 current_state = calib_emg;
thijslubberman 7:db050a878cff 460 timer.reset();
thijslubberman 7:db050a878cff 461 state_changed = true;
thijslubberman 7:db050a878cff 462 led_green = 1;
thijslubberman 7:db050a878cff 463 led_blue = 0;
thijslubberman 7:db050a878cff 464
thijslubberman 7:db050a878cff 465 }
WouterJS 4:34ad002cb646 466
WouterJS 4:34ad002cb646 467
WouterJS 1:a9c933f1dc71 468 }
WouterJS 1:a9c933f1dc71 469 void do_state_calib_emg(){
WouterJS 1:a9c933f1dc71 470 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 471 state_changed = false;
WouterJS 1:a9c933f1dc71 472 }
WouterJS 1:a9c933f1dc71 473
WouterJS 1:a9c933f1dc71 474 if(filteredsignal1 > max1){//calibrate to a new maximum
WouterJS 1:a9c933f1dc71 475 max1 = filteredsignal1;
thijslubberman 7:db050a878cff 476 threshold1 = 0.5 * max1;
WouterJS 1:a9c933f1dc71 477 }
WouterJS 1:a9c933f1dc71 478 if(filteredsignal2 > max2){//calibrate to a new maximum
WouterJS 1:a9c933f1dc71 479 max2 = filteredsignal2;
WouterJS 1:a9c933f1dc71 480 threshold2 = 0.5 * max2;
WouterJS 1:a9c933f1dc71 481 }
WouterJS 1:a9c933f1dc71 482
thijslubberman 11:c7e27de26ac0 483 if (timer.read() > 5 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 484 current_state = operational;
WouterJS 1:a9c933f1dc71 485 timer.reset();
WouterJS 1:a9c933f1dc71 486 state_changed = true;
thijslubberman 7:db050a878cff 487 led_green = 0;
thijslubberman 11:c7e27de26ac0 488 led_red = 0;
WouterJS 1:a9c933f1dc71 489 }
WouterJS 1:a9c933f1dc71 490 }
WouterJS 1:a9c933f1dc71 491
WouterJS 1:a9c933f1dc71 492 void do_state_operational(){
WouterJS 1:a9c933f1dc71 493 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 494 state_changed = false;
WouterJS 1:a9c933f1dc71 495 }
WouterJS 1:a9c933f1dc71 496
WouterJS 1:a9c933f1dc71 497 switch(movement) {// a separate function for what happens in each state
WouterJS 1:a9c933f1dc71 498 case rest:
WouterJS 1:a9c933f1dc71 499 do_wait();
WouterJS 1:a9c933f1dc71 500 break;
WouterJS 1:a9c933f1dc71 501 case forward:
WouterJS 1:a9c933f1dc71 502 do_forward();
WouterJS 1:a9c933f1dc71 503
WouterJS 1:a9c933f1dc71 504 break;
WouterJS 1:a9c933f1dc71 505 case backward:
WouterJS 1:a9c933f1dc71 506 do_backward();
WouterJS 1:a9c933f1dc71 507
WouterJS 1:a9c933f1dc71 508 break;
WouterJS 1:a9c933f1dc71 509 case up:
WouterJS 1:a9c933f1dc71 510 do_up();
WouterJS 1:a9c933f1dc71 511 break;
WouterJS 1:a9c933f1dc71 512 case down:
WouterJS 1:a9c933f1dc71 513 do_down();
WouterJS 1:a9c933f1dc71 514 break;
WouterJS 1:a9c933f1dc71 515 }
thijslubberman 11:c7e27de26ac0 516
WouterJS 1:a9c933f1dc71 517
WouterJS 1:a9c933f1dc71 518 }
WouterJS 1:a9c933f1dc71 519
WouterJS 1:a9c933f1dc71 520 void do_state_waiting(){
WouterJS 1:a9c933f1dc71 521 if (state_changed==true) {
WouterJS 1:a9c933f1dc71 522 state_changed = false;
WouterJS 1:a9c933f1dc71 523 }
WouterJS 1:a9c933f1dc71 524
WouterJS 1:a9c933f1dc71 525 if (filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
WouterJS 1:a9c933f1dc71 526 current_state = operational;
WouterJS 1:a9c933f1dc71 527 state_changed = true;
thijslubberman 11:c7e27de26ac0 528 movement = rest;
thijslubberman 11:c7e27de26ac0 529 led_green = 0;
thijslubberman 11:c7e27de26ac0 530 led_blue = 0;
thijslubberman 11:c7e27de26ac0 531 led_red = 0;
WouterJS 1:a9c933f1dc71 532 }
WouterJS 1:a9c933f1dc71 533 }
WouterJS 1:a9c933f1dc71 534 //////////////// END ROBOT ARM STATES //////////////////////////////
WouterJS 4:34ad002cb646 535
WouterJS 4:34ad002cb646 536
thijslubberman 7:db050a878cff 537 void motor_controller(){
thijslubberman 7:db050a878cff 538
WouterJS 4:34ad002cb646 539 float jacobian[4];
WouterJS 4:34ad002cb646 540 float inv_jacobian[4];
WouterJS 4:34ad002cb646 541
WouterJS 4:34ad002cb646 542 jacobian[0] = L1;
thijslubberman 10:91173ed1e841 543 jacobian[1] = (L2*cos(deg_m2/180*pi))+L1;
WouterJS 4:34ad002cb646 544 jacobian[2] = -L0;
thijslubberman 10:91173ed1e841 545 jacobian[3] = -L0 - (L2*sin(deg_m2/180*pi));
WouterJS 1:a9c933f1dc71 546
WouterJS 9:4f594927cff3 547 float det = 1/((jacobian[0]*jacobian[3])-(jacobian[1]*jacobian[2]));
WouterJS 1:a9c933f1dc71 548
WouterJS 4:34ad002cb646 549 inv_jacobian[0] = det*jacobian[3];
WouterJS 4:34ad002cb646 550 inv_jacobian[1] = -det*jacobian[1];
WouterJS 4:34ad002cb646 551 inv_jacobian[2] = -det*jacobian[2];
WouterJS 4:34ad002cb646 552 inv_jacobian[3] = det*jacobian[0];
WouterJS 4:34ad002cb646 553
WouterJS 4:34ad002cb646 554
thijslubberman 7:db050a878cff 555 ref_v1 = inv_jacobian[0]*twist[0]+inv_jacobian[1]*twist[1];
thijslubberman 7:db050a878cff 556 ref_v2 = inv_jacobian[2]*twist[0]+inv_jacobian[3]*twist[1];
WouterJS 4:34ad002cb646 557
thijslubberman 7:db050a878cff 558
WouterJS 4:34ad002cb646 559
thijslubberman 10:91173ed1e841 560 ref_q1 = ref_q1 + 0.002*ref_v1 * sign1;
thijslubberman 10:91173ed1e841 561 ref_q2 = ref_q2 + 0.002*ref_v2 * sign2;
thijslubberman 7:db050a878cff 562
thijslubberman 8:c7d21f5f87d8 563 error1 = deg_m1 - ref_q1;
thijslubberman 8:c7d21f5f87d8 564 error2 = deg_m2 - ref_q2;
WouterJS 9:4f594927cff3 565
WouterJS 9:4f594927cff3 566
thijslubberman 8:c7d21f5f87d8 567
thijslubberman 8:c7d21f5f87d8 568
thijslubberman 8:c7d21f5f87d8 569
WouterJS 1:a9c933f1dc71 570
WouterJS 9:4f594927cff3 571 // if(error1 > 5){
WouterJS 9:4f594927cff3 572 // u1 = 1; }
WouterJS 9:4f594927cff3 573 //if(error1 < -5){
WouterJS 9:4f594927cff3 574 // u1 = -1; }
WouterJS 9:4f594927cff3 575 //else{
thijslubberman 8:c7d21f5f87d8 576 d_error1 = (error1 - olderror1)/Ts;
WouterJS 9:4f594927cff3 577 filtered_d_error1 = LowpassFilter1.step(d_error1);
thijslubberman 7:db050a878cff 578 int_u1 = int_u1 + error1 * Ts;
thijslubberman 8:c7d21f5f87d8 579 u1 = kp1*error1 + int_u1*ki1 + kd1*filtered_d_error1;
thijslubberman 8:c7d21f5f87d8 580 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 581 if(u1>1){
thijslubberman 8:c7d21f5f87d8 582 u1 =1;
WouterJS 9:4f594927cff3 583 }
thijslubberman 8:c7d21f5f87d8 584 if(u1<-1){
thijslubberman 8:c7d21f5f87d8 585 u1 = -1;
thijslubberman 8:c7d21f5f87d8 586 }
thijslubberman 8:c7d21f5f87d8 587
WouterJS 9:4f594927cff3 588 //}
thijslubberman 8:c7d21f5f87d8 589 olderror1 = error1;
WouterJS 1:a9c933f1dc71 590
WouterJS 9:4f594927cff3 591 //if(error2 > 5){
WouterJS 9:4f594927cff3 592 // u2 = 1;}
WouterJS 9:4f594927cff3 593 // if(error2 < -5){
WouterJS 9:4f594927cff3 594 // u2 = -1;}
WouterJS 9:4f594927cff3 595 //else{
thijslubberman 8:c7d21f5f87d8 596 d_error2 = (error2 - olderror2)/Ts;
WouterJS 9:4f594927cff3 597 filtered_d_error2 = LowpassFilter2.step(d_error2);
thijslubberman 7:db050a878cff 598 int_u2 = int_u2 + error2 * Ts;
thijslubberman 8:c7d21f5f87d8 599 u2 = kp2*error2 + int_u2*ki2 + kd2*filtered_d_error2;
thijslubberman 8:c7d21f5f87d8 600 teraterm2 = u2;
thijslubberman 8:c7d21f5f87d8 601 if(u2>1){
thijslubberman 8:c7d21f5f87d8 602 u2 =1;
thijslubberman 8:c7d21f5f87d8 603 }
thijslubberman 8:c7d21f5f87d8 604 if(u2<-1){
thijslubberman 8:c7d21f5f87d8 605 u2 = -1;
thijslubberman 8:c7d21f5f87d8 606 }
WouterJS 9:4f594927cff3 607 // }
WouterJS 9:4f594927cff3 608 olderror2 = error2;
WouterJS 9:4f594927cff3 609
thijslubberman 7:db050a878cff 610
WouterJS 1:a9c933f1dc71 611 }
WouterJS 1:a9c933f1dc71 612
WouterJS 1:a9c933f1dc71 613 void loop_function() { //Main loop function
WouterJS 0:3710031b2621 614 measureall();
WouterJS 0:3710031b2621 615 switch(current_state) {
WouterJS 0:3710031b2621 616 case failure:
WouterJS 0:3710031b2621 617 do_state_failure(); // a separate function for what happens in each state
WouterJS 0:3710031b2621 618 break;
WouterJS 1:a9c933f1dc71 619 case calib_motor:
WouterJS 1:a9c933f1dc71 620 do_state_calib_motor();
WouterJS 1:a9c933f1dc71 621 break ;
WouterJS 4:34ad002cb646 622 case homing:
WouterJS 4:34ad002cb646 623 do_state_homing();
WouterJS 4:34ad002cb646 624 break;
WouterJS 0:3710031b2621 625 case calib_emg:
WouterJS 0:3710031b2621 626 do_state_calib_emg();
WouterJS 0:3710031b2621 627 break;
WouterJS 0:3710031b2621 628 case operational:
WouterJS 0:3710031b2621 629 do_state_operational();
WouterJS 0:3710031b2621 630 break;
WouterJS 1:a9c933f1dc71 631 case waiting:
WouterJS 1:a9c933f1dc71 632 do_state_waiting();
WouterJS 0:3710031b2621 633 break;
WouterJS 0:3710031b2621 634 }
WouterJS 0:3710031b2621 635 motor_controller();
WouterJS 1:a9c933f1dc71 636 // Outputs data to the computer
thijslubberman 7:db050a878cff 637
WouterJS 0:3710031b2621 638 }
WouterJS 0:3710031b2621 639
WouterJS 0:3710031b2621 640
WouterJS 0:3710031b2621 641 int main()
WouterJS 1:a9c933f1dc71 642 {
WouterJS 4:34ad002cb646 643 // motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
WouterJS 3:055eb9f256fc 644
WouterJS 5:892531e4e015 645 motor1_direction = 0; //set motor 1 to run clockwisedirection for calibration
thijslubberman 7:db050a878cff 646 motor1_speed_control = 0.15;//to make sure the motor will not run at too high velocity
WouterJS 5:892531e4e015 647 motor2_direction = 0; // set motor 2 to run clockwise direction
thijslubberman 7:db050a878cff 648 motor2_speed_control = 0.15;
WouterJS 4:34ad002cb646 649
thijslubberman 7:db050a878cff 650 led_red = 1;
thijslubberman 7:db050a878cff 651 led_green = 1;
thijslubberman 7:db050a878cff 652 led_blue = 1;
WouterJS 3:055eb9f256fc 653 timer.start();
WouterJS 3:055eb9f256fc 654 loop_timer.attach(&loop_function, Ts);
WouterJS 3:055eb9f256fc 655 sample_timer.attach(&scopedata, Ts);
WouterJS 3:055eb9f256fc 656 sample_timer2.attach(&filterall, Ts);
WouterJS 9:4f594927cff3 657
WouterJS 9:4f594927cff3 658 pc.baud(115200);
WouterJS 2:fa90eaa14f99 659 while (true) {
thijslubberman 11:c7e27de26ac0 660 printf("%i %i %f %f %f %f \n\r",movement,current_state,error1,error2,deg_m1, deg_m2);
WouterJS 0:3710031b2621 661 }
WouterJS 0:3710031b2621 662 }