Wouter Bos / Mbed 2 deprecated group29_x_y_control_dynamic_x-lims

presentatie versie met potmeters enabled

Dependencies:   Encoder HIDScope mbed

main.cpp@1:4c9994ac229c, 2015-10-27 (annotated)

Committer:
Zeekat
Date:
Tue Oct 27 14:08:23 2015 +0000
Revision:
1:4c9994ac229c
Parent:
0:b0e2b38ab272
Child:
2:85ab9173d947
goed werkende versie. emg signaal kun je mee switchen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zeekat 0:b0e2b38ab272 1 #include "mbed.h"
Zeekat 0:b0e2b38ab272 2 #include "MODSERIAL.h"
Zeekat 0:b0e2b38ab272 3 #include "encoder.h"
Zeekat 0:b0e2b38ab272 4 #include "HIDScope.h"
Zeekat 0:b0e2b38ab272 5 #include "math.h"
Zeekat 0:b0e2b38ab272 6
Zeekat 0:b0e2b38ab272 7 Serial pc(USBTX,USBRX); // MODSERIAL output
Zeekat 0:b0e2b38ab272 8 HIDScope scope(4); // definieerd het aantal kanalen van de scope
Zeekat 0:b0e2b38ab272 9
Zeekat 0:b0e2b38ab272 10 // Define Tickers and control frequencies
Zeekat 0:b0e2b38ab272 11 Ticker controller1, controller2, main_filter, cartesian, send;
Zeekat 0:b0e2b38ab272 12 // Go flag variables belonging to Tickers
Zeekat 0:b0e2b38ab272 13 volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false;
Zeekat 0:b0e2b38ab272 14
Zeekat 0:b0e2b38ab272 15 // Frequency control
Zeekat 0:b0e2b38ab272 16 double controlfreq = 50 ; // controlloops frequentie (Hz)
Zeekat 0:b0e2b38ab272 17 double controlstep = 1/controlfreq; // timestep derived from controlfreq
Zeekat 0:b0e2b38ab272 18
Zeekat 0:b0e2b38ab272 19 double EMG_freq = 200;
Zeekat 0:b0e2b38ab272 20 double EMG_step = 1/EMG_freq;
Zeekat 0:b0e2b38ab272 21
Zeekat 0:b0e2b38ab272 22 //////////////////////// IN-OUTPUT /////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 23 //MOTOR OUTPUTPINS
Zeekat 0:b0e2b38ab272 24 PwmOut motor1_aan(D6), motor2_aan(D5); // PWM signaal motor 2 (uit sheets)
Zeekat 0:b0e2b38ab272 25 DigitalOut motor1_rich(D7), motor2_rich(D4); // digitaal signaal voor richting
Zeekat 0:b0e2b38ab272 26
Zeekat 0:b0e2b38ab272 27 // ENCODER INPUTPINS
Zeekat 0:b0e2b38ab272 28 Encoder motor1_enc(D12,D11), motor2_enc(D10,D9); // encoder outputpins
Zeekat 0:b0e2b38ab272 29
Zeekat 0:b0e2b38ab272 30 // EXTRA INPUTS AND REQUIRED VARIABLES
Zeekat 0:b0e2b38ab272 31 //POTMETERS (used for debugging purposes, not reliable anymore)
Zeekat 0:b0e2b38ab272 32 AnalogIn potright(A4); // define the potmeter outputpins
Zeekat 0:b0e2b38ab272 33 AnalogIn potleft(A5);
Zeekat 0:b0e2b38ab272 34
Zeekat 0:b0e2b38ab272 35 // Analoge input signalen defineren
Zeekat 0:b0e2b38ab272 36 AnalogIn EMG_in(A0); // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
Zeekat 0:b0e2b38ab272 37 AnalogIn EMG_int(A2); // deze leest A3 uit
Zeekat 0:b0e2b38ab272 38
Zeekat 0:b0e2b38ab272 39 // BUTTONS
Zeekat 0:b0e2b38ab272 40 // control flow
Zeekat 0:b0e2b38ab272 41 DigitalIn buttonlinks(PTA4);
Zeekat 0:b0e2b38ab272 42 DigitalIn buttonrechts(PTC6);
Zeekat 0:b0e2b38ab272 43 DigitalIn reset_button(D1);
Zeekat 0:b0e2b38ab272 44 // init values
Zeekat 0:b0e2b38ab272 45 bool loop_start = false;
Zeekat 0:b0e2b38ab272 46 bool calib_start = false;
Zeekat 0:b0e2b38ab272 47 bool reset = false;
Zeekat 0:b0e2b38ab272 48
Zeekat 0:b0e2b38ab272 49 // axis control
Zeekat 0:b0e2b38ab272 50 DigitalIn switch_xy_button(D0);
Zeekat 0:b0e2b38ab272 51 // init values
Zeekat 0:b0e2b38ab272 52 bool switch_xy = false;
Zeekat 0:b0e2b38ab272 53 // LED outputs on bioshield
Zeekat 0:b0e2b38ab272 54 DigitalOut led_right(D2);
Zeekat 0:b0e2b38ab272 55 // LED outputs on dev-board
Zeekat 0:b0e2b38ab272 56 DigitalOut ledred(LED1);
Zeekat 0:b0e2b38ab272 57 DigitalOut ledgreen(LED2);
Zeekat 0:b0e2b38ab272 58 DigitalOut ledblue(LED3);
Zeekat 0:b0e2b38ab272 59
Zeekat 0:b0e2b38ab272 60
Zeekat 0:b0e2b38ab272 61 //////////////////////////////////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 62 ////////////////////////// GLOBAL VARIABLES ///////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 63 ////////////////////////////////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 64
Zeekat 0:b0e2b38ab272 65 double t = 0; // used for circle movement
Zeekat 1:4c9994ac229c 66
Zeekat 1:4c9994ac229c 67 double sw1 = 0;
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 0:b0e2b38ab272 115 double input_threshold = 0.25; // 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 1:4c9994ac229c 122 double Vmax_x = 7.5, 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 0:b0e2b38ab272 133 const double m2_Kd = 0.3;
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 0:b0e2b38ab272 142 // EMG calibration variables
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 0:b0e2b38ab272 147 double cal_samples = controlfreq*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 0:b0e2b38ab272 152 // differential action filter, same is used for both 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 0:b0e2b38ab272 163 // differential action filter (lowpass 5Hz at 50Hz)
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 0:b0e2b38ab272 165 // input filter (lowpass 1Hz at 50samples) (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 0:b0e2b38ab272 194 const double denlow5_3 =0.956543676511203;
Zeekat 0:b0e2b38ab272 195
Zeekat 0:b0e2b38ab272 196 ////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 197 /////////////////// START OF SIDE FUNCTIONS ////////////////////
Zeekat 0:b0e2b38ab272 198 //////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 199 // these functions are tailored to perform 1 specific function
Zeekat 0:b0e2b38ab272 200
Zeekat 0:b0e2b38ab272 201 // this funtion flips leds on and off accordin to input with 0 being on
Zeekat 0:b0e2b38ab272 202 void LED(int red,int green,int blue)
Zeekat 0:b0e2b38ab272 203 {
Zeekat 0:b0e2b38ab272 204 ledred.write(red);
Zeekat 0:b0e2b38ab272 205 ledgreen.write(green);
Zeekat 0:b0e2b38ab272 206 ledblue.write(blue);
Zeekat 0:b0e2b38ab272 207 }
Zeekat 0:b0e2b38ab272 208
Zeekat 0:b0e2b38ab272 209 // counts 2 radians
Zeekat 0:b0e2b38ab272 210 // this function takes counts from the encoder and converts it to the amount of radians from the zero position.
Zeekat 0:b0e2b38ab272 211 // It has been set up for standard 2X DECODING!!!
Zeekat 0:b0e2b38ab272 212 double get_radians(double counts)
Zeekat 0:b0e2b38ab272 213 {
Zeekat 0:b0e2b38ab272 214 double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, last warning)
Zeekat 0:b0e2b38ab272 215 return radians;
Zeekat 0:b0e2b38ab272 216 }
Zeekat 0:b0e2b38ab272 217
Zeekat 0:b0e2b38ab272 218
Zeekat 0:b0e2b38ab272 219 // This functions takes a 0->1 input, uses passing by reference (&c_reference)
Zeekat 0:b0e2b38ab272 220 // to create a reference that moves with a variable speed. It is meant for -1->1 values
Zeekat 0:b0e2b38ab272 221 double reference_f(double input, double &c_reference, double limlow, double limhigh, double Vmax)
Zeekat 0:b0e2b38ab272 222 {
Zeekat 0:b0e2b38ab272 223 double reference = c_reference + input * controlstep * Vmax ;
Zeekat 0:b0e2b38ab272 224 // two if statements check if the reference exceeds the limits placed upon the arms
Zeekat 0:b0e2b38ab272 225 if(reference < limlow){reference = limlow;}
Zeekat 0:b0e2b38ab272 226 if(reference > limhigh){reference = limhigh;}
Zeekat 0:b0e2b38ab272 227 c_reference = reference; // change the global variable to the latest location.
Zeekat 0:b0e2b38ab272 228 return reference;
Zeekat 0:b0e2b38ab272 229 }
Zeekat 0:b0e2b38ab272 230
Zeekat 0:b0e2b38ab272 231
Zeekat 0:b0e2b38ab272 232 // This function takes the controller outputvalue and ensures it is between -1 and 1
Zeekat 0:b0e2b38ab272 233 // 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 234 double outputlimiter (double output, double limit)
Zeekat 0:b0e2b38ab272 235 {
Zeekat 0:b0e2b38ab272 236 if(output> limit)
Zeekat 0:b0e2b38ab272 237 {
Zeekat 0:b0e2b38ab272 238 output = 1;
Zeekat 0:b0e2b38ab272 239 }
Zeekat 0:b0e2b38ab272 240 else if(output < limit && output > 0)
Zeekat 0:b0e2b38ab272 241 {
Zeekat 0:b0e2b38ab272 242 output = output;
Zeekat 0:b0e2b38ab272 243 }
Zeekat 0:b0e2b38ab272 244 else if(output > -limit && output < 0)
Zeekat 0:b0e2b38ab272 245 {
Zeekat 0:b0e2b38ab272 246 output = output;
Zeekat 0:b0e2b38ab272 247 }
Zeekat 0:b0e2b38ab272 248 else if(output < -limit)
Zeekat 0:b0e2b38ab272 249 {
Zeekat 0:b0e2b38ab272 250 (output = -1);
Zeekat 0:b0e2b38ab272 251 }
Zeekat 0:b0e2b38ab272 252 return output;
Zeekat 0:b0e2b38ab272 253 }
Zeekat 0:b0e2b38ab272 254
Zeekat 0:b0e2b38ab272 255
Zeekat 0:b0e2b38ab272 256 // BIQUADFILTER CODE GIVEN IN SHEETS (input format: den, den, nom, nom, nom)
Zeekat 0:b0e2b38ab272 257 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 258 {
Zeekat 0:b0e2b38ab272 259 double v = u - a1*v1 - a2*v2;
Zeekat 0:b0e2b38ab272 260 double y = b0*v + b1*v1 + b2*v2;
Zeekat 0:b0e2b38ab272 261 v2 = v1;
Zeekat 0:b0e2b38ab272 262 v1 = v;
Zeekat 0:b0e2b38ab272 263 return y;
Zeekat 0:b0e2b38ab272 264 }
Zeekat 0:b0e2b38ab272 265
Zeekat 0:b0e2b38ab272 266 // PID Controller given in sheets
Zeekat 0:b0e2b38ab272 267 // adapted to use the same differential filter, and to split the different terms
Zeekat 0:b0e2b38ab272 268 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
Zeekat 0:b0e2b38ab272 269 {
Zeekat 0:b0e2b38ab272 270 // Proportional
Zeekat 0:b0e2b38ab272 271 double P = Kp * e;
Zeekat 0:b0e2b38ab272 272 // Integral
Zeekat 0:b0e2b38ab272 273 e_int = e_int + Ts * e;
Zeekat 0:b0e2b38ab272 274 double I = e_int * Ki;
Zeekat 0:b0e2b38ab272 275 // Derivative
Zeekat 0:b0e2b38ab272 276 double e_derr = (e - e_prev)/Ts;
Zeekat 0:b0e2b38ab272 277 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 278 //
Zeekat 0:b0e2b38ab272 279 e_prev = e;
Zeekat 0:b0e2b38ab272 280 double D = Kd* e_derr;
Zeekat 0:b0e2b38ab272 281 // PID
Zeekat 0:b0e2b38ab272 282 double output = P + I + D;
Zeekat 0:b0e2b38ab272 283 return output;
Zeekat 0:b0e2b38ab272 284 }
Zeekat 0:b0e2b38ab272 285
Zeekat 0:b0e2b38ab272 286
Zeekat 0:b0e2b38ab272 287 // function that limits the angles that can be used in the motor reference signal
Zeekat 0:b0e2b38ab272 288 double angle_limits(double phi, double limlow, double limhigh)
Zeekat 0:b0e2b38ab272 289 {
Zeekat 0:b0e2b38ab272 290 if(phi < limlow)
Zeekat 0:b0e2b38ab272 291 {
Zeekat 0:b0e2b38ab272 292 phi = limlow;
Zeekat 0:b0e2b38ab272 293 }
Zeekat 0:b0e2b38ab272 294 if(phi > limhigh)
Zeekat 0:b0e2b38ab272 295 {
Zeekat 0:b0e2b38ab272 296 phi = limhigh;
Zeekat 0:b0e2b38ab272 297 }
Zeekat 0:b0e2b38ab272 298 return phi;
Zeekat 0:b0e2b38ab272 299 }
Zeekat 0:b0e2b38ab272 300
Zeekat 0:b0e2b38ab272 301 // this function adapts the filtered emg signal for use in the reference generation
Zeekat 0:b0e2b38ab272 302 // adds threshold value and normalizes between 0 and 1
Zeekat 0:b0e2b38ab272 303 double adapt_signal(double input)
Zeekat 0:b0e2b38ab272 304 {
Zeekat 0:b0e2b38ab272 305 // add threshold value for outputs
Zeekat 0:b0e2b38ab272 306 if(input < input_threshold){input = 0;}
Zeekat 0:b0e2b38ab272 307
Zeekat 0:b0e2b38ab272 308 // return the input to a value between 0 and 1 (otherwise you will get jumps in input)
Zeekat 0:b0e2b38ab272 309 input = (input-input_threshold) * (1/(1-input_threshold));
Zeekat 0:b0e2b38ab272 310
Zeekat 0:b0e2b38ab272 311 // if below 0 = 0 (otherwise values like -input_threshold start popping up)
Zeekat 0:b0e2b38ab272 312 if(input < 0){input = 0;}
Zeekat 0:b0e2b38ab272 313
Zeekat 0:b0e2b38ab272 314 // limit signal maximum to 1
Zeekat 0:b0e2b38ab272 315 if(input > 1){input = 1;}
Zeekat 0:b0e2b38ab272 316 return input;
Zeekat 0:b0e2b38ab272 317 }
Zeekat 0:b0e2b38ab272 318
Zeekat 0:b0e2b38ab272 319 // send stuff to putty to check things
Zeekat 0:b0e2b38ab272 320 void mod_send()
Zeekat 0:b0e2b38ab272 321 {
Zeekat 0:b0e2b38ab272 322 pc.printf("xx = %f, yy = %f, phi1 = %f, phi2 = %f \n",xx,yy,phi_one,phi_two);
Zeekat 0:b0e2b38ab272 323 }
Zeekat 0:b0e2b38ab272 324 /////////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 325 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
Zeekat 0:b0e2b38ab272 326 ///////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 327 // these functions are called by go-flags and are used to update main variables and send signals to motor
Zeekat 0:b0e2b38ab272 328
Zeekat 0:b0e2b38ab272 329 // function that updates the values of the filtered emg-signal
Zeekat 0:b0e2b38ab272 330 void EMG_filter()
Zeekat 0:b0e2b38ab272 331 {
Zeekat 0:b0e2b38ab272 332 // filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
Zeekat 0:b0e2b38ab272 333 double u1 = EMG_in.read();
Zeekat 0:b0e2b38ab272 334 double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
Zeekat 0:b0e2b38ab272 335 double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
Zeekat 0:b0e2b38ab272 336 double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
Zeekat 0:b0e2b38ab272 337 double y4 = abs(y3);
Zeekat 0:b0e2b38ab272 338 double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
Zeekat 0:b0e2b38ab272 339 // update global variables
Zeekat 0:b0e2b38ab272 340 output1 = y5;
Zeekat 0:b0e2b38ab272 341 output1_amp = y5*emg_gain1; // update global variable
Zeekat 0:b0e2b38ab272 342
Zeekat 0:b0e2b38ab272 343 // filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
Zeekat 0:b0e2b38ab272 344 double u1t = EMG_int.read();
Zeekat 0:b0e2b38ab272 345 double y1t = biquadfilter( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
Zeekat 0:b0e2b38ab272 346 double y2t = biquadfilter( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
Zeekat 0:b0e2b38ab272 347 double y3t = biquadfilter( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
Zeekat 0:b0e2b38ab272 348 double y4t = abs(y3t);
Zeekat 0:b0e2b38ab272 349 double y5t = biquadfilter( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
Zeekat 0:b0e2b38ab272 350 // update global variables
Zeekat 0:b0e2b38ab272 351 output2 = y5t;
Zeekat 0:b0e2b38ab272 352 output2_amp = y5t*emg_gain2;
Zeekat 1:4c9994ac229c 353 scope.set(0,output1_amp);
Zeekat 1:4c9994ac229c 354 scope.set(1,output2_amp);
Zeekat 1:4c9994ac229c 355 scope.set(2,u1);
Zeekat 1:4c9994ac229c 356 scope.set(3,u1t);
Zeekat 1:4c9994ac229c 357 scope.send();
Zeekat 0:b0e2b38ab272 358 }
Zeekat 0:b0e2b38ab272 359
Zeekat 0:b0e2b38ab272 360
Zeekat 0:b0e2b38ab272 361 // function that updates the required motor angles from the current filtered emg
Zeekat 0:b0e2b38ab272 362 void det_angles()
Zeekat 0:b0e2b38ab272 363 {
Zeekat 0:b0e2b38ab272 364 // convert global to local variable
Zeekat 0:b0e2b38ab272 365 double xy_input1 = output1_amp;
Zeekat 0:b0e2b38ab272 366 double xy_input2 = output2_amp;
Zeekat 0:b0e2b38ab272 367
Zeekat 0:b0e2b38ab272 368 // use potmeter for debugging purposes (note: does not give a smooth signal due to mechanical breakdown)
Zeekat 1:4c9994ac229c 369 //xy_input1 = potright.read();
Zeekat 1:4c9994ac229c 370 //xy_input2 = potleft.read();
Zeekat 0:b0e2b38ab272 371
Zeekat 0:b0e2b38ab272 372 xy_input1 = adapt_signal(xy_input1);
Zeekat 0:b0e2b38ab272 373 xy_input2 = adapt_signal(xy_input2);
Zeekat 1:4c9994ac229c 374
Zeekat 1:4c9994ac229c 375 if(xy_input1 > input_threshold && xy_input2 > input_threshold)
Zeekat 1:4c9994ac229c 376 {
Zeekat 1:4c9994ac229c 377 sw1++;
Zeekat 1:4c9994ac229c 378 }
Zeekat 1:4c9994ac229c 379 if(sw1 == 30)
Zeekat 1:4c9994ac229c 380 {
Zeekat 1:4c9994ac229c 381 switch_xy = !switch_xy;
Zeekat 1:4c9994ac229c 382 led_right.write(!led_right.read()); // turn on led when switched
Zeekat 1:4c9994ac229c 383 sw1 = 0;
Zeekat 1:4c9994ac229c 384 }
Zeekat 1:4c9994ac229c 385 if(xy_input1 < input_threshold || xy_input2 < input_threshold)
Zeekat 1:4c9994ac229c 386 {
Zeekat 1:4c9994ac229c 387 sw1 = 0;
Zeekat 1:4c9994ac229c 388 }
Zeekat 0:b0e2b38ab272 389
Zeekat 0:b0e2b38ab272 390 double xy_main_input = xy_input1 - xy_input2 ; // subtract inputs to create a signal that can go from -1 to 1
Zeekat 0:b0e2b38ab272 391
Zeekat 0:b0e2b38ab272 392 //scope.set(0,xy_main_input);
Zeekat 0:b0e2b38ab272 393 // limit the output between -1 and 1 (signal is not supposed to be able to go above but last check)
Zeekat 0:b0e2b38ab272 394 if(xy_main_input>1) {xy_main_input = 1;}
Zeekat 0:b0e2b38ab272 395 if(xy_main_input<-1) {xy_main_input = -1;}
Zeekat 0:b0e2b38ab272 396
Zeekat 0:b0e2b38ab272 397 if(switch_xy == false) // use the signal to change the x-reference
Zeekat 0:b0e2b38ab272 398 {
Zeekat 0:b0e2b38ab272 399 xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x); // change the global x-reference
Zeekat 0:b0e2b38ab272 400 // calculate the y limits belonging to that particular x coordinate and update global variables
Zeekat 1:4c9994ac229c 401 y_min = - sqrt(5184 - pow(xx,2));
Zeekat 0:b0e2b38ab272 402 if(y_min<y_min_max){y_min = y_min_max;} // make sure the arm cannot hit the table (may later be removed)
Zeekat 1:4c9994ac229c 403 y_max = sqrt(5184 - pow(xx,2));
Zeekat 0:b0e2b38ab272 404
Zeekat 0:b0e2b38ab272 405 }
Zeekat 0:b0e2b38ab272 406 if(switch_xy == true) // use the signal to change the y-reference
Zeekat 0:b0e2b38ab272 407 {
Zeekat 0:b0e2b38ab272 408 yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y); // change the y-reference
Zeekat 0:b0e2b38ab272 409
Zeekat 0:b0e2b38ab272 410 }
Zeekat 1:4c9994ac229c 411
Zeekat 0:b0e2b38ab272 412 // check the y-reference (otherwise if x is controlled after y has been controlled, the circle is not followed).
Zeekat 0:b0e2b38ab272 413 if(yy < y_min){yy = y_min;}
Zeekat 0:b0e2b38ab272 414 if(yy > y_max){yy = y_max;}
Zeekat 0:b0e2b38ab272 415
Zeekat 0:b0e2b38ab272 416 // let the arm make a circle (testing)
Zeekat 0:b0e2b38ab272 417 // xx = 60 + 5*cos(t);
Zeekat 0:b0e2b38ab272 418 // yy = 5*sin(t);
Zeekat 0:b0e2b38ab272 419 // t = t + 0.01;
Zeekat 0:b0e2b38ab272 420
Zeekat 0:b0e2b38ab272 421 // x-y to arm-angles math
Zeekat 0:b0e2b38ab272 422 double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
Zeekat 0:b0e2b38ab272 423 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 424 double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
Zeekat 0:b0e2b38ab272 425 double theta_one = (atan2(yy,xx)+beta);
Zeekat 0:b0e2b38ab272 426 double theta_two = (-pi + alfa);
Zeekat 0:b0e2b38ab272 427
Zeekat 0:b0e2b38ab272 428 // convert arm-angles to motor angles( (x transmission) and offset (+ offset) to account for reset position)
Zeekat 0:b0e2b38ab272 429 double phi1 = 4*(theta_one) + phi_one_offset;
Zeekat 0:b0e2b38ab272 430 double phi2 = 4*(theta_one+theta_two) + phi_two_offset; // math assumes angle relative to first arm. motor does not change relative orientation, so angle wrt horizontal position is needed.
Zeekat 0:b0e2b38ab272 431 phi2 = -phi2; // reverse angle because of transmission.
Zeekat 0:b0e2b38ab272 432
Zeekat 0:b0e2b38ab272 433 // check the angles and apply the limits
Zeekat 0:b0e2b38ab272 434 phi1 = angle_limits(phi1,limlow1,limhigh1);
Zeekat 0:b0e2b38ab272 435 phi2 = angle_limits(phi2,limlow2,limhigh2);
Zeekat 0:b0e2b38ab272 436
Zeekat 0:b0e2b38ab272 437 // smooth the input signal (lowpass 1Hz). (to reduce the freq content after reaching limits and to make the signal less jittery)
Zeekat 0:b0e2b38ab272 438 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 439 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 440
Zeekat 0:b0e2b38ab272 441 // write into global variables
Zeekat 0:b0e2b38ab272 442 phi_one = phi1;
Zeekat 0:b0e2b38ab272 443 phi_two = phi2;
Zeekat 0:b0e2b38ab272 444
Zeekat 0:b0e2b38ab272 445 // if the reset button has been pressed, continiously write the start position into the global variables to reset the arm
Zeekat 0:b0e2b38ab272 446 if(reset == true)
Zeekat 0:b0e2b38ab272 447 {
Zeekat 0:b0e2b38ab272 448 phi_one = reset_phi_one;
Zeekat 0:b0e2b38ab272 449 phi_two = reset_phi_two;
Zeekat 0:b0e2b38ab272 450 }
Zeekat 0:b0e2b38ab272 451 }
Zeekat 0:b0e2b38ab272 452
Zeekat 0:b0e2b38ab272 453
Zeekat 0:b0e2b38ab272 454 // MOTOR 1
Zeekat 0:b0e2b38ab272 455 void motor1_control()
Zeekat 0:b0e2b38ab272 456 {
Zeekat 0:b0e2b38ab272 457 // change global into local variable
Zeekat 0:b0e2b38ab272 458 double reference1 = phi_one;
Zeekat 0:b0e2b38ab272 459
Zeekat 0:b0e2b38ab272 460 // add smooth start up
Zeekat 0:b0e2b38ab272 461 // for a certain amount of function iterations slowly add the delta phi between positions
Zeekat 0:b0e2b38ab272 462 // (used to gently move to start position or move to reset position)
Zeekat 0:b0e2b38ab272 463 if(rc1 < start_loops)
Zeekat 0:b0e2b38ab272 464 {
Zeekat 0:b0e2b38ab272 465 rc1++;
Zeekat 0:b0e2b38ab272 466 reference1 = phi_one_curr + ((double) rc1/start_loops)*(reference1-phi_one_curr);
Zeekat 0:b0e2b38ab272 467 }
Zeekat 0:b0e2b38ab272 468
Zeekat 0:b0e2b38ab272 469 double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 0:b0e2b38ab272 470 double error1 = (reference1 - rads1); // determine the error (reference - position)
Zeekat 0:b0e2b38ab272 471 double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
Zeekat 0:b0e2b38ab272 472 // check the real angles and stop if exceeded (NaN protection) // doesnt work because start angle is lower so output stays 0
Zeekat 0:b0e2b38ab272 473 // if(rads1 < (limlow1 - 0.05)){m_output1 = 0;}
Zeekat 0:b0e2b38ab272 474 // if(rads1 > (limhigh1 + 0.05)){m_output1 = 0;}
Zeekat 0:b0e2b38ab272 475 m_output1 = outputlimiter(m_output1,1); // relimit the output between -1 and 1 for safety
Zeekat 0:b0e2b38ab272 476 if(m_output1 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:b0e2b38ab272 477 motor1_rich.write(0);
Zeekat 0:b0e2b38ab272 478 motor1_aan.write(m_output1);
Zeekat 0:b0e2b38ab272 479 } else if(m_output1 < 0) {
Zeekat 0:b0e2b38ab272 480 motor1_rich.write(1);
Zeekat 0:b0e2b38ab272 481 motor1_aan.write(abs(m_output1));
Zeekat 0:b0e2b38ab272 482 }
Zeekat 0:b0e2b38ab272 483 }
Zeekat 0:b0e2b38ab272 484
Zeekat 0:b0e2b38ab272 485 // MOTOR 2
Zeekat 0:b0e2b38ab272 486 void motor2_control()
Zeekat 0:b0e2b38ab272 487 {
Zeekat 0:b0e2b38ab272 488 double reference2 = phi_two;
Zeekat 0:b0e2b38ab272 489
Zeekat 0:b0e2b38ab272 490 // add smooth start up
Zeekat 0:b0e2b38ab272 491 // for a certain amount of function iterations slowly add the delta phi between positions
Zeekat 0:b0e2b38ab272 492 // (used to gently move to start position [x,y] = [60,0] or move to the reset position [phi1,phi2] = (0,0)
Zeekat 0:b0e2b38ab272 493 if(rc2 < start_loops)
Zeekat 0:b0e2b38ab272 494 {
Zeekat 0:b0e2b38ab272 495 rc2++;
Zeekat 0:b0e2b38ab272 496 reference2 = phi_two_curr + ((double) rc2/start_loops)*(reference2-phi_two_curr);
Zeekat 0:b0e2b38ab272 497 }
Zeekat 0:b0e2b38ab272 498
Zeekat 0:b0e2b38ab272 499 double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
Zeekat 0:b0e2b38ab272 500 double error2 = (reference2 - rads2); // determine the error (reference - position)
Zeekat 0:b0e2b38ab272 501 double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
Zeekat 0:b0e2b38ab272 502 // check the real angles and stop if exceeded
Zeekat 0:b0e2b38ab272 503 // if(rads2 < limlow2 - 0.05){m_output2 = 0;}
Zeekat 0:b0e2b38ab272 504 // if(rads2 > limhigh2 + 0.05){m_output2 = 0;}
Zeekat 0:b0e2b38ab272 505 m_output2 = outputlimiter(m_output2,1); // final output limit (not really needed, is for safety)
Zeekat 0:b0e2b38ab272 506 if(m_output2 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:b0e2b38ab272 507 motor2_rich.write(0);
Zeekat 0:b0e2b38ab272 508 motor2_aan.write(m_output2);
Zeekat 0:b0e2b38ab272 509 } else if(m_output2 < 0) {
Zeekat 0:b0e2b38ab272 510 motor2_rich.write(1);
Zeekat 0:b0e2b38ab272 511 motor2_aan.write(abs(m_output2));
Zeekat 0:b0e2b38ab272 512 }
Zeekat 0:b0e2b38ab272 513 }
Zeekat 0:b0e2b38ab272 514
Zeekat 0:b0e2b38ab272 515 // calibrate the emg-signal
Zeekat 0:b0e2b38ab272 516 // works bij taking a certain amount of samples taking the max then normalize to the desired value
Zeekat 0:b0e2b38ab272 517 // went to max-value type. must be tested.!
Zeekat 0:b0e2b38ab272 518 void calibrate_amp()
Zeekat 0:b0e2b38ab272 519 {
Zeekat 0:b0e2b38ab272 520 double max1 = 0;
Zeekat 0:b0e2b38ab272 521 double max2 = 0;
Zeekat 0:b0e2b38ab272 522 for(int i = 0; i<cal_samples; i++)
Zeekat 0:b0e2b38ab272 523 {
Zeekat 0:b0e2b38ab272 524 EMG_filter(); // run filter
Zeekat 0:b0e2b38ab272 525 double input1 = output1; // take data from global variable
Zeekat 0:b0e2b38ab272 526 if(input1>max1){max1 = input1;} // take max input
Zeekat 0:b0e2b38ab272 527 double input2 = output2;
Zeekat 0:b0e2b38ab272 528 if(input2>max2){max2 = input2;} // take max input
Zeekat 0:b0e2b38ab272 529 wait(controlstep); // !! has to run at same interval as filter in main loop !! otherwise a 'different' signal will be used for calibration
Zeekat 0:b0e2b38ab272 530 }
Zeekat 0:b0e2b38ab272 531 emg_gain1 = normalize_emg_value/max1; // normalize the amplification so that the maximum signal hits the desired one
Zeekat 0:b0e2b38ab272 532 emg_gain2 = normalize_emg_value/max2;
Zeekat 0:b0e2b38ab272 533 pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2); // print the calculated gains to putty
Zeekat 0:b0e2b38ab272 534
Zeekat 0:b0e2b38ab272 535 }
Zeekat 0:b0e2b38ab272 536 //////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 537 //////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
Zeekat 0:b0e2b38ab272 538 ////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 539
Zeekat 0:b0e2b38ab272 540
Zeekat 0:b0e2b38ab272 541 void EMG_activate(){emg_go = true;}
Zeekat 0:b0e2b38ab272 542 void angle_activate(){cart_go = true;}
Zeekat 0:b0e2b38ab272 543 void motor1_activate(){motor1_go = true;}
Zeekat 0:b0e2b38ab272 544 void motor2_activate(){motor2_go = true;}
Zeekat 0:b0e2b38ab272 545
Zeekat 0:b0e2b38ab272 546 int main()
Zeekat 0:b0e2b38ab272 547 {
Zeekat 0:b0e2b38ab272 548 pc.baud(115200);
Zeekat 0:b0e2b38ab272 549 main_filter.attach(&EMG_activate, EMG_step);
Zeekat 0:b0e2b38ab272 550 cartesian.attach(&angle_activate, controlstep);
Zeekat 0:b0e2b38ab272 551 controller1.attach(&motor1_activate, controlstep); // call a go-flag
Zeekat 0:b0e2b38ab272 552 controller2.attach(&motor2_activate, controlstep);
Zeekat 0:b0e2b38ab272 553 send.attach(&mod_send, 1); // send data (only global variables) to putty
Zeekat 0:b0e2b38ab272 554 while(true)
Zeekat 0:b0e2b38ab272 555 {
Zeekat 0:b0e2b38ab272 556 // button press functions
Zeekat 0:b0e2b38ab272 557 // flow buttons
Zeekat 0:b0e2b38ab272 558 if(buttonlinks.read() == 0)
Zeekat 0:b0e2b38ab272 559 {
Zeekat 0:b0e2b38ab272 560 loop_start = !loop_start;
Zeekat 1:4c9994ac229c 561 wait(0.2);
Zeekat 1:4c9994ac229c 562 while(buttonlinks.read() == 0);
Zeekat 0:b0e2b38ab272 563 }
Zeekat 0:b0e2b38ab272 564 if(buttonrechts.read() == 0)
Zeekat 0:b0e2b38ab272 565 {
Zeekat 0:b0e2b38ab272 566 calib_start = !calib_start;
Zeekat 1:4c9994ac229c 567 wait(0.2);
Zeekat 1:4c9994ac229c 568 while(buttonrechts.read() == 0);
Zeekat 0:b0e2b38ab272 569 }
Zeekat 0:b0e2b38ab272 570 // reverse buttons
Zeekat 0:b0e2b38ab272 571 if(switch_xy_button.read() == 0)
Zeekat 0:b0e2b38ab272 572 {
Zeekat 0:b0e2b38ab272 573 switch_xy = !switch_xy;
Zeekat 0:b0e2b38ab272 574 led_right.write(!led_right.read()); // turn on led when switched to y control
Zeekat 1:4c9994ac229c 575 wait(0.2);
Zeekat 1:4c9994ac229c 576 while(switch_xy_button.read() == 0);
Zeekat 0:b0e2b38ab272 577 }
Zeekat 0:b0e2b38ab272 578 if(reset_button.read() == 0)
Zeekat 0:b0e2b38ab272 579 {
Zeekat 0:b0e2b38ab272 580 reset = !reset;
Zeekat 0:b0e2b38ab272 581 phi_one_curr = phi_one;
Zeekat 0:b0e2b38ab272 582 phi_two_curr = phi_two;
Zeekat 0:b0e2b38ab272 583 rc1 = 0;
Zeekat 0:b0e2b38ab272 584 rc2 = 0;
Zeekat 0:b0e2b38ab272 585 wait(0.2);
Zeekat 1:4c9994ac229c 586 while(reset_button.read() == 0);
Zeekat 0:b0e2b38ab272 587
Zeekat 0:b0e2b38ab272 588 }
Zeekat 0:b0e2b38ab272 589 //////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 590 // Main Control stuff and options
Zeekat 0:b0e2b38ab272 591 if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops
Zeekat 0:b0e2b38ab272 592 {
Zeekat 0:b0e2b38ab272 593 LED(1,1,0); // turn blue led on
Zeekat 0:b0e2b38ab272 594 if(cart_go) { cart_go = false; det_angles();}
Zeekat 0:b0e2b38ab272 595 if(emg_go) { emg_go = false; EMG_filter();}
Zeekat 0:b0e2b38ab272 596 if(motor1_go) { motor1_go = false; motor1_control();}
Zeekat 0:b0e2b38ab272 597 if(motor2_go) { motor2_go = false; motor2_control();}
Zeekat 0:b0e2b38ab272 598 }
Zeekat 0:b0e2b38ab272 599 // shut off both motors
Zeekat 0:b0e2b38ab272 600 if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);}
Zeekat 0:b0e2b38ab272 601
Zeekat 0:b0e2b38ab272 602 // turn green led on // start calibration procedures
Zeekat 0:b0e2b38ab272 603 if(loop_start == false && calib_start == true)
Zeekat 0:b0e2b38ab272 604 { LED(1,0,1);
Zeekat 0:b0e2b38ab272 605 motor1_aan.write(0);
Zeekat 0:b0e2b38ab272 606 motor2_aan.write(0);
Zeekat 0:b0e2b38ab272 607 calibrate_amp(); // 10 second calibration
Zeekat 0:b0e2b38ab272 608 calib_start = false; // turn calibration mode off
Zeekat 0:b0e2b38ab272 609 }
Zeekat 0:b0e2b38ab272 610
Zeekat 0:b0e2b38ab272 611 // turn red led on (show both buttons have been pressed)
Zeekat 0:b0e2b38ab272 612 if(loop_start == true && calib_start == true)
Zeekat 0:b0e2b38ab272 613 {
Zeekat 0:b0e2b38ab272 614 LED(0,1,1);
Zeekat 0:b0e2b38ab272 615 motor1_aan.write(0);
Zeekat 0:b0e2b38ab272 616 motor2_aan.write(0);
Zeekat 0:b0e2b38ab272 617 }
Zeekat 0:b0e2b38ab272 618
Zeekat 0:b0e2b38ab272 619 // turn leds off (both buttons false)
Zeekat 0:b0e2b38ab272 620 else { LED(1,1,1);}
Zeekat 0:b0e2b38ab272 621 }
Zeekat 0:b0e2b38ab272 622 }