Wouter Bos / Mbed 2 deprecated 005-motor_x-y_control

werkend x-y control

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@9:5140b3a95dc9, 2015-10-23 (annotated)

Committer:
Zeekat
Date:
Fri Oct 23 19:30:39 2015 +0000
Revision:
9:5140b3a95dc9
Parent:
8:649a5f555b7b 
working version. smooth to ref

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