Thijs Lubberman / Mbed 2 deprecated MotorArchitecture2

motorarmpje

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture by Wouter Schuttert

main.cpp@8:c7d21f5f87d8, 2018-10-31 (annotated)

Committer:
thijslubberman
Date:
Wed Oct 31 09:47:09 2018 +0000
Revision:
8:c7d21f5f87d8
Parent:
7:db050a878cff 
31-10 morning

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