Wouter Bos / Mbed 2 deprecated group29_x_y_control_dynamic_x-lims

presentatie versie met potmeters enabled

Dependencies:   Encoder HIDScope mbed

main.cpp@7:acf28eb906c4, 2015-10-29 (annotated)

Committer:
Zeekat
Date:
Thu Oct 29 11:18:30 2015 +0000
Revision:
7:acf28eb906c4
Parent:
6:30afff8ae34a
Child:
8:f2f45be5a060
tekst doorgenomen en verbeterd

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