Wouter Bos / Mbed 2 deprecated 005-motor_x-y_control

werkend x-y control

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@6:bfd24400e9d0, 2015-10-22 (annotated)

Committer:
Zeekat
Date:
Thu Oct 22 12:21:06 2015 +0000
Revision:
6:bfd24400e9d0
Parent:
5:867fe891b990
Child:
7:84abed2f376c
last version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zeekat 0:a643b1b38abe 1 #include "mbed.h"
Zeekat 0:a643b1b38abe 2 #include "MODSERIAL.h"
Zeekat 0:a643b1b38abe 3 #include "encoder.h"
Zeekat 0:a643b1b38abe 4 #include "HIDScope.h"
Zeekat 1:f3910e46b831 5 #include "math.h"
Zeekat 0:a643b1b38abe 6
Zeekat 4:c371fc59749e 7
Zeekat 4:c371fc59749e 8 ///// points..... mooi maken calib
Zeekat 0:a643b1b38abe 9 Serial pc(USBTX,USBRX);
Zeekat 2:867a1eb33bbe 10 HIDScope scope(4); // definieerd het aantal kanalen van de scope
Zeekat 0:a643b1b38abe 11
Zeekat 0:a643b1b38abe 12 // Define Tickers and control frequencies
Zeekat 1:f3910e46b831 13 Ticker controller1, controller2, main_filter, cartesian; // definieer de ticker die controler1 doet
Zeekat 0:a643b1b38abe 14 // Go flag variables
Zeekat 1:f3910e46b831 15 volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false;
Zeekat 0:a643b1b38abe 16
Zeekat 0:a643b1b38abe 17 // Frequency control
Zeekat 0:a643b1b38abe 18 double controlfreq = 50 ; // controlloops frequentie (Hz)
Zeekat 0:a643b1b38abe 19 double controlstep = 1/controlfreq; // timestep derived from controlfreq
Zeekat 0:a643b1b38abe 20
Zeekat 0:a643b1b38abe 21
Zeekat 0:a643b1b38abe 22 //MOTOR OUTPUTPINS
Zeekat 0:a643b1b38abe 23 // motor 1
Zeekat 0:a643b1b38abe 24 PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets)
Zeekat 0:a643b1b38abe 25 DigitalOut motor1_rich(D7); // digitaal signaal voor richting
Zeekat 0:a643b1b38abe 26 // motor 2
Zeekat 0:a643b1b38abe 27 PwmOut motor2_aan(D5);
Zeekat 0:a643b1b38abe 28 DigitalOut motor2_rich(D4);
Zeekat 0:a643b1b38abe 29
Zeekat 0:a643b1b38abe 30 // ENCODER INPUTPINS
Zeekat 0:a643b1b38abe 31 Encoder motor1_enc(D12,D11); // encoder outputpins
Zeekat 0:a643b1b38abe 32 Encoder motor2_enc(D10,D9);
Zeekat 0:a643b1b38abe 33
Zeekat 1:f3910e46b831 34 ////////
Zeekat 1:f3910e46b831 35 // physical constants
Zeekat 1:f3910e46b831 36 const double L = 36; // lenght of arms
Zeekat 1:f3910e46b831 37 const double pi = 3.1415926535897;
Zeekat 1:f3910e46b831 38 ///////////////////////////
Zeekat 1:f3910e46b831 39 // Main control loop transfer variables
Zeekat 1:f3910e46b831 40 // here all variables that transfer bewtween the primary control functions
Zeekat 5:867fe891b990 41
Zeekat 5:867fe891b990 42 // filter to calibration
Zeekat 1:f3910e46b831 43 double output1;
Zeekat 1:f3910e46b831 44 double output2;
Zeekat 1:f3910e46b831 45
Zeekat 5:867fe891b990 46 // filter to x-y
Zeekat 5:867fe891b990 47 double output1_amp;
Zeekat 5:867fe891b990 48 double output2_amp;
Zeekat 5:867fe891b990 49
Zeekat 5:867fe891b990 50 // x-y to motor control
Zeekat 2:867a1eb33bbe 51 double phi_one;
Zeekat 2:867a1eb33bbe 52 double phi_two;
Zeekat 1:f3910e46b831 53
Zeekat 0:a643b1b38abe 54
Zeekat 0:a643b1b38abe 55 // EXTRA INPUTS AND REQUIRED VARIABLES
Zeekat 0:a643b1b38abe 56 //POTMETERS
Zeekat 3:a73502236647 57 AnalogIn potright(A4); // define the potmeter outputpins
Zeekat 3:a73502236647 58 AnalogIn potleft(A5);
Zeekat 1:f3910e46b831 59
Zeekat 1:f3910e46b831 60 // Analoge input signalen defineren
Zeekat 4:c371fc59749e 61 AnalogIn EMG_in(A0); // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
Zeekat 4:c371fc59749e 62 AnalogIn EMG_int(A2); // deze leest A3 uit
Zeekat 0:a643b1b38abe 63
Zeekat 0:a643b1b38abe 64 // BUTTONS
Zeekat 0:a643b1b38abe 65 // control flow
Zeekat 0:a643b1b38abe 66 DigitalIn buttonlinks(PTA4);
Zeekat 0:a643b1b38abe 67 DigitalIn buttonrechts(PTC6);
Zeekat 0:a643b1b38abe 68 // init values
Zeekat 0:a643b1b38abe 69 bool loop_start = false;
Zeekat 0:a643b1b38abe 70 bool calib_start = false;
Zeekat 0:a643b1b38abe 71
Zeekat 0:a643b1b38abe 72 // direction control
Zeekat 0:a643b1b38abe 73 DigitalIn reverse_button_links(D0);
Zeekat 0:a643b1b38abe 74 DigitalIn reverse_button_rechts(D1);
Zeekat 0:a643b1b38abe 75 // init values
Zeekat 0:a643b1b38abe 76 bool reverse_links = false;
Zeekat 0:a643b1b38abe 77 bool reverse_rechts = false;
Zeekat 3:a73502236647 78
Zeekat 3:a73502236647 79
Zeekat 0:a643b1b38abe 80 // LED
Zeekat 0:a643b1b38abe 81 DigitalOut ledred(LED1);
Zeekat 0:a643b1b38abe 82 DigitalOut ledgreen(LED2);
Zeekat 0:a643b1b38abe 83 DigitalOut ledblue(LED3);
Zeekat 0:a643b1b38abe 84
Zeekat 0:a643b1b38abe 85 // REFERENCE SIGNAL SETTINGS
Zeekat 0:a643b1b38abe 86 double input_threshold = 0.25; // the minimum value the signal must have to change the reference.
Zeekat 0:a643b1b38abe 87 // Define storage variables for reference values
Zeekat 0:a643b1b38abe 88 double c_reference1 = 0;
Zeekat 0:a643b1b38abe 89 double c_reference2 = 0;
Zeekat 3:a73502236647 90
Zeekat 3:a73502236647 91 // define start up variables
Zeekat 3:a73502236647 92 double start_up_time = 2;
Zeekat 3:a73502236647 93 double start_loops = start_up_time*controlfreq;
Zeekat 3:a73502236647 94 int rc1 = 0;
Zeekat 3:a73502236647 95 int rc2 = 0;
Zeekat 3:a73502236647 96
Zeekat 0:a643b1b38abe 97 // limit angles (in radians)
Zeekat 0:a643b1b38abe 98 // motor1
Zeekat 3:a73502236647 99 const double limlow1 = 1; // min height in rad
Zeekat 3:a73502236647 100 const double limhigh1 = 6.3; // max height in rad
Zeekat 0:a643b1b38abe 101 // motor 2
Zeekat 3:a73502236647 102 const double limlow2 = -4.0; // maximum height, motor has been inverted due to transmission
Zeekat 3:a73502236647 103 const double limhigh2 = 2.5; // minimum height in rad
Zeekat 0:a643b1b38abe 104
Zeekat 0:a643b1b38abe 105 // Define the maximum rate of change for the reference (velocity)
Zeekat 0:a643b1b38abe 106 double Vmax = 3; // rad/sec
Zeekat 0:a643b1b38abe 107
Zeekat 0:a643b1b38abe 108 // CONTROLLER SETTINGS
Zeekat 0:a643b1b38abe 109 // motor 1
Zeekat 0:a643b1b38abe 110 const double m1_Kp = 5; // Proportional constant
Zeekat 0:a643b1b38abe 111 const double m1_Ki = 0.5; // integration constant
Zeekat 0:a643b1b38abe 112 const double m1_Kd = 0.4; // differentiation constant
Zeekat 0:a643b1b38abe 113 // motor 2
Zeekat 0:a643b1b38abe 114 const double m2_Kp = 2;
Zeekat 2:867a1eb33bbe 115 const double m2_Ki = 0.5;
Zeekat 0:a643b1b38abe 116 const double m2_Kd = 0.1;
Zeekat 0:a643b1b38abe 117 // storage variables
Zeekat 0:a643b1b38abe 118 // motor 1
Zeekat 0:a643b1b38abe 119 double m1_err_int = 0;
Zeekat 0:a643b1b38abe 120 double m1_prev_err = 0;
Zeekat 0:a643b1b38abe 121 // motor 2
Zeekat 0:a643b1b38abe 122 double m2_err_int = 0;
Zeekat 0:a643b1b38abe 123 double m2_prev_err = 0;
Zeekat 0:a643b1b38abe 124
Zeekat 3:a73502236647 125 // EMG calibration variables
Zeekat 6:bfd24400e9d0 126 double emg_gain1 = 7; // set to one for unamplified signal
Zeekat 6:bfd24400e9d0 127 double emg_gain2 = 7;
Zeekat 4:c371fc59749e 128
Zeekat 6:bfd24400e9d0 129 double cal_samples = 125;
Zeekat 5:867fe891b990 130 double normalize_emg_value = 1; // set te desired value to calibrate the signal to
Zeekat 0:a643b1b38abe 131
Zeekat 0:a643b1b38abe 132 //// FILTER VARIABLES
Zeekat 0:a643b1b38abe 133 // storage variables
Zeekat 0:a643b1b38abe 134 // differential action filter, same is used for both controllers
Zeekat 0:a643b1b38abe 135 double m_f_v1 = 0, m_f_v2 = 0;
Zeekat 0:a643b1b38abe 136 // input filter to smooth signal
Zeekat 0:a643b1b38abe 137 double r1_f_v1 = 0, r1_f_v2 = 0;
Zeekat 0:a643b1b38abe 138 double r2_f_v1 = 0, r2_f_v2 = 0;
Zeekat 0:a643b1b38abe 139
Zeekat 0:a643b1b38abe 140 // Filter coefficients
Zeekat 0:a643b1b38abe 141 // differential action filter (lowpass 5Hz at 50samples)
Zeekat 0:a643b1b38abe 142 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; // coefficients from sheets are used as first test.
Zeekat 0:a643b1b38abe 143 // input filter (lowpass 1Hz at 50samples)
Zeekat 0:a643b1b38abe 144 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:a643b1b38abe 145
Zeekat 0:a643b1b38abe 146 // tweede orde notch filter 50 Hz
Zeekat 0:a643b1b38abe 147 // biquad 1 coefficienten
Zeekat 0:a643b1b38abe 148 const double numnotch50biq1_1 = 1;
Zeekat 0:a643b1b38abe 149 const double numnotch50biq1_2 = -1.61816178466632;
Zeekat 0:a643b1b38abe 150 const double numnotch50biq1_3 = 1.00000006127058;
Zeekat 0:a643b1b38abe 151 const double dennotch50biq1_2 = -1.59325742941798;
Zeekat 0:a643b1b38abe 152 const double dennotch50biq1_3 = 0.982171881701431;
Zeekat 0:a643b1b38abe 153 // biquad 2 coefficienten
Zeekat 0:a643b1b38abe 154 const double numnotch50biq2_1 = 1;
Zeekat 0:a643b1b38abe 155 const double numnotch50biq2_2 = -1.61816171933244;
Zeekat 0:a643b1b38abe 156 const double numnotch50biq2_3 = 0.999999938729428;
Zeekat 0:a643b1b38abe 157 const double dennotch50biq2_2 = -1.61431180968071;
Zeekat 0:a643b1b38abe 158 const double dennotch50biq2_3 = 0.982599066293075;
Zeekat 0:a643b1b38abe 159 // highpass filter 20 Hz coefficienten
Zeekat 0:a643b1b38abe 160 const double numhigh20_1 = 0.837089190566345;
Zeekat 0:a643b1b38abe 161 const double numhigh20_2 = -1.67417838113269;
Zeekat 0:a643b1b38abe 162 const double numhigh20_3 = 0.837089190566345;
Zeekat 0:a643b1b38abe 163 const double denhigh20_2 = -1.64745998107698;
Zeekat 0:a643b1b38abe 164 const double denhigh20_3 = 0.700896781188403;
Zeekat 0:a643b1b38abe 165 // lowpass 5 Hz coefficienten
Zeekat 0:a643b1b38abe 166 const double numlow5_1 =0.000944691843840162;
Zeekat 0:a643b1b38abe 167 const double numlow5_2 =0.00188938368768032;
Zeekat 0:a643b1b38abe 168 const double numlow5_3 =0.000944691843840162;
Zeekat 0:a643b1b38abe 169 const double denlow5_2 =-1.91119706742607;
Zeekat 0:a643b1b38abe 170 const double denlow5_3 =0.914975834801434;
Zeekat 0:a643b1b38abe 171
Zeekat 1:f3910e46b831 172 // Define the storage variables and filter coeficients for eight filters
Zeekat 1:f3910e46b831 173 //filter 1
Zeekat 1:f3910e46b831 174 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 1:f3910e46b831 175 // filter2
Zeekat 1:f3910e46b831 176 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:a643b1b38abe 177
Zeekat 0:a643b1b38abe 178 ////////////////////////////////////////////////////////////////
Zeekat 0:a643b1b38abe 179 /////////////////// START OF SIDE FUNCTIONS ////////////////////
Zeekat 0:a643b1b38abe 180 //////////////////////////////////////////////////////////////
Zeekat 0:a643b1b38abe 181 // these functions are tailored to perform 1 specific function
Zeekat 0:a643b1b38abe 182
Zeekat 0:a643b1b38abe 183 // this funtion flips leds on and off accordin to input with 0 being on
Zeekat 0:a643b1b38abe 184 void LED(int red,int green,int blue)
Zeekat 0:a643b1b38abe 185 {
Zeekat 0:a643b1b38abe 186 ledred.write(red);
Zeekat 0:a643b1b38abe 187 ledgreen.write(green);
Zeekat 0:a643b1b38abe 188 ledblue.write(blue);
Zeekat 0:a643b1b38abe 189 }
Zeekat 0:a643b1b38abe 190
Zeekat 0:a643b1b38abe 191 // counts 2 radians
Zeekat 0:a643b1b38abe 192 // this function takes counts from the encoder and converts it to the amount of radians from the zero position.
Zeekat 0:a643b1b38abe 193 // It has been set up for standard 2X DECODING!!!
Zeekat 0:a643b1b38abe 194 double get_radians(double counts)
Zeekat 0:a643b1b38abe 195 {
Zeekat 0:a643b1b38abe 196 double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, last warning)
Zeekat 0:a643b1b38abe 197 return radians;
Zeekat 0:a643b1b38abe 198 }
Zeekat 0:a643b1b38abe 199
Zeekat 0:a643b1b38abe 200
Zeekat 0:a643b1b38abe 201 // This functions takes a 0->1 input, uses passing by reference (&c_reference)
Zeekat 0:a643b1b38abe 202 // to create a reference that moves with a variable speed. It is meant for 0->1 values
Zeekat 0:a643b1b38abe 203 double reference_f(double input, double &c_reference, double limlow, double limhigh)
Zeekat 0:a643b1b38abe 204 {
Zeekat 0:a643b1b38abe 205 double reference = c_reference + input * controlstep * Vmax ;
Zeekat 0:a643b1b38abe 206 // two if statements check if the reference exceeds the limits placed upon the arms
Zeekat 0:a643b1b38abe 207 if(reference < limlow){reference = limlow;}
Zeekat 0:a643b1b38abe 208 if(reference > limhigh){reference = limhigh;}
Zeekat 0:a643b1b38abe 209 c_reference = reference; // change the global variable to the latest location.
Zeekat 0:a643b1b38abe 210 return reference;
Zeekat 0:a643b1b38abe 211 }
Zeekat 0:a643b1b38abe 212
Zeekat 0:a643b1b38abe 213
Zeekat 0:a643b1b38abe 214 // This function takes the controller outputvalue and ensures it is between -1 and 1
Zeekat 0:a643b1b38abe 215 // 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:a643b1b38abe 216 double outputlimiter (double output, double limit)
Zeekat 0:a643b1b38abe 217 {
Zeekat 0:a643b1b38abe 218 if(output> limit)
Zeekat 0:a643b1b38abe 219 {
Zeekat 0:a643b1b38abe 220 output = 1;
Zeekat 0:a643b1b38abe 221 }
Zeekat 0:a643b1b38abe 222 else if(output < limit && output > 0)
Zeekat 0:a643b1b38abe 223 {
Zeekat 0:a643b1b38abe 224 output = output;
Zeekat 0:a643b1b38abe 225 }
Zeekat 0:a643b1b38abe 226 else if(output > -limit && output < 0)
Zeekat 0:a643b1b38abe 227 {
Zeekat 0:a643b1b38abe 228 output = output;
Zeekat 0:a643b1b38abe 229 }
Zeekat 0:a643b1b38abe 230 else if(output < -limit)
Zeekat 0:a643b1b38abe 231 {
Zeekat 0:a643b1b38abe 232 (output = -1);
Zeekat 0:a643b1b38abe 233 }
Zeekat 0:a643b1b38abe 234 return output;
Zeekat 0:a643b1b38abe 235 }
Zeekat 0:a643b1b38abe 236
Zeekat 0:a643b1b38abe 237
Zeekat 0:a643b1b38abe 238 // BIQUADFILTER CODE GIVEN IN SHEETS
Zeekat 0:a643b1b38abe 239 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:a643b1b38abe 240 {
Zeekat 0:a643b1b38abe 241 double v = u - a1*v1 - a2*v2;
Zeekat 0:a643b1b38abe 242 double y = b0*v + b1*v1 + b2*v2;
Zeekat 0:a643b1b38abe 243 v2 = v1;
Zeekat 0:a643b1b38abe 244 v1 = v;
Zeekat 0:a643b1b38abe 245 return y;
Zeekat 0:a643b1b38abe 246 }
Zeekat 0:a643b1b38abe 247
Zeekat 1:f3910e46b831 248 // biquadfilters die bij het filteren van signaal 2 horen, copy paste, alle waardes zijn veranderd naar +t (t van two of twee)
Zeekat 5:867fe891b990 249 // (niet netjes maar werkt goed)
Zeekat 1:f3910e46b831 250 double biquadfiltert(double ut, double &v1t, double &v2t, const double a1t, const double a2t, const double b0t, const double b1t, const double b2t)
Zeekat 0:a643b1b38abe 251 {
Zeekat 1:f3910e46b831 252 double vt = ut- a1t*v1t-a2t*v2t;
Zeekat 1:f3910e46b831 253 double yt = b0t*vt+b1t*v1t+b2t*v2t;
Zeekat 1:f3910e46b831 254 v2t = v1t;
Zeekat 1:f3910e46b831 255 v1t = vt;
Zeekat 1:f3910e46b831 256 return yt;
Zeekat 0:a643b1b38abe 257 }
Zeekat 0:a643b1b38abe 258
Zeekat 0:a643b1b38abe 259 // PID Controller given in sheets
Zeekat 5:867fe891b990 260 // aangepast om zelfde filter te gebruiken en om de termen te splitsen
Zeekat 0:a643b1b38abe 261 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
Zeekat 0:a643b1b38abe 262 {
Zeekat 0:a643b1b38abe 263 // Proportional
Zeekat 0:a643b1b38abe 264 double P = Kp * e;
Zeekat 0:a643b1b38abe 265 // Integral
Zeekat 0:a643b1b38abe 266 e_int = e_int + Ts * e;
Zeekat 0:a643b1b38abe 267 double I = e_int * Ki;
Zeekat 0:a643b1b38abe 268 // Derivative
Zeekat 0:a643b1b38abe 269 double e_derr = (e - e_prev)/Ts;
Zeekat 0:a643b1b38abe 270 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:a643b1b38abe 271 //
Zeekat 0:a643b1b38abe 272 e_prev = e;
Zeekat 0:a643b1b38abe 273 double D = Kd* e_derr;
Zeekat 0:a643b1b38abe 274 // PID
Zeekat 0:a643b1b38abe 275 double output = P + I + D;
Zeekat 0:a643b1b38abe 276 return output;
Zeekat 0:a643b1b38abe 277 }
Zeekat 3:a73502236647 278
Zeekat 5:867fe891b990 279 double angle_limits(double phi, double limlow, double limhigh)
Zeekat 3:a73502236647 280 {
Zeekat 5:867fe891b990 281 if(phi < limlow)
Zeekat 5:867fe891b990 282 {
Zeekat 5:867fe891b990 283 phi = limlow;
Zeekat 5:867fe891b990 284 }
Zeekat 5:867fe891b990 285 if(phi > limhigh)
Zeekat 5:867fe891b990 286 {
Zeekat 5:867fe891b990 287 phi = limhigh;
Zeekat 5:867fe891b990 288 }
Zeekat 5:867fe891b990 289 return phi;
Zeekat 5:867fe891b990 290 }
Zeekat 3:a73502236647 291
Zeekat 0:a643b1b38abe 292 /////////////////////////////////////////////////////////////////////
Zeekat 0:a643b1b38abe 293 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
Zeekat 0:a643b1b38abe 294 ///////////////////////////////////////////////////////////////////
Zeekat 1:f3910e46b831 295 // these functions are called by go-flags and are used to update main variables and send signals to motor
Zeekat 0:a643b1b38abe 296
Zeekat 5:867fe891b990 297 // function that updates the inputs
Zeekat 5:867fe891b990 298 void EMG_filter()
Zeekat 5:867fe891b990 299 {
Zeekat 5:867fe891b990 300 // filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
Zeekat 5:867fe891b990 301 double u1 = EMG_in.read();
Zeekat 5:867fe891b990 302 double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
Zeekat 5:867fe891b990 303 double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
Zeekat 5:867fe891b990 304 double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
Zeekat 5:867fe891b990 305 double y4 = abs(y3);
Zeekat 5:867fe891b990 306 double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
Zeekat 5:867fe891b990 307 // update global variables
Zeekat 5:867fe891b990 308 output1 = y5;
Zeekat 5:867fe891b990 309 output1_amp = y5*emg_gain1; // update global variable
Zeekat 5:867fe891b990 310
Zeekat 5:867fe891b990 311 // filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
Zeekat 5:867fe891b990 312 double u1t = EMG_int.read();
Zeekat 5:867fe891b990 313 double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
Zeekat 5:867fe891b990 314 double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
Zeekat 5:867fe891b990 315 double y3t = biquadfiltert( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
Zeekat 5:867fe891b990 316 double y4t = abs(y3t);
Zeekat 5:867fe891b990 317 double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
Zeekat 5:867fe891b990 318 // update global variables
Zeekat 5:867fe891b990 319 output2 = y5t;
Zeekat 5:867fe891b990 320 output2_amp = y5t*emg_gain2;
Zeekat 5:867fe891b990 321
Zeekat 6:bfd24400e9d0 322 scope.set(0,output1);
Zeekat 6:bfd24400e9d0 323 scope.set(1,output2);
Zeekat 6:bfd24400e9d0 324 scope.set(2,output1_amp);
Zeekat 6:bfd24400e9d0 325 scope.set(3,output2_amp);
Zeekat 5:867fe891b990 326 scope.send();
Zeekat 5:867fe891b990 327 }
Zeekat 5:867fe891b990 328
Zeekat 5:867fe891b990 329
Zeekat 5:867fe891b990 330 // function that updates the required motor angles
Zeekat 5:867fe891b990 331 void det_angles()
Zeekat 5:867fe891b990 332 {
Zeekat 5:867fe891b990 333
Zeekat 6:bfd24400e9d0 334 if(output1_amp>1) {output1_amp = 1;}
Zeekat 6:bfd24400e9d0 335 if(output2_amp>1) {output2_amp = 1;}
Zeekat 5:867fe891b990 336
Zeekat 6:bfd24400e9d0 337 //output1 = potright.read();
Zeekat 6:bfd24400e9d0 338 //output2 = potleft.read();
Zeekat 5:867fe891b990 339
Zeekat 5:867fe891b990 340 double xx = 50+output1_amp*20;
Zeekat 5:867fe891b990 341
Zeekat 6:bfd24400e9d0 342 double ymin = -16; //- sqrt(4900 - pow(xx,2));
Zeekat 6:bfd24400e9d0 343 double ymax = 16; //sqrt(4900 - pow(xx,2));
Zeekat 5:867fe891b990 344 double yy = ymin+output2_amp*(ymax-ymin);
Zeekat 5:867fe891b990 345 double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
Zeekat 5:867fe891b990 346 double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
Zeekat 5:867fe891b990 347 double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
Zeekat 5:867fe891b990 348
Zeekat 5:867fe891b990 349 double theta_one = (atan2(yy,xx)+beta);
Zeekat 5:867fe891b990 350 double theta_two = (-pi + alfa);
Zeekat 5:867fe891b990 351
Zeekat 5:867fe891b990 352 double phi1 = 4*(theta_one) + 2.8;
Zeekat 5:867fe891b990 353 double phi2 = 4*(theta_one+theta_two) + 1.85;
Zeekat 5:867fe891b990 354 phi2 = -phi2;
Zeekat 5:867fe891b990 355
Zeekat 5:867fe891b990 356 // check the input angles and apply the limits
Zeekat 5:867fe891b990 357 phi1 = angle_limits(phi1,limlow1,limhigh1);
Zeekat 5:867fe891b990 358 phi2 = angle_limits(phi2,limlow2,limhigh2);
Zeekat 5:867fe891b990 359
Zeekat 5:867fe891b990 360 // smooth the input signal (lowpass 1Hz)
Zeekat 5:867fe891b990 361 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 5:867fe891b990 362 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 5:867fe891b990 363
Zeekat 5:867fe891b990 364 // write into global variables
Zeekat 5:867fe891b990 365 phi_one = phi1;
Zeekat 5:867fe891b990 366 phi_two = phi2;
Zeekat 5:867fe891b990 367
Zeekat 5:867fe891b990 368 pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two);
Zeekat 5:867fe891b990 369 }
Zeekat 0:a643b1b38abe 370 // MOTOR 1
Zeekat 0:a643b1b38abe 371 void motor1_control()
Zeekat 0:a643b1b38abe 372 {
Zeekat 3:a73502236647 373
Zeekat 3:a73502236647 374 double reference1 = phi_one;
Zeekat 0:a643b1b38abe 375
Zeekat 5:867fe891b990 376 // add smooth start up
Zeekat 3:a73502236647 377 if(rc1 < start_loops)
Zeekat 3:a73502236647 378 {
Zeekat 3:a73502236647 379 rc1++;
Zeekat 3:a73502236647 380 reference1 = ((double) rc1/start_loops)*reference1;
Zeekat 3:a73502236647 381 }
Zeekat 3:a73502236647 382 else
Zeekat 3:a73502236647 383 {
Zeekat 3:a73502236647 384 reference1 = reference1;
Zeekat 3:a73502236647 385 }
Zeekat 5:867fe891b990 386
Zeekat 0:a643b1b38abe 387 double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 0:a643b1b38abe 388 double error1 = (reference1 - rads1); // determine the error (reference - position)
Zeekat 1:f3910e46b831 389 double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
Zeekat 0:a643b1b38abe 390
Zeekat 1:f3910e46b831 391 m_output1 = outputlimiter(m_output1,1); // relimit the output for safety
Zeekat 1:f3910e46b831 392 if(m_output1 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:a643b1b38abe 393 motor1_rich.write(0);
Zeekat 1:f3910e46b831 394 motor1_aan.write(m_output1);
Zeekat 1:f3910e46b831 395 } else if(m_output1 < 0) {
Zeekat 0:a643b1b38abe 396 motor1_rich.write(1);
Zeekat 1:f3910e46b831 397 motor1_aan.write(abs(m_output1));
Zeekat 0:a643b1b38abe 398 }
Zeekat 0:a643b1b38abe 399 }
Zeekat 0:a643b1b38abe 400
Zeekat 0:a643b1b38abe 401 // MOTOR 2
Zeekat 0:a643b1b38abe 402 void motor2_control()
Zeekat 0:a643b1b38abe 403 {
Zeekat 3:a73502236647 404 double reference2 = phi_two;
Zeekat 0:a643b1b38abe 405
Zeekat 3:a73502236647 406 if(rc2 < start_loops)
Zeekat 3:a73502236647 407 {
Zeekat 3:a73502236647 408 rc2++;
Zeekat 3:a73502236647 409 reference2 = ((double) rc2/start_loops)*reference2;
Zeekat 3:a73502236647 410 }
Zeekat 3:a73502236647 411 else
Zeekat 3:a73502236647 412 {
Zeekat 3:a73502236647 413 reference2 = reference2;
Zeekat 3:a73502236647 414 }
Zeekat 3:a73502236647 415
Zeekat 0:a643b1b38abe 416 double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
Zeekat 0:a643b1b38abe 417 double error2 = (reference2 - rads2); // determine the error (reference - position)
Zeekat 1:f3910e46b831 418 double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
Zeekat 0:a643b1b38abe 419
Zeekat 1:f3910e46b831 420 m_output2 = outputlimiter(m_output2,1);
Zeekat 1:f3910e46b831 421 if(m_output2 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:a643b1b38abe 422 motor2_rich.write(0);
Zeekat 1:f3910e46b831 423 motor2_aan.write(m_output2);
Zeekat 1:f3910e46b831 424 } else if(m_output2 < 0) {
Zeekat 0:a643b1b38abe 425 motor2_rich.write(1);
Zeekat 1:f3910e46b831 426 motor2_aan.write(abs(m_output2));
Zeekat 0:a643b1b38abe 427 }
Zeekat 0:a643b1b38abe 428 }
Zeekat 0:a643b1b38abe 429
Zeekat 5:867fe891b990 430 // calibrate the emg-signal
Zeekat 5:867fe891b990 431 // works bij taking a certain amount of samples adding them and then normalize to the desired value
Zeekat 5:867fe891b990 432 void calibrate_amp()
Zeekat 1:f3910e46b831 433 {
Zeekat 5:867fe891b990 434 double total1 = 0;
Zeekat 5:867fe891b990 435 double total2 = 0;
Zeekat 5:867fe891b990 436 for(int i = 0; i<cal_samples; i++)
Zeekat 5:867fe891b990 437 {
Zeekat 5:867fe891b990 438 EMG_filter(); // run filter
Zeekat 5:867fe891b990 439 double input1 = output1;
Zeekat 5:867fe891b990 440 total1 = total1 + input1; // sum inputs
Zeekat 5:867fe891b990 441
Zeekat 5:867fe891b990 442 double input2 = output2;
Zeekat 5:867fe891b990 443 total2 = total2 + input2;
Zeekat 6:bfd24400e9d0 444 wait(0.02);
Zeekat 5:867fe891b990 445 }
Zeekat 5:867fe891b990 446 emg_gain1 = normalize_emg_value/(total1/cal_samples); // normalize the amplification so that the maximum signal hits the desired one
Zeekat 5:867fe891b990 447 emg_gain2 = normalize_emg_value/(total2/cal_samples);
Zeekat 5:867fe891b990 448 pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2);
Zeekat 2:867a1eb33bbe 449
Zeekat 1:f3910e46b831 450 }
Zeekat 0:a643b1b38abe 451 //////////////////////////////////////////////////////////////////
Zeekat 0:a643b1b38abe 452 //////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
Zeekat 0:a643b1b38abe 453 ////////////////////////////////////////////////////////////////
Zeekat 0:a643b1b38abe 454
Zeekat 1:f3910e46b831 455
Zeekat 1:f3910e46b831 456 void EMG_activate()
Zeekat 1:f3910e46b831 457 {
Zeekat 1:f3910e46b831 458 emg_go = true;
Zeekat 1:f3910e46b831 459 }
Zeekat 1:f3910e46b831 460 void angle_activate()
Zeekat 1:f3910e46b831 461 {
Zeekat 1:f3910e46b831 462 cart_go = true;
Zeekat 1:f3910e46b831 463 }
Zeekat 0:a643b1b38abe 464 void motor1_activate()
Zeekat 0:a643b1b38abe 465 {
Zeekat 0:a643b1b38abe 466 motor1_go = true;
Zeekat 0:a643b1b38abe 467 }
Zeekat 0:a643b1b38abe 468 void motor2_activate()
Zeekat 0:a643b1b38abe 469 {
Zeekat 0:a643b1b38abe 470 motor2_go = true;
Zeekat 0:a643b1b38abe 471 }
Zeekat 0:a643b1b38abe 472
Zeekat 0:a643b1b38abe 473 int main()
Zeekat 0:a643b1b38abe 474 {
Zeekat 0:a643b1b38abe 475 pc.baud(115200);
Zeekat 4:c371fc59749e 476 main_filter.attach(&EMG_activate, controlstep);
Zeekat 1:f3910e46b831 477 cartesian.attach(&angle_activate, controlstep);
Zeekat 0:a643b1b38abe 478 controller1.attach(&motor1_activate, controlstep); // call a go-flag
Zeekat 0:a643b1b38abe 479 controller2.attach(&motor2_activate, controlstep);
Zeekat 0:a643b1b38abe 480 while(true)
Zeekat 0:a643b1b38abe 481 {
Zeekat 0:a643b1b38abe 482 // button press functions
Zeekat 0:a643b1b38abe 483 // flow buttons
Zeekat 0:a643b1b38abe 484 if(buttonlinks.read() == 0)
Zeekat 0:a643b1b38abe 485 {
Zeekat 0:a643b1b38abe 486 loop_start = !loop_start;
Zeekat 0:a643b1b38abe 487 wait(buttonlinks.read() == 1);
Zeekat 0:a643b1b38abe 488 wait(0.3);
Zeekat 0:a643b1b38abe 489 }
Zeekat 0:a643b1b38abe 490 if(buttonrechts.read() == 0)
Zeekat 0:a643b1b38abe 491 {
Zeekat 0:a643b1b38abe 492 calib_start = !calib_start;
Zeekat 0:a643b1b38abe 493 wait(buttonrechts.read() == 1);
Zeekat 0:a643b1b38abe 494 wait(0.3);
Zeekat 0:a643b1b38abe 495 }
Zeekat 0:a643b1b38abe 496 // reverse buttons
Zeekat 0:a643b1b38abe 497 if(reverse_button_links.read() == 0)
Zeekat 0:a643b1b38abe 498 {
Zeekat 0:a643b1b38abe 499 reverse_links = !reverse_links;
Zeekat 0:a643b1b38abe 500 wait(reverse_button_links.read() == 1);
Zeekat 0:a643b1b38abe 501 wait(0.3);
Zeekat 0:a643b1b38abe 502 }
Zeekat 0:a643b1b38abe 503 if(reverse_button_rechts.read() == 0)
Zeekat 0:a643b1b38abe 504 {
Zeekat 0:a643b1b38abe 505 reverse_rechts = !reverse_rechts;
Zeekat 0:a643b1b38abe 506 wait(reverse_button_rechts.read() == 1);
Zeekat 0:a643b1b38abe 507 wait(0.3);
Zeekat 0:a643b1b38abe 508 }
Zeekat 3:a73502236647 509
Zeekat 0:a643b1b38abe 510 //////////////////////////////////////////////////
Zeekat 0:a643b1b38abe 511 // Main Control stuff and options
Zeekat 0:a643b1b38abe 512 if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops
Zeekat 0:a643b1b38abe 513 {
Zeekat 0:a643b1b38abe 514 LED(1,1,0); // turn blue led on
Zeekat 1:f3910e46b831 515 if(cart_go) { cart_go = false; det_angles();}
Zeekat 1:f3910e46b831 516 if(emg_go) { emg_go = false; EMG_filter();}
Zeekat 0:a643b1b38abe 517 if(motor1_go) { motor1_go = false; motor1_control();}
Zeekat 0:a643b1b38abe 518 if(motor2_go) { motor2_go = false; motor2_control();}
Zeekat 0:a643b1b38abe 519 }
Zeekat 0:a643b1b38abe 520 // shut off both motors
Zeekat 0:a643b1b38abe 521 if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);}
Zeekat 0:a643b1b38abe 522
Zeekat 0:a643b1b38abe 523 // turn green led on // start calibration procedures
Zeekat 3:a73502236647 524 if(loop_start == false && calib_start == true)
Zeekat 3:a73502236647 525 { LED(1,0,1);
Zeekat 3:a73502236647 526 motor1_aan.write(0);
Zeekat 3:a73502236647 527 motor2_aan.write(0);
Zeekat 3:a73502236647 528 calibrate_amp(); // 10 second calibration
Zeekat 3:a73502236647 529 calib_start = false; // turn fork off
Zeekat 3:a73502236647 530 }
Zeekat 0:a643b1b38abe 531
Zeekat 0:a643b1b38abe 532 // turn red led on
Zeekat 3:a73502236647 533 if(loop_start == true && calib_start == true)
Zeekat 3:a73502236647 534 {
Zeekat 3:a73502236647 535 LED(0,1,1);
Zeekat 3:a73502236647 536 motor1_aan.write(0);
Zeekat 3:a73502236647 537 motor2_aan.write(0);
Zeekat 3:a73502236647 538 // switch_type = !switch_type;
Zeekat 3:a73502236647 539 }
Zeekat 0:a643b1b38abe 540
Zeekat 0:a643b1b38abe 541 // turn leds off (both buttons false)
Zeekat 0:a643b1b38abe 542 else { LED(1,1,1);}
Zeekat 0:a643b1b38abe 543 }
Zeekat 0:a643b1b38abe 544 }