Wouter Bos / Mbed 2 deprecated group29_x_y_control_dynamic_x-lims

presentatie versie met potmeters enabled

Dependencies:   Encoder HIDScope mbed

main.cpp@9:831891bb0d11, 2015-11-02 (annotated)

Committer:
Zeekat
Date:
Mon Nov 02 11:55:24 2015 +0000
Revision:
9:831891bb0d11
Parent:
8:f2f45be5a060
Child:
10:93a76bd81eef
presentation version with potmeters enabled

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zeekat 0:b0e2b38ab272 1 #include "mbed.h"
Zeekat 0:b0e2b38ab272 2 #include "encoder.h"
Zeekat 0:b0e2b38ab272 3 #include "HIDScope.h"
Zeekat 0:b0e2b38ab272 4 #include "math.h"
Zeekat 0:b0e2b38ab272 5
Zeekat 6:30afff8ae34a 6 HIDScope scope(4); // defines the amount of channels of the scope
Zeekat 0:b0e2b38ab272 7
Zeekat 0:b0e2b38ab272 8 // Define Tickers and control frequencies
Zeekat 5:ad4ae6b65474 9 Ticker controller1, controller2, main_filter, cartesian;
Zeekat 0:b0e2b38ab272 10 // Go flag variables belonging to Tickers
Zeekat 0:b0e2b38ab272 11 volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false;
Zeekat 0:b0e2b38ab272 12
Zeekat 0:b0e2b38ab272 13 // Frequency control
Zeekat 5:ad4ae6b65474 14 double controlfreq = 200 ; // controlloops frequentie (Hz)
Zeekat 0:b0e2b38ab272 15 double controlstep = 1/controlfreq; // timestep derived from controlfreq
Zeekat 0:b0e2b38ab272 16
Zeekat 0:b0e2b38ab272 17 double EMG_freq = 200;
Zeekat 0:b0e2b38ab272 18 double EMG_step = 1/EMG_freq;
Zeekat 0:b0e2b38ab272 19
Zeekat 0:b0e2b38ab272 20 //////////////////////// IN-OUTPUT /////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 21 //MOTOR OUTPUTPINS
Zeekat 6:30afff8ae34a 22 PwmOut motor1_aan(D6), motor2_aan(D5); // PWM signaal motor 2
Zeekat 6:30afff8ae34a 23 DigitalOut motor1_rich(D7), motor2_rich(D4); // digitaal signaal for direction
Zeekat 0:b0e2b38ab272 24
Zeekat 0:b0e2b38ab272 25 // ENCODER INPUTPINS
Zeekat 6:30afff8ae34a 26 Encoder motor1_enc(D12,D11), motor2_enc(D10,D9); // encoderinputpins
Zeekat 0:b0e2b38ab272 27
Zeekat 0:b0e2b38ab272 28 // EXTRA INPUTS AND REQUIRED VARIABLES
Zeekat 0:b0e2b38ab272 29 //POTMETERS (used for debugging purposes, not reliable anymore)
Zeekat 0:b0e2b38ab272 30 AnalogIn potright(A4); // define the potmeter outputpins
Zeekat 0:b0e2b38ab272 31 AnalogIn potleft(A5);
Zeekat 0:b0e2b38ab272 32
Zeekat 0:b0e2b38ab272 33 // Analoge input signalen defineren
Zeekat 6:30afff8ae34a 34 AnalogIn EMG_in(A0);
Zeekat 6:30afff8ae34a 35 AnalogIn EMG_int(A2);
Zeekat 0:b0e2b38ab272 36
Zeekat 0:b0e2b38ab272 37 // BUTTONS
Zeekat 0:b0e2b38ab272 38 // control flow
Zeekat 6:30afff8ae34a 39 DigitalIn buttonlinks(PTA4); // button for starting the motor controller
Zeekat 6:30afff8ae34a 40 DigitalIn buttonrechts(PTC6); // button for startin calibration procedures
Zeekat 6:30afff8ae34a 41 DigitalIn reset_button(D1); // button for returning the arm to the start position
Zeekat 8:f2f45be5a060 42 DigitalIn switch_xy_button(D0); // button for starting a preprogrammed movement. (pick up cup)
Zeekat 0:b0e2b38ab272 43 // init values
Zeekat 0:b0e2b38ab272 44 bool loop_start = false;
Zeekat 0:b0e2b38ab272 45 bool calib_start = false;
Zeekat 0:b0e2b38ab272 46 bool reset = false;
Zeekat 9:831891bb0d11 47 bool program = false;
Zeekat 9:831891bb0d11 48
Zeekat 0:b0e2b38ab272 49
Zeekat 0:b0e2b38ab272 50 // LED outputs on bioshield
Zeekat 0:b0e2b38ab272 51 DigitalOut led_right(D2);
Zeekat 0:b0e2b38ab272 52 // LED outputs on dev-board
Zeekat 0:b0e2b38ab272 53 DigitalOut ledred(LED1);
Zeekat 0:b0e2b38ab272 54 DigitalOut ledgreen(LED2);
Zeekat 0:b0e2b38ab272 55 DigitalOut ledblue(LED3);
Zeekat 0:b0e2b38ab272 56
Zeekat 0:b0e2b38ab272 57
Zeekat 0:b0e2b38ab272 58 //////////////////////////////////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 59 ////////////////////////// GLOBAL VARIABLES ///////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 60 ////////////////////////////////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 61
Zeekat 6:30afff8ae34a 62 // switch axes
Zeekat 8:f2f45be5a060 63 // bool switch_xy = false; // bool for switching axes
Zeekat 5:ad4ae6b65474 64 double sw1 = 0; // counter for switching axes
Zeekat 9:831891bb0d11 65 double t_switch = 0.8; // seconds for switching
Zeekat 9:831891bb0d11 66 bool switch_xy = false;
Zeekat 6:30afff8ae34a 67
Zeekat 0:b0e2b38ab272 68 // physical constants
Zeekat 0:b0e2b38ab272 69 const double L = 36; // lenght of arms
Zeekat 0:b0e2b38ab272 70 const double pi = 3.1415926535897; // pi
Zeekat 0:b0e2b38ab272 71
Zeekat 0:b0e2b38ab272 72 // angle limitations (in radians)
Zeekat 0:b0e2b38ab272 73 // motor1
Zeekat 0:b0e2b38ab272 74 const double limlow1 = 0.5; // min height in rad
Zeekat 0:b0e2b38ab272 75 const double limhigh1 = 6.3; // max height in rad
Zeekat 0:b0e2b38ab272 76 // motor 2
Zeekat 0:b0e2b38ab272 77 const double limlow2 = -4.0; // maximum height, motor has been inverted due to transmission
Zeekat 0:b0e2b38ab272 78 const double limhigh2 = 2.5; // minimum height in rad
Zeekat 0:b0e2b38ab272 79
Zeekat 0:b0e2b38ab272 80 // offset angle (radians needed to change the arms to horizontal position from reset position)
Zeekat 0:b0e2b38ab272 81 const double phi_one_offset = 2.8;
Zeekat 0:b0e2b38ab272 82 const double phi_two_offset = 1.85;
Zeekat 0:b0e2b38ab272 83
Zeekat 0:b0e2b38ab272 84 ///////////////////////////
Zeekat 0:b0e2b38ab272 85 // Main control loop transfer variables
Zeekat 0:b0e2b38ab272 86 // here all variables that transfer bewtween the primary control functions
Zeekat 0:b0e2b38ab272 87
Zeekat 0:b0e2b38ab272 88 // filter to calibration
Zeekat 0:b0e2b38ab272 89 double output1;
Zeekat 0:b0e2b38ab272 90 double output2;
Zeekat 0:b0e2b38ab272 91 // filter to x-y
Zeekat 0:b0e2b38ab272 92 double output1_amp;
Zeekat 0:b0e2b38ab272 93 double output2_amp;
Zeekat 0:b0e2b38ab272 94 // x-y to motor control
Zeekat 0:b0e2b38ab272 95 double phi_one;
Zeekat 0:b0e2b38ab272 96 double phi_two;
Zeekat 0:b0e2b38ab272 97
Zeekat 0:b0e2b38ab272 98 ////////////////////////////
Zeekat 0:b0e2b38ab272 99 // NOTE: start position is [x,y] = [60,0], reset position is [phi1,phi2] = [0,0]
Zeekat 0:b0e2b38ab272 100 // define smooth variables (is to create a slowed movement instead of going to ref value inmediately)
Zeekat 0:b0e2b38ab272 101 double start_up_time = 3;
Zeekat 0:b0e2b38ab272 102 double start_loops = start_up_time*controlfreq;
Zeekat 0:b0e2b38ab272 103 int rc1 = 0; // counters in function to enable relatively smooth movement
Zeekat 0:b0e2b38ab272 104 int rc2 = 0;
Zeekat 0:b0e2b38ab272 105
Zeekat 0:b0e2b38ab272 106 // define return to start variables
Zeekat 0:b0e2b38ab272 107 double reset_phi_one = 0;
Zeekat 0:b0e2b38ab272 108 double reset_phi_two = 0;
Zeekat 0:b0e2b38ab272 109
Zeekat 0:b0e2b38ab272 110 // storage variables to store the location at the beginning of the smoothed movement (0 on startup)
Zeekat 0:b0e2b38ab272 111 double phi_one_curr = 0;
Zeekat 0:b0e2b38ab272 112 double phi_two_curr = 0;
Zeekat 0:b0e2b38ab272 113
Zeekat 0:b0e2b38ab272 114 // REFERENCE SETTINGS
Zeekat 9:831891bb0d11 115 double input_threshold = 0.5; // the minimum value the signal must have in order to change the reference.
Zeekat 0:b0e2b38ab272 116 // Define storage variables for reference values (also start position)
Zeekat 0:b0e2b38ab272 117 double c_reference_x = 60, c_reference_y = 0;
Zeekat 0:b0e2b38ab272 118 // x-settings (no y-settings because these are calculated from the current x-position)
Zeekat 1:4c9994ac229c 119 double x_min = 47, x_max = 70, y_min_max = -32;
Zeekat 0:b0e2b38ab272 120 double xx,yy,y_min,y_max;
Zeekat 0:b0e2b38ab272 121 // Define the maximum rate of change for the x and y reference signals (velocity)
Zeekat 8:f2f45be5a060 122 double Vmax_x = 10, Vmax_y = 15; // [cm/s]
Zeekat 0:b0e2b38ab272 123
Zeekat 0:b0e2b38ab272 124
Zeekat 0:b0e2b38ab272 125 // CONTROLLER SETTINGS
Zeekat 0:b0e2b38ab272 126 // motor 1
Zeekat 0:b0e2b38ab272 127 const double m1_Kp = 5; // Proportional constant
Zeekat 0:b0e2b38ab272 128 const double m1_Ki = 0.5; // integration constant
Zeekat 0:b0e2b38ab272 129 const double m1_Kd = 0.4; // differentiation constant
Zeekat 0:b0e2b38ab272 130 // motor 2
Zeekat 0:b0e2b38ab272 131 const double m2_Kp = 3;
Zeekat 0:b0e2b38ab272 132 const double m2_Ki = 0.5;
Zeekat 6:30afff8ae34a 133 const double m2_Kd = 0.4;
Zeekat 0:b0e2b38ab272 134 // storage variables. these variables are used to store data between controller iterations
Zeekat 0:b0e2b38ab272 135 // motor 1
Zeekat 0:b0e2b38ab272 136 double m1_err_int = 0;
Zeekat 0:b0e2b38ab272 137 double m1_prev_err = 0;
Zeekat 0:b0e2b38ab272 138 // motor 2
Zeekat 0:b0e2b38ab272 139 double m2_err_int = 0;
Zeekat 0:b0e2b38ab272 140 double m2_prev_err = 0;
Zeekat 0:b0e2b38ab272 141
Zeekat 6:30afff8ae34a 142 // EMG calibration variables (standard is 7)
Zeekat 0:b0e2b38ab272 143 double emg_gain1 = 7;
Zeekat 0:b0e2b38ab272 144 double emg_gain2 = 7;
Zeekat 0:b0e2b38ab272 145
Zeekat 0:b0e2b38ab272 146 double cal_time = 2.5; // amount of time calibration should take (seconds)
Zeekat 5:ad4ae6b65474 147 double cal_samples = EMG_freq*cal_time; // amount of samples that is used (dependent on the frequency of the filter)
Zeekat 0:b0e2b38ab272 148 double normalize_emg_value = 1; // set the desired value to calibrate the signal to (eg max signal = 1)
Zeekat 0:b0e2b38ab272 149
Zeekat 0:b0e2b38ab272 150 // FILTER VARIABLES
Zeekat 0:b0e2b38ab272 151 // storage variables
Zeekat 6:30afff8ae34a 152 // differential action filter, same is used for PID controllers
Zeekat 0:b0e2b38ab272 153 double m_f_v1 = 0, m_f_v2 = 0;
Zeekat 0:b0e2b38ab272 154 // input filter to smooth angle reference signals
Zeekat 0:b0e2b38ab272 155 double r1_f_v1 = 0, r1_f_v2 = 0, r2_f_v1 = 0, r2_f_v2 = 0;
Zeekat 0:b0e2b38ab272 156 // Define the storage variables and filter coeficients for eight filters
Zeekat 0:b0e2b38ab272 157 // EMG filter 1
Zeekat 0:b0e2b38ab272 158 double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0;
Zeekat 0:b0e2b38ab272 159 // EMG filter 2
Zeekat 0:b0e2b38ab272 160 double f1_v1t = 0, f1_v2t = 0, f2_v1t = 0, f2_v2t = 0, f3_v1t = 0, f3_v2t = 0,f4_v1t = 0, f4_v2t = 0;
Zeekat 0:b0e2b38ab272 161
Zeekat 0:b0e2b38ab272 162 // Filter coefficients
Zeekat 8:f2f45be5a060 163 // differential action filter (lowpass 5Hz at 200Hz)
Zeekat 0:b0e2b38ab272 164 const double m_f_a1 = -1.1430, m_f_a2 = 0.4128, m_f_b0 = 0.0675, m_f_b1 = 0.1349, m_f_b2 = 0.0675;
Zeekat 8:f2f45be5a060 165 // input filter (lowpass 1Hz at 200Hz) (used to make the angle signals smooth)
Zeekat 0:b0e2b38ab272 166 const double r1_f_a1 = -1.6475, r1_f_a2 = 0.7009, r1_f_b0 = 0.0134, r1_f_b1 = 0.0267, r1_f_b2 = 0.0134;
Zeekat 0:b0e2b38ab272 167
Zeekat 0:b0e2b38ab272 168 // EMG-Filter (calculated for 200)
Zeekat 0:b0e2b38ab272 169
Zeekat 0:b0e2b38ab272 170 const double numnotch50biq1_1 = 1;
Zeekat 0:b0e2b38ab272 171 const double numnotch50biq1_2 = 1.11568254634875e-11;
Zeekat 0:b0e2b38ab272 172 const double numnotch50biq1_3 = 1.00000002278002;
Zeekat 0:b0e2b38ab272 173 const double dennotch50biq1_2 = -0.0434777721916752;
Zeekat 0:b0e2b38ab272 174 const double dennotch50biq1_3 = 0.956543692050407;
Zeekat 0:b0e2b38ab272 175 // biquad 2 coefficienten
Zeekat 0:b0e2b38ab272 176 const double numnotch50biq2_1 = 1;
Zeekat 0:b0e2b38ab272 177 const double numnotch50biq2_2 = -1.11571030192437e-11;
Zeekat 0:b0e2b38ab272 178 const double numnotch50biq2_3 = 0.999999977219980;
Zeekat 0:b0e2b38ab272 179 const double dennotch50biq2_2 = 0.0434777721916751;
Zeekat 0:b0e2b38ab272 180 const double dennotch50biq2_3 = 0.956543692050417;
Zeekat 0:b0e2b38ab272 181
Zeekat 0:b0e2b38ab272 182 // highpass filter 20 Hz coefficienten
Zeekat 0:b0e2b38ab272 183 const double numhigh20_1 = 0.638945525159022;
Zeekat 0:b0e2b38ab272 184 const double numhigh20_2 = -1.27789105031804;
Zeekat 0:b0e2b38ab272 185 const double numhigh20_3 = 0.638945525159022;
Zeekat 0:b0e2b38ab272 186 const double denhigh20_2 = -1.14298050253990;
Zeekat 0:b0e2b38ab272 187 const double denhigh20_3 = 0.412801598096189;
Zeekat 0:b0e2b38ab272 188
Zeekat 0:b0e2b38ab272 189 // lowpass 5 Hz coefficienten
Zeekat 0:b0e2b38ab272 190 const double numlow5_1 =0.000241359049041961;
Zeekat 0:b0e2b38ab272 191 const double numlow5_2 =0.000482718098083923;
Zeekat 0:b0e2b38ab272 192 const double numlow5_3 =0.000241359049041961;
Zeekat 0:b0e2b38ab272 193 const double denlow5_2 =-1.95557824031504;
Zeekat 2:85ab9173d947 194 const double denlow5_3 =0.956543676511203;
Zeekat 2:85ab9173d947 195
Zeekat 2:85ab9173d947 196
Zeekat 2:85ab9173d947 197 // programmed movements
Zeekat 2:85ab9173d947 198 // counter
Zeekat 2:85ab9173d947 199 double mt1 = 0;
Zeekat 2:85ab9173d947 200 // time
Zeekat 5:ad4ae6b65474 201 double T1 = 4, T2 = T1 + 2, T3 = T2 + 6, T4 = T3 + 1, T5 = T4 + 6, T6 = T5 + 4, T7 = T6 + 4, T8 = T7 + 1;
Zeekat 2:85ab9173d947 202
Zeekat 3:ebd94237935e 203 // pak beker
Zeekat 3:ebd94237935e 204 double x1 = 50, y1 = -37;
Zeekat 3:ebd94237935e 205 double x2 = 60, y2 = -37;
Zeekat 3:ebd94237935e 206 double x3 = 60, y3 = 30;
Zeekat 5:ad4ae6b65474 207 double x4 = 60, y4 = 30;
Zeekat 5:ad4ae6b65474 208 double x5 = 60, y5 = -37;
Zeekat 5:ad4ae6b65474 209 double x6 = 50, y6 = -37 ;
Zeekat 5:ad4ae6b65474 210 double x7 = 50, y7 = 0;
Zeekat 5:ad4ae6b65474 211 double x8 = 50, y8 = 0;
Zeekat 0:b0e2b38ab272 212
Zeekat 0:b0e2b38ab272 213 ////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 214 /////////////////// START OF SIDE FUNCTIONS ////////////////////
Zeekat 0:b0e2b38ab272 215 //////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 216 // these functions are tailored to perform 1 specific function
Zeekat 0:b0e2b38ab272 217
Zeekat 0:b0e2b38ab272 218 // this funtion flips leds on and off accordin to input with 0 being on
Zeekat 0:b0e2b38ab272 219 void LED(int red,int green,int blue)
Zeekat 0:b0e2b38ab272 220 {
Zeekat 0:b0e2b38ab272 221 ledred.write(red);
Zeekat 0:b0e2b38ab272 222 ledgreen.write(green);
Zeekat 0:b0e2b38ab272 223 ledblue.write(blue);
Zeekat 0:b0e2b38ab272 224 }
Zeekat 0:b0e2b38ab272 225
Zeekat 0:b0e2b38ab272 226 // counts 2 radians
Zeekat 0:b0e2b38ab272 227 // this function takes counts from the encoder and converts it to the amount of radians from the zero position.
Zeekat 0:b0e2b38ab272 228 // It has been set up for standard 2X DECODING!!!
Zeekat 0:b0e2b38ab272 229 double get_radians(double counts)
Zeekat 0:b0e2b38ab272 230 {
Zeekat 0:b0e2b38ab272 231 double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, last warning)
Zeekat 0:b0e2b38ab272 232 return radians;
Zeekat 0:b0e2b38ab272 233 }
Zeekat 0:b0e2b38ab272 234
Zeekat 0:b0e2b38ab272 235
Zeekat 0:b0e2b38ab272 236 // This functions takes a 0->1 input, uses passing by reference (&c_reference)
Zeekat 0:b0e2b38ab272 237 // to create a reference that moves with a variable speed. It is meant for -1->1 values
Zeekat 0:b0e2b38ab272 238 double reference_f(double input, double &c_reference, double limlow, double limhigh, double Vmax)
Zeekat 0:b0e2b38ab272 239 {
Zeekat 0:b0e2b38ab272 240 double reference = c_reference + input * controlstep * Vmax ;
Zeekat 0:b0e2b38ab272 241 // two if statements check if the reference exceeds the limits placed upon the arms
Zeekat 0:b0e2b38ab272 242 if(reference < limlow){reference = limlow;}
Zeekat 0:b0e2b38ab272 243 if(reference > limhigh){reference = limhigh;}
Zeekat 0:b0e2b38ab272 244 c_reference = reference; // change the global variable to the latest location.
Zeekat 0:b0e2b38ab272 245 return reference;
Zeekat 0:b0e2b38ab272 246 }
Zeekat 0:b0e2b38ab272 247
Zeekat 0:b0e2b38ab272 248
Zeekat 0:b0e2b38ab272 249 // This function takes the controller outputvalue and ensures it is between -1 and 1
Zeekat 0:b0e2b38ab272 250 // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
Zeekat 0:b0e2b38ab272 251 double outputlimiter (double output, double limit)
Zeekat 0:b0e2b38ab272 252 {
Zeekat 0:b0e2b38ab272 253 if(output> limit)
Zeekat 0:b0e2b38ab272 254 {
Zeekat 0:b0e2b38ab272 255 output = 1;
Zeekat 0:b0e2b38ab272 256 }
Zeekat 0:b0e2b38ab272 257 else if(output < limit && output > 0)
Zeekat 0:b0e2b38ab272 258 {
Zeekat 0:b0e2b38ab272 259 output = output;
Zeekat 0:b0e2b38ab272 260 }
Zeekat 0:b0e2b38ab272 261 else if(output > -limit && output < 0)
Zeekat 0:b0e2b38ab272 262 {
Zeekat 0:b0e2b38ab272 263 output = output;
Zeekat 0:b0e2b38ab272 264 }
Zeekat 0:b0e2b38ab272 265 else if(output < -limit)
Zeekat 0:b0e2b38ab272 266 {
Zeekat 0:b0e2b38ab272 267 (output = -1);
Zeekat 0:b0e2b38ab272 268 }
Zeekat 0:b0e2b38ab272 269 return output;
Zeekat 0:b0e2b38ab272 270 }
Zeekat 0:b0e2b38ab272 271
Zeekat 0:b0e2b38ab272 272
Zeekat 0:b0e2b38ab272 273 // BIQUADFILTER CODE GIVEN IN SHEETS (input format: den, den, nom, nom, nom)
Zeekat 0:b0e2b38ab272 274 double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
Zeekat 0:b0e2b38ab272 275 {
Zeekat 0:b0e2b38ab272 276 double v = u - a1*v1 - a2*v2;
Zeekat 0:b0e2b38ab272 277 double y = b0*v + b1*v1 + b2*v2;
Zeekat 0:b0e2b38ab272 278 v2 = v1;
Zeekat 0:b0e2b38ab272 279 v1 = v;
Zeekat 0:b0e2b38ab272 280 return y;
Zeekat 0:b0e2b38ab272 281 }
Zeekat 0:b0e2b38ab272 282
Zeekat 0:b0e2b38ab272 283 // PID Controller given in sheets
Zeekat 0:b0e2b38ab272 284 // adapted to use the same differential filter, and to split the different terms
Zeekat 0:b0e2b38ab272 285 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
Zeekat 0:b0e2b38ab272 286 {
Zeekat 0:b0e2b38ab272 287 // Proportional
Zeekat 0:b0e2b38ab272 288 double P = Kp * e;
Zeekat 0:b0e2b38ab272 289 // Integral
Zeekat 0:b0e2b38ab272 290 e_int = e_int + Ts * e;
Zeekat 0:b0e2b38ab272 291 double I = e_int * Ki;
Zeekat 0:b0e2b38ab272 292 // Derivative
Zeekat 0:b0e2b38ab272 293 double e_derr = (e - e_prev)/Ts;
Zeekat 0:b0e2b38ab272 294 e_derr = biquadfilter(e_derr, m_f_v1, m_f_v2, m_f_a1, m_f_a2, m_f_b0, m_f_b1, m_f_b2);
Zeekat 0:b0e2b38ab272 295 //
Zeekat 0:b0e2b38ab272 296 e_prev = e;
Zeekat 0:b0e2b38ab272 297 double D = Kd* e_derr;
Zeekat 0:b0e2b38ab272 298 // PID
Zeekat 0:b0e2b38ab272 299 double output = P + I + D;
Zeekat 0:b0e2b38ab272 300 return output;
Zeekat 0:b0e2b38ab272 301 }
Zeekat 0:b0e2b38ab272 302
Zeekat 0:b0e2b38ab272 303
Zeekat 0:b0e2b38ab272 304 // function that limits the angles that can be used in the motor reference signal
Zeekat 0:b0e2b38ab272 305 double angle_limits(double phi, double limlow, double limhigh)
Zeekat 0:b0e2b38ab272 306 {
Zeekat 5:ad4ae6b65474 307 if(phi < limlow) // determine if the reference is lower than the minimum angle
Zeekat 0:b0e2b38ab272 308 {
Zeekat 5:ad4ae6b65474 309 phi = limlow; // if lower, set the lower limit as reference.
Zeekat 0:b0e2b38ab272 310 }
Zeekat 5:ad4ae6b65474 311 if(phi > limhigh) // determine if the reference is higher than the maximum angle
Zeekat 0:b0e2b38ab272 312 {
Zeekat 0:b0e2b38ab272 313 phi = limhigh;
Zeekat 0:b0e2b38ab272 314 }
Zeekat 0:b0e2b38ab272 315 return phi;
Zeekat 0:b0e2b38ab272 316 }
Zeekat 0:b0e2b38ab272 317
Zeekat 0:b0e2b38ab272 318 // this function adapts the filtered emg signal for use in the reference generation
Zeekat 0:b0e2b38ab272 319 // adds threshold value and normalizes between 0 and 1
Zeekat 0:b0e2b38ab272 320 double adapt_signal(double input)
Zeekat 0:b0e2b38ab272 321 {
Zeekat 0:b0e2b38ab272 322 // add threshold value for outputs
Zeekat 5:ad4ae6b65474 323 if(input < input_threshold){input = 0;} // set the input as zero if the signal is lower than the threshold
Zeekat 0:b0e2b38ab272 324
Zeekat 0:b0e2b38ab272 325 // return the input to a value between 0 and 1 (otherwise you will get jumps in input)
Zeekat 0:b0e2b38ab272 326 input = (input-input_threshold) * (1/(1-input_threshold));
Zeekat 0:b0e2b38ab272 327
Zeekat 0:b0e2b38ab272 328 // if below 0 = 0 (otherwise values like -input_threshold start popping up)
Zeekat 0:b0e2b38ab272 329 if(input < 0){input = 0;}
Zeekat 0:b0e2b38ab272 330
Zeekat 0:b0e2b38ab272 331 // limit signal maximum to 1
Zeekat 0:b0e2b38ab272 332 if(input > 1){input = 1;}
Zeekat 0:b0e2b38ab272 333 return input;
Zeekat 0:b0e2b38ab272 334 }
Zeekat 0:b0e2b38ab272 335
Zeekat 7:acf28eb906c4 336 // funtion switches the direction that is controlled
Zeekat 5:ad4ae6b65474 337 void switch_axes (double input1,double input2)
Zeekat 0:b0e2b38ab272 338 {
Zeekat 5:ad4ae6b65474 339 if(input1 > input_threshold && input2 > input_threshold) // when both signals are above the threshold, add one to global counter
Zeekat 5:ad4ae6b65474 340 {
Zeekat 5:ad4ae6b65474 341 sw1++;
Zeekat 5:ad4ae6b65474 342 }
Zeekat 7:acf28eb906c4 343 if(sw1 == t_switch*controlfreq) // if global counter > t*freq flip the bool.
Zeekat 5:ad4ae6b65474 344 {
Zeekat 5:ad4ae6b65474 345 switch_xy = !switch_xy;
Zeekat 5:ad4ae6b65474 346 led_right.write(!led_right.read()); // turn on led when switched
Zeekat 5:ad4ae6b65474 347 sw1 = 0;
Zeekat 5:ad4ae6b65474 348 }
Zeekat 5:ad4ae6b65474 349 if(input1 < input_threshold || input2 < input_threshold) // if one becomes lower than the threshold, set the global variable to zero
Zeekat 5:ad4ae6b65474 350 {
Zeekat 5:ad4ae6b65474 351 sw1 = 0;
Zeekat 5:ad4ae6b65474 352 }
Zeekat 0:b0e2b38ab272 353 }
Zeekat 2:85ab9173d947 354
Zeekat 5:ad4ae6b65474 355 // this function allows the xx and yy variables to follow a figure determined by a set of coordinates
Zeekat 5:ad4ae6b65474 356 // this function is very simple, can be (possibly) improved by writing a single loop.
Zeekat 2:85ab9173d947 357 void square_move()
Zeekat 2:85ab9173d947 358 {
Zeekat 8:f2f45be5a060 359 if (mt1 > 0 && mt1 < T1*controlfreq)
Zeekat 2:85ab9173d947 360 {
Zeekat 5:ad4ae6b65474 361 xx = x8 + (x1-x8)*(mt1/(T1*controlfreq));
Zeekat 5:ad4ae6b65474 362 yy = y8 + (y1-y8)*(mt1/(T1*controlfreq));
Zeekat 2:85ab9173d947 363 }
Zeekat 5:ad4ae6b65474 364 else if (mt1 >= T1*controlfreq && mt1 < T2*controlfreq)
Zeekat 2:85ab9173d947 365 {
Zeekat 3:ebd94237935e 366 xx = x1 + (x2-x1)*(mt1-T1*controlfreq)/(T2*controlfreq-T1*controlfreq) ;
Zeekat 2:85ab9173d947 367 yy = y1 + (y2-y1)*(mt1-T1*controlfreq)/(T2*controlfreq-T1*controlfreq) ;
Zeekat 2:85ab9173d947 368 }
Zeekat 5:ad4ae6b65474 369 else if (mt1 >= T2*controlfreq && mt1 < T3*controlfreq)
Zeekat 2:85ab9173d947 370 {
Zeekat 2:85ab9173d947 371 xx = x2 + (x3-x2)*(mt1-T2*controlfreq)/(T3*controlfreq-T2*controlfreq) ;
Zeekat 3:ebd94237935e 372 yy = y2 + (y3-y2)*(mt1-T2*controlfreq)/(T3*controlfreq-T2*controlfreq);
Zeekat 2:85ab9173d947 373 }
Zeekat 5:ad4ae6b65474 374 else if (mt1 >= T3*controlfreq && mt1 < T4*controlfreq)
Zeekat 2:85ab9173d947 375 {
Zeekat 3:ebd94237935e 376 xx = x3 + (x4-x3)*(mt1-T3*controlfreq)/(T4*controlfreq-T3*controlfreq) ;
Zeekat 2:85ab9173d947 377 yy = y3 + (y4-y3)*(mt1-T3*controlfreq)/(T4*controlfreq-T3*controlfreq) ;
Zeekat 2:85ab9173d947 378 }
Zeekat 5:ad4ae6b65474 379 else if (mt1 >= T4*controlfreq && mt1 < T5*controlfreq)
Zeekat 4:9684b6f8b63c 380 {
Zeekat 4:9684b6f8b63c 381 xx = x4 + (x5-x4)*(mt1-T4*controlfreq)/(T5*controlfreq-T4*controlfreq) ;
Zeekat 4:9684b6f8b63c 382 yy = y4 + (y5-y4)*(mt1-T4*controlfreq)/(T5*controlfreq-T4*controlfreq) ;
Zeekat 4:9684b6f8b63c 383 }
Zeekat 5:ad4ae6b65474 384 else if (mt1 >= T5*controlfreq && mt1 < T6*controlfreq)
Zeekat 4:9684b6f8b63c 385 {
Zeekat 4:9684b6f8b63c 386 xx = x5 + (x6-x5)*(mt1-T5*controlfreq)/(T6*controlfreq-T5*controlfreq) ;
Zeekat 4:9684b6f8b63c 387 yy = y5 + (y6-y5)*(mt1-T5*controlfreq)/(T6*controlfreq-T5*controlfreq) ;
Zeekat 4:9684b6f8b63c 388 }
Zeekat 5:ad4ae6b65474 389 else if (mt1 >= T6*controlfreq && mt1 < T7*controlfreq)
Zeekat 5:ad4ae6b65474 390 {
Zeekat 5:ad4ae6b65474 391 xx = x6 + (x7-x6)*(mt1-T6*controlfreq)/(T7*controlfreq-T6*controlfreq) ;
Zeekat 5:ad4ae6b65474 392 yy = y6 + (y7-y6)*(mt1-T6*controlfreq)/(T7*controlfreq-T6*controlfreq) ;
Zeekat 5:ad4ae6b65474 393 }
Zeekat 5:ad4ae6b65474 394 else if (mt1 >= T7*controlfreq && mt1 < T8*controlfreq)
Zeekat 5:ad4ae6b65474 395 {
Zeekat 5:ad4ae6b65474 396 xx = x7 + (x8-x7)*(mt1-T7*controlfreq)/(T8*controlfreq-T7*controlfreq) ;
Zeekat 5:ad4ae6b65474 397 yy = y7 + (y8-y7)*(mt1-T7*controlfreq)/(T8*controlfreq-T7*controlfreq) ;
Zeekat 5:ad4ae6b65474 398 }
Zeekat 5:ad4ae6b65474 399 else if (mt1 >= T8*controlfreq)
Zeekat 2:85ab9173d947 400 {
Zeekat 2:85ab9173d947 401 mt1 = 0;
Zeekat 5:ad4ae6b65474 402 xx = x8;
Zeekat 5:ad4ae6b65474 403 yy = y8;
Zeekat 2:85ab9173d947 404 }
Zeekat 2:85ab9173d947 405 mt1++;
Zeekat 2:85ab9173d947 406 }
Zeekat 0:b0e2b38ab272 407 /////////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 408 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
Zeekat 0:b0e2b38ab272 409 ///////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 410 // these functions are called by go-flags and are used to update main variables and send signals to motor
Zeekat 0:b0e2b38ab272 411
Zeekat 0:b0e2b38ab272 412 // function that updates the values of the filtered emg-signal
Zeekat 0:b0e2b38ab272 413 void EMG_filter()
Zeekat 0:b0e2b38ab272 414 {
Zeekat 5:ad4ae6b65474 415 // filtering of EMG signal 1 (A0) first notch(2 biquads), then highpass, rectify(abs()), lowpass
Zeekat 0:b0e2b38ab272 416 double u1 = EMG_in.read();
Zeekat 0:b0e2b38ab272 417 double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
Zeekat 0:b0e2b38ab272 418 double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
Zeekat 0:b0e2b38ab272 419 double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
Zeekat 0:b0e2b38ab272 420 double y4 = abs(y3);
Zeekat 0:b0e2b38ab272 421 double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
Zeekat 0:b0e2b38ab272 422 // update global variables
Zeekat 5:ad4ae6b65474 423 output1 = y5; // output for calibration
Zeekat 5:ad4ae6b65474 424 output1_amp = y5*emg_gain1; // output for control loop
Zeekat 0:b0e2b38ab272 425
Zeekat 5:ad4ae6b65474 426 // filtering of EMG signal 2 (A2) same as before
Zeekat 0:b0e2b38ab272 427 double u1t = EMG_int.read();
Zeekat 0:b0e2b38ab272 428 double y1t = biquadfilter( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
Zeekat 0:b0e2b38ab272 429 double y2t = biquadfilter( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
Zeekat 0:b0e2b38ab272 430 double y3t = biquadfilter( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
Zeekat 0:b0e2b38ab272 431 double y4t = abs(y3t);
Zeekat 0:b0e2b38ab272 432 double y5t = biquadfilter( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
Zeekat 0:b0e2b38ab272 433 // update global variables
Zeekat 0:b0e2b38ab272 434 output2 = y5t;
Zeekat 0:b0e2b38ab272 435 output2_amp = y5t*emg_gain2;
Zeekat 5:ad4ae6b65474 436 }
Zeekat 0:b0e2b38ab272 437
Zeekat 0:b0e2b38ab272 438
Zeekat 0:b0e2b38ab272 439 // function that updates the required motor angles from the current filtered emg
Zeekat 0:b0e2b38ab272 440 void det_angles()
Zeekat 0:b0e2b38ab272 441 {
Zeekat 0:b0e2b38ab272 442 // convert global to local variable
Zeekat 0:b0e2b38ab272 443 double xy_input1 = output1_amp;
Zeekat 0:b0e2b38ab272 444 double xy_input2 = output2_amp;
Zeekat 0:b0e2b38ab272 445
Zeekat 0:b0e2b38ab272 446 // use potmeter for debugging purposes (note: does not give a smooth signal due to mechanical breakdown)
Zeekat 9:831891bb0d11 447 xy_input1 = potright.read();
Zeekat 9:831891bb0d11 448 xy_input2 = potleft.read();
Zeekat 0:b0e2b38ab272 449
Zeekat 5:ad4ae6b65474 450 // add a threshold to the signals and limit to [0,1]
Zeekat 0:b0e2b38ab272 451 xy_input1 = adapt_signal(xy_input1);
Zeekat 0:b0e2b38ab272 452 xy_input2 = adapt_signal(xy_input2);
Zeekat 1:4c9994ac229c 453
Zeekat 5:ad4ae6b65474 454 // function that when both muscles are above a threshold for 3/5s switches the axes
Zeekat 5:ad4ae6b65474 455 switch_axes(xy_input1,xy_input2);
Zeekat 5:ad4ae6b65474 456
Zeekat 0:b0e2b38ab272 457 double xy_main_input = xy_input1 - xy_input2 ; // subtract inputs to create a signal that can go from -1 to 1
Zeekat 9:831891bb0d11 458
Zeekat 0:b0e2b38ab272 459 // limit the output between -1 and 1 (signal is not supposed to be able to go above but last check)
Zeekat 0:b0e2b38ab272 460 if(xy_main_input>1) {xy_main_input = 1;}
Zeekat 0:b0e2b38ab272 461 if(xy_main_input<-1) {xy_main_input = -1;}
Zeekat 5:ad4ae6b65474 462
Zeekat 5:ad4ae6b65474 463 // calculate the y limits belonging to that particular x coordinate and update global variables
Zeekat 8:f2f45be5a060 464 y_min = - sqrt(5041 - pow(xx,2));
Zeekat 2:85ab9173d947 465 if(y_min<y_min_max){y_min = y_min_max;} // make sure the arm cannot hit the table (may later be removed)
Zeekat 8:f2f45be5a060 466 y_max = sqrt(5041 - pow(xx,2));
Zeekat 8:f2f45be5a060 467
Zeekat 8:f2f45be5a060 468 // add x_max (trial) !!!!
Zeekat 8:f2f45be5a060 469 x_max = sqrt(5041 - pow(yy,2));
Zeekat 8:f2f45be5a060 470 if (x_max > 70){x_max = 70;}
Zeekat 2:85ab9173d947 471
Zeekat 5:ad4ae6b65474 472 // use the signal to change the x-reference
Zeekat 5:ad4ae6b65474 473 if(switch_xy == false){xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x);}
Zeekat 5:ad4ae6b65474 474 // use the signal to change the y-reference
Zeekat 5:ad4ae6b65474 475 if(switch_xy == true){yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y);}
Zeekat 5:ad4ae6b65474 476
Zeekat 8:f2f45be5a060 477 // start the pre-programmed movement
Zeekat 9:831891bb0d11 478 // if(program){square_move();}
Zeekat 5:ad4ae6b65474 479
Zeekat 8:f2f45be5a060 480 // check the references (otherwise unwanted behavior)
Zeekat 8:f2f45be5a060 481 //if(yy < y_min){yy = y_min;}
Zeekat 8:f2f45be5a060 482 //if(yy > y_max){yy = y_max;}
Zeekat 8:f2f45be5a060 483
Zeekat 8:f2f45be5a060 484 // if(xx > x_max){xx = x_max;}
Zeekat 0:b0e2b38ab272 485
Zeekat 0:b0e2b38ab272 486 // x-y to arm-angles math
Zeekat 0:b0e2b38ab272 487 double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
Zeekat 0:b0e2b38ab272 488 double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
Zeekat 0:b0e2b38ab272 489 double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
Zeekat 0:b0e2b38ab272 490 double theta_one = (atan2(yy,xx)+beta);
Zeekat 0:b0e2b38ab272 491 double theta_two = (-pi + alfa);
Zeekat 0:b0e2b38ab272 492
Zeekat 0:b0e2b38ab272 493 // convert arm-angles to motor angles( (x transmission) and offset (+ offset) to account for reset position)
Zeekat 0:b0e2b38ab272 494 double phi1 = 4*(theta_one) + phi_one_offset;
Zeekat 5:ad4ae6b65474 495 // math assumes angle relative to first arm. motor does not change relative orientation, so angle wrt horizontal position is needed.
Zeekat 5:ad4ae6b65474 496 double phi2 = 4*(theta_one+theta_two) + phi_two_offset;
Zeekat 0:b0e2b38ab272 497 phi2 = -phi2; // reverse angle because of transmission.
Zeekat 0:b0e2b38ab272 498
Zeekat 0:b0e2b38ab272 499 // check the angles and apply the limits
Zeekat 0:b0e2b38ab272 500 phi1 = angle_limits(phi1,limlow1,limhigh1);
Zeekat 0:b0e2b38ab272 501 phi2 = angle_limits(phi2,limlow2,limhigh2);
Zeekat 0:b0e2b38ab272 502
Zeekat 0:b0e2b38ab272 503 // smooth the input signal (lowpass 1Hz). (to reduce the freq content after reaching limits and to make the signal less jittery)
Zeekat 0:b0e2b38ab272 504 phi1 = biquadfilter(phi1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
Zeekat 0:b0e2b38ab272 505 phi2 = biquadfilter(phi2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
Zeekat 0:b0e2b38ab272 506
Zeekat 0:b0e2b38ab272 507 // write into global variables
Zeekat 0:b0e2b38ab272 508 phi_one = phi1;
Zeekat 0:b0e2b38ab272 509 phi_two = phi2;
Zeekat 0:b0e2b38ab272 510 // if the reset button has been pressed, continiously write the start position into the global variables to reset the arm
Zeekat 0:b0e2b38ab272 511 if(reset == true)
Zeekat 0:b0e2b38ab272 512 {
Zeekat 0:b0e2b38ab272 513 phi_one = reset_phi_one;
Zeekat 0:b0e2b38ab272 514 phi_two = reset_phi_two;
Zeekat 0:b0e2b38ab272 515 }
Zeekat 0:b0e2b38ab272 516 }
Zeekat 0:b0e2b38ab272 517
Zeekat 0:b0e2b38ab272 518
Zeekat 0:b0e2b38ab272 519 // MOTOR 1
Zeekat 0:b0e2b38ab272 520 void motor1_control()
Zeekat 0:b0e2b38ab272 521 {
Zeekat 0:b0e2b38ab272 522 // change global into local variable
Zeekat 0:b0e2b38ab272 523 double reference1 = phi_one;
Zeekat 0:b0e2b38ab272 524
Zeekat 0:b0e2b38ab272 525 // add smooth start up
Zeekat 0:b0e2b38ab272 526 // for a certain amount of function iterations slowly add the delta phi between positions
Zeekat 0:b0e2b38ab272 527 // (used to gently move to start position or move to reset position)
Zeekat 0:b0e2b38ab272 528 if(rc1 < start_loops)
Zeekat 0:b0e2b38ab272 529 {
Zeekat 0:b0e2b38ab272 530 rc1++;
Zeekat 0:b0e2b38ab272 531 reference1 = phi_one_curr + ((double) rc1/start_loops)*(reference1-phi_one_curr);
Zeekat 0:b0e2b38ab272 532 }
Zeekat 0:b0e2b38ab272 533 double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 0:b0e2b38ab272 534 double error1 = (reference1 - rads1); // determine the error (reference - position)
Zeekat 0:b0e2b38ab272 535 double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
Zeekat 5:ad4ae6b65474 536 m_output1 = outputlimiter(m_output1,1); // relimit the output between -1 and 1 for safety
Zeekat 0:b0e2b38ab272 537 if(m_output1 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:b0e2b38ab272 538 motor1_rich.write(0);
Zeekat 0:b0e2b38ab272 539 motor1_aan.write(m_output1);
Zeekat 0:b0e2b38ab272 540 } else if(m_output1 < 0) {
Zeekat 0:b0e2b38ab272 541 motor1_rich.write(1);
Zeekat 0:b0e2b38ab272 542 motor1_aan.write(abs(m_output1));
Zeekat 0:b0e2b38ab272 543 }
Zeekat 0:b0e2b38ab272 544 }
Zeekat 0:b0e2b38ab272 545
Zeekat 0:b0e2b38ab272 546 // MOTOR 2
Zeekat 0:b0e2b38ab272 547 void motor2_control()
Zeekat 0:b0e2b38ab272 548 {
Zeekat 0:b0e2b38ab272 549 double reference2 = phi_two;
Zeekat 0:b0e2b38ab272 550
Zeekat 0:b0e2b38ab272 551 // add smooth start up
Zeekat 0:b0e2b38ab272 552 // for a certain amount of function iterations slowly add the delta phi between positions
Zeekat 0:b0e2b38ab272 553 // (used to gently move to start position [x,y] = [60,0] or move to the reset position [phi1,phi2] = (0,0)
Zeekat 0:b0e2b38ab272 554 if(rc2 < start_loops)
Zeekat 0:b0e2b38ab272 555 {
Zeekat 0:b0e2b38ab272 556 rc2++;
Zeekat 0:b0e2b38ab272 557 reference2 = phi_two_curr + ((double) rc2/start_loops)*(reference2-phi_two_curr);
Zeekat 5:ad4ae6b65474 558 }
Zeekat 0:b0e2b38ab272 559 double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
Zeekat 0:b0e2b38ab272 560 double error2 = (reference2 - rads2); // determine the error (reference - position)
Zeekat 0:b0e2b38ab272 561 double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
Zeekat 0:b0e2b38ab272 562 m_output2 = outputlimiter(m_output2,1); // final output limit (not really needed, is for safety)
Zeekat 0:b0e2b38ab272 563 if(m_output2 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:b0e2b38ab272 564 motor2_rich.write(0);
Zeekat 0:b0e2b38ab272 565 motor2_aan.write(m_output2);
Zeekat 0:b0e2b38ab272 566 } else if(m_output2 < 0) {
Zeekat 0:b0e2b38ab272 567 motor2_rich.write(1);
Zeekat 0:b0e2b38ab272 568 motor2_aan.write(abs(m_output2));
Zeekat 0:b0e2b38ab272 569 }
Zeekat 0:b0e2b38ab272 570 }
Zeekat 0:b0e2b38ab272 571
Zeekat 0:b0e2b38ab272 572 // calibrate the emg-signal
Zeekat 0:b0e2b38ab272 573 // works bij taking a certain amount of samples taking the max then normalize to the desired value
Zeekat 0:b0e2b38ab272 574 // went to max-value type. must be tested.!
Zeekat 0:b0e2b38ab272 575 void calibrate_amp()
Zeekat 0:b0e2b38ab272 576 {
Zeekat 0:b0e2b38ab272 577 double max1 = 0;
Zeekat 0:b0e2b38ab272 578 double max2 = 0;
Zeekat 0:b0e2b38ab272 579 for(int i = 0; i<cal_samples; i++)
Zeekat 0:b0e2b38ab272 580 {
Zeekat 0:b0e2b38ab272 581 EMG_filter(); // run filter
Zeekat 0:b0e2b38ab272 582 double input1 = output1; // take data from global variable
Zeekat 0:b0e2b38ab272 583 if(input1>max1){max1 = input1;} // take max input
Zeekat 0:b0e2b38ab272 584 double input2 = output2;
Zeekat 0:b0e2b38ab272 585 if(input2>max2){max2 = input2;} // take max input
Zeekat 5:ad4ae6b65474 586 wait(EMG_step); // !! has to run at same interval as filter in main loop !! otherwise a 'different' signal will be used for calibration
Zeekat 0:b0e2b38ab272 587 }
Zeekat 0:b0e2b38ab272 588 emg_gain1 = normalize_emg_value/max1; // normalize the amplification so that the maximum signal hits the desired one
Zeekat 0:b0e2b38ab272 589 emg_gain2 = normalize_emg_value/max2;
Zeekat 5:ad4ae6b65474 590 }
Zeekat 0:b0e2b38ab272 591 //////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 592 //////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
Zeekat 0:b0e2b38ab272 593 ////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 594
Zeekat 0:b0e2b38ab272 595
Zeekat 0:b0e2b38ab272 596 void EMG_activate(){emg_go = true;}
Zeekat 0:b0e2b38ab272 597 void angle_activate(){cart_go = true;}
Zeekat 0:b0e2b38ab272 598 void motor1_activate(){motor1_go = true;}
Zeekat 0:b0e2b38ab272 599 void motor2_activate(){motor2_go = true;}
Zeekat 0:b0e2b38ab272 600
Zeekat 0:b0e2b38ab272 601 int main()
Zeekat 0:b0e2b38ab272 602 {
Zeekat 0:b0e2b38ab272 603 main_filter.attach(&EMG_activate, EMG_step);
Zeekat 0:b0e2b38ab272 604 cartesian.attach(&angle_activate, controlstep);
Zeekat 0:b0e2b38ab272 605 controller1.attach(&motor1_activate, controlstep); // call a go-flag
Zeekat 0:b0e2b38ab272 606 controller2.attach(&motor2_activate, controlstep);
Zeekat 0:b0e2b38ab272 607 while(true)
Zeekat 0:b0e2b38ab272 608 {
Zeekat 0:b0e2b38ab272 609 // button press functions
Zeekat 0:b0e2b38ab272 610 // flow buttons
Zeekat 0:b0e2b38ab272 611 if(buttonlinks.read() == 0)
Zeekat 0:b0e2b38ab272 612 {
Zeekat 0:b0e2b38ab272 613 loop_start = !loop_start;
Zeekat 1:4c9994ac229c 614 wait(0.2);
Zeekat 1:4c9994ac229c 615 while(buttonlinks.read() == 0);
Zeekat 0:b0e2b38ab272 616 }
Zeekat 0:b0e2b38ab272 617 if(buttonrechts.read() == 0)
Zeekat 0:b0e2b38ab272 618 {
Zeekat 0:b0e2b38ab272 619 calib_start = !calib_start;
Zeekat 1:4c9994ac229c 620 wait(0.2);
Zeekat 1:4c9994ac229c 621 while(buttonrechts.read() == 0);
Zeekat 0:b0e2b38ab272 622 }
Zeekat 8:f2f45be5a060 623 // switch axes
Zeekat 8:f2f45be5a060 624 if(switch_xy_button.read() == 0)
Zeekat 0:b0e2b38ab272 625 {
Zeekat 8:f2f45be5a060 626 switch_xy = !switch_xy;
Zeekat 8:f2f45be5a060 627 led_right.write(!led_right.read());
Zeekat 1:4c9994ac229c 628 wait(0.2);
Zeekat 8:f2f45be5a060 629 while(switch_xy_button.read() == 0);
Zeekat 0:b0e2b38ab272 630 }
Zeekat 0:b0e2b38ab272 631 if(reset_button.read() == 0)
Zeekat 0:b0e2b38ab272 632 {
Zeekat 0:b0e2b38ab272 633 reset = !reset;
Zeekat 0:b0e2b38ab272 634 phi_one_curr = phi_one;
Zeekat 0:b0e2b38ab272 635 phi_two_curr = phi_two;
Zeekat 0:b0e2b38ab272 636 rc1 = 0;
Zeekat 0:b0e2b38ab272 637 rc2 = 0;
Zeekat 0:b0e2b38ab272 638 wait(0.2);
Zeekat 1:4c9994ac229c 639 while(reset_button.read() == 0);
Zeekat 0:b0e2b38ab272 640 }
Zeekat 0:b0e2b38ab272 641 //////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 642 // Main Control stuff and options
Zeekat 0:b0e2b38ab272 643 if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops
Zeekat 0:b0e2b38ab272 644 {
Zeekat 0:b0e2b38ab272 645 LED(1,1,0); // turn blue led on
Zeekat 0:b0e2b38ab272 646 if(cart_go) { cart_go = false; det_angles();}
Zeekat 0:b0e2b38ab272 647 if(emg_go) { emg_go = false; EMG_filter();}
Zeekat 0:b0e2b38ab272 648 if(motor1_go) { motor1_go = false; motor1_control();}
Zeekat 0:b0e2b38ab272 649 if(motor2_go) { motor2_go = false; motor2_control();}
Zeekat 0:b0e2b38ab272 650 }
Zeekat 0:b0e2b38ab272 651 // shut off both motors
Zeekat 0:b0e2b38ab272 652 if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);}
Zeekat 0:b0e2b38ab272 653
Zeekat 0:b0e2b38ab272 654 // turn green led on // start calibration procedures
Zeekat 0:b0e2b38ab272 655 if(loop_start == false && calib_start == true)
Zeekat 0:b0e2b38ab272 656 { LED(1,0,1);
Zeekat 0:b0e2b38ab272 657 motor1_aan.write(0);
Zeekat 0:b0e2b38ab272 658 motor2_aan.write(0);
Zeekat 0:b0e2b38ab272 659 calibrate_amp(); // 10 second calibration
Zeekat 0:b0e2b38ab272 660 calib_start = false; // turn calibration mode off
Zeekat 0:b0e2b38ab272 661 }
Zeekat 0:b0e2b38ab272 662
Zeekat 0:b0e2b38ab272 663 // turn red led on (show both buttons have been pressed)
Zeekat 0:b0e2b38ab272 664 if(loop_start == true && calib_start == true)
Zeekat 0:b0e2b38ab272 665 {
Zeekat 0:b0e2b38ab272 666 LED(0,1,1);
Zeekat 0:b0e2b38ab272 667 motor1_aan.write(0);
Zeekat 0:b0e2b38ab272 668 motor2_aan.write(0);
Zeekat 0:b0e2b38ab272 669 }
Zeekat 0:b0e2b38ab272 670
Zeekat 0:b0e2b38ab272 671 // turn leds off (both buttons false)
Zeekat 0:b0e2b38ab272 672 else { LED(1,1,1);}
Zeekat 0:b0e2b38ab272 673 }
Zeekat 0:b0e2b38ab272 674 }