Thijs Lubberman / Mbed 2 deprecated MotorArchitecture2

motorarmpje

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture by Wouter Schuttert

main.cpp@7:db050a878cff, 2018-10-30 (annotated)

Committer:
thijslubberman
Date:
Tue Oct 30 14:15:57 2018 +0000
Revision:
7:db050a878cff
Parent:
6:b526cf83fd4f
Child:
8:c7d21f5f87d8
robotarm

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