werkend x-y control
Dependencies: Encoder HIDScope MODSERIAL mbed
main.cpp
- Committer:
- Zeekat
- Date:
- 2015-10-23
- Revision:
- 9:5140b3a95dc9
- Parent:
- 8:649a5f555b7b
File content as of revision 9:5140b3a95dc9:
#include "mbed.h" #include "MODSERIAL.h" #include "encoder.h" #include "HIDScope.h" #include "math.h" Serial pc(USBTX,USBRX); // MODSERIAL output HIDScope scope(4); // definieerd het aantal kanalen van de scope // Define Tickers and control frequencies Ticker controller1, controller2, main_filter, cartesian, send; // Go flag variables belonging to Tickers volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false; // Frequency control double controlfreq = 50 ; // controlloops frequentie (Hz) double controlstep = 1/controlfreq; // timestep derived from controlfreq //////////////////////// IN-OUTPUT ///////////////////////////////////////////// //MOTOR OUTPUTPINS PwmOut motor1_aan(D6), motor2_aan(D5); // PWM signaal motor 2 (uit sheets) DigitalOut motor1_rich(D7), motor2_rich(D4); // digitaal signaal voor richting // ENCODER INPUTPINS Encoder motor1_enc(D12,D11), motor2_enc(D10,D9); // encoder outputpins // EXTRA INPUTS AND REQUIRED VARIABLES //POTMETERS (used for debuggin purposes, not reliable anymore) AnalogIn potright(A4); // define the potmeter outputpins AnalogIn potleft(A5); // Analoge input signalen defineren AnalogIn EMG_in(A0); // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen AnalogIn EMG_int(A2); // deze leest A3 uit // BUTTONS // control flow DigitalIn buttonlinks(PTA4); DigitalIn buttonrechts(PTC6); DigitalIn reset_button(D1); // init values bool loop_start = false; bool calib_start = false; bool reset = false; // axis control DigitalIn switch_xy_button(D0); // init values bool switch_xy = false; // LED outputs on bioshield DigitalOut led_right(D2); // LED outputs on dev-board DigitalOut ledred(LED1); DigitalOut ledgreen(LED2); DigitalOut ledblue(LED3); ////////////////////////////////////////////////////////////////////////////////////////////// double t = 0; // physical constants const double L = 36; // lenght of arms const double pi = 3.1415926535897; // angle limitations (in radians) // motor1 const double limlow1 = 1; // min height in rad const double limhigh1 = 6.3; // max height in rad // motor 2 const double limlow2 = -4.0; // maximum height, motor has been inverted due to transmission const double limhigh2 = 2.5; // minimum height in rad /////////////////////////// // Main control loop transfer variables // here all variables that transfer bewtween the primary control functions // filter to calibration double output1; double output2; // filter to x-y double output1_amp; double output2_amp; // x-y to motor control double phi_one; double phi_two; // define start up variables (is to create a delayed start up instead of going to the ref value inmediately) double start_up_time = 3; double start_loops = start_up_time*controlfreq; int rc1 = 0; // counters in function to enable slow start up int rc2 = 0; // define return to start variables double reset_phi_one = 0; double reset_phi_two = 0; double phi_one_curr = 0; double phi_two_curr = 0; // REFERENCE SETTINGS double input_threshold = 0.25; // the minimum value the signal must have to change the reference. // Define storage variables for reference values (also start position) double c_reference_x = 60, c_reference_y = 0; // x-settings (no y-settings because these are calculated from the current x-position) double x_min = 47, x_max = 70; double xx,yy,y_min,y_max; // Define the maximum rate of change for the reference (velocity) double Vmax_x = 10, Vmax_y = 10; // [cm/s] // CONTROLLER SETTINGS // motor 1 const double m1_Kp = 5; // Proportional constant const double m1_Ki = 0.5; // integration constant const double m1_Kd = 0.4; // differentiation constant // motor 2 const double m2_Kp = 3; const double m2_Ki = 0.5; const double m2_Kd = 0.3; // storage variables. these variables are used to store data between controller iterations // motor 1 double m1_err_int = 0; double m1_prev_err = 0; // motor 2 double m2_err_int = 0; double m2_prev_err = 0; // EMG calibration variables double emg_gain1 = 7; double emg_gain2 = 7; double cal_time = 2.5; // amount of time calibration should take double cal_samples = controlfreq*cal_time; double normalize_emg_value = 1; // set te desired value to calibrate the signal to // FILTER VARIABLES // storage variables // differential action filter, same is used for both controllers double m_f_v1 = 0, m_f_v2 = 0; // input filter to smooth signal double r1_f_v1 = 0, r1_f_v2 = 0, r2_f_v1 = 0, r2_f_v2 = 0; // Define the storage variables and filter coeficients for eight filters // EMG filter 1 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; // EMG filter2 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; // Filter coefficients // differential action filter (lowpass 5Hz at 50Hz) 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; // input filter (lowpass 1Hz at 50samples) (used to make the x-y angle signal as smooth as possible 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; // EMG-Filter (calculated for 500Hz) // tweede orde notch filter 50 Hz // biquad 1 coefficienten const double numnotch50biq1_1 = 1, numnotch50biq1_2 = -1.61816178466632, numnotch50biq1_3 = 1.00000006127058, dennotch50biq1_2 = -1.59325742941798, dennotch50biq1_3 = 0.982171881701431; // biquad 2 coefficienten const double numnotch50biq2_1 = 1, numnotch50biq2_2 = -1.61816171933244, numnotch50biq2_3 = 0.999999938729428, dennotch50biq2_2 = -1.61431180968071, dennotch50biq2_3 = 0.982599066293075; // highpass filter 20 Hz coefficienten const double numhigh20_1 = 0.837089190566345, numhigh20_2 = -1.67417838113269, numhigh20_3 = 0.837089190566345, denhigh20_2 = -1.64745998107698, denhigh20_3 = 0.700896781188403; // lowpass 5 Hz coefficienten const double numlow5_1 =0.000944691843840162, numlow5_2 =0.00188938368768032, numlow5_3 =0.000944691843840162, denlow5_2 =-1.91119706742607, denlow5_3 =0.914975834801434; //////////////////////////////////////////////////////////////// /////////////////// START OF SIDE FUNCTIONS //////////////////// ////////////////////////////////////////////////////////////// // these functions are tailored to perform 1 specific function // this funtion flips leds on and off accordin to input with 0 being on void LED(int red,int green,int blue) { ledred.write(red); ledgreen.write(green); ledblue.write(blue); } // counts 2 radians // this function takes counts from the encoder and converts it to the amount of radians from the zero position. // It has been set up for standard 2X DECODING!!! double get_radians(double counts) { double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, last warning) return radians; } // This functions takes a 0->1 input, uses passing by reference (&c_reference) // to create a reference that moves with a variable speed. It is meant for -1->1 values double reference_f(double input, double &c_reference, double limlow, double limhigh, double Vmax) { double reference = c_reference + input * controlstep * Vmax ; // two if statements check if the reference exceeds the limits placed upon the arms if(reference < limlow){reference = limlow;} if(reference > limhigh){reference = limhigh;} c_reference = reference; // change the global variable to the latest location. return reference; } // This function takes the controller outputvalue and ensures it is between -1 and 1 // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction). double outputlimiter (double output, double limit) { if(output> limit) { output = 1; } else if(output < limit && output > 0) { output = output; } else if(output > -limit && output < 0) { output = output; } else if(output < -limit) { (output = -1); } return output; } // BIQUADFILTER CODE GIVEN IN SHEETS (input format: den, den, nom, nom, nom) double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2 = v1; v1 = v; return y; } // PID Controller given in sheets // aangepast om zelfde filter te gebruiken en om de termen te splitsen double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev) { // Proportional double P = Kp * e; // Integral e_int = e_int + Ts * e; double I = e_int * Ki; // Derivative double e_derr = (e - e_prev)/Ts; 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); // e_prev = e; double D = Kd* e_derr; // PID double output = P + I + D; return output; } // function that limits the angles that can be used in the motor reference signal double angle_limits(double phi, double limlow, double limhigh) { if(phi < limlow) { phi = limlow; } if(phi > limhigh) { phi = limhigh; } return phi; } void mod_send() { pc.printf("xx = %f, yy = %f, phi1 = %f, phi2 = %f \n",xx,yy,phi_one,phi_two); } ///////////////////////////////////////////////////////////////////// ////////////////// PRIMARY CONTROL FUNCTIONS /////////////////////// /////////////////////////////////////////////////////////////////// // these functions are called by go-flags and are used to update main variables and send signals to motor // function that updates the inputs void EMG_filter() { // filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass double u1 = EMG_in.read(); double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); double y4 = abs(y3); double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); // update global variables output1 = y5; output1_amp = y5*emg_gain1; // update global variable // filteren van EMG signaal 2 (A2), zelfde proces als signaal 1 double u1t = EMG_int.read(); double y1t = biquadfilter( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); double y2t = biquadfilter( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); double y3t = biquadfilter( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); double y4t = abs(y3t); double y5t = biquadfilter( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); // update global variables output2 = y5t; output2_amp = y5t*emg_gain2; scope.set(0,output1); scope.set(1,output2); scope.set(0,output1_amp); scope.set(1,output2_amp); scope.send(); } // function that updates the required motor angles void det_angles() { // convert global to local variable double xy_input1 = output1_amp; double xy_input2 = output2_amp; // use potmeter for debugging purposes xy_input1 = potright.read(); xy_input2 = potleft.read(); // add threshold value for outputs if(xy_input1 < input_threshold){xy_input1 = 0;} if(xy_input2 < input_threshold){xy_input2 = 0;} // return the input to a value between 0 and 1 (otherwise you will get jumps in input) xy_input1 = (xy_input1-input_threshold) * (1/(1-input_threshold)); xy_input2 = (xy_input2-input_threshold) * (1/(1-input_threshold)) ; // if below 0 = 0 if(xy_input1 < 0){xy_input1 = 0;} if(xy_input2 < 0){xy_input2 = 0;} // limit signal to 1 if(xy_input1 > 1){xy_input1 = 1;} if(xy_input2 > 1){xy_input2 = 1;} double xy_main_input = xy_input1 - xy_input2 ; // subtract inputs to create a signal that can go from -1 to 1 //scope.set(0,xy_main_input); // limit the output between -1 and 1 if(xy_main_input>1) {xy_main_input = 1;} if(xy_main_input<-1) {xy_main_input = -1;} if(switch_xy == false) // x-movement { xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x); // change the global x-reference // calculate the y limits belonging to that particular x coordinate y_min = - sqrt(4900 - pow(xx,2)); if(y_min<-35){y_min = -35;} // make sure the arm cannot hit the table (may later be removed) y_max = sqrt(4900 - pow(xx,2)); } if(switch_xy == true) { yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y); // change the y-reference } // last check for the depedent y-reference (otherwise if x is controlled after y, the circle is not followed). if(yy < y_min){yy = y_min;} if(yy > y_max){yy = y_max;} // let the arm make a circle // xx = 60 + 5*cos(t); // yy = 5*sin(t); // t = t + 0.01; // x-y to arm-angles math double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r double theta_one = (atan2(yy,xx)+beta); double theta_two = (-pi + alfa); // convert arm-angles to motor angles (x transmission) and offset (+ offset) to account for reset position double phi1 = 4*(theta_one) + 2.8; 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. phi2 = -phi2; // reverse angle because of double timing belt. // check the input angles and apply the limits phi1 = angle_limits(phi1,limlow1,limhigh1); phi2 = angle_limits(phi2,limlow2,limhigh2); // smooth the input signal (lowpass 1Hz). (to reduce the freq content after reaching limit) phi1 = biquadfilter(phi1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2); phi2 = biquadfilter(phi2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2); // write into global variables phi_one = phi1; phi_two = phi2; if(reset == true) { phi_one = reset_phi_one; phi_two = reset_phi_two; } // modserial // pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two); } // MOTOR 1 void motor1_control() { // change global into local variable double reference1 = phi_one; // add smooth start up if(rc1 < start_loops) { rc1++; reference1 = phi_one_curr + ((double) rc1/start_loops)*(reference1-phi_one_curr); } double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor double error1 = (reference1 - rads1); // determine the error (reference - position) double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err); // check the real angles and stop if exceeded (NaN protection) // if(rads1 < (limlow1 - 0.05)){m_output1 = 0;} // if(rads1 > (limhigh1 + 0.05)){m_output1 = 0;} m_output1 = outputlimiter(m_output1,1); // relimit the output between -1 and 1 for safety if(m_output1 > 0) { // uses the calculated output to determine the direction of the motor motor1_rich.write(0); motor1_aan.write(m_output1); } else if(m_output1 < 0) { motor1_rich.write(1); motor1_aan.write(abs(m_output1)); } } // MOTOR 2 void motor2_control() { double reference2 = phi_two; if(rc2 < start_loops) { rc2++; reference2 = phi_two_curr + ((double) rc2/start_loops)*(reference2-phi_two_curr); } double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor double error2 = (reference2 - rads2); // determine the error (reference - position) double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err); // check the real angles and stop if exceeded // if(rads2 < limlow2 - 0.05){m_output2 = 0;} // if(rads2 > limhigh2 + 0.05){m_output2 = 0;} m_output2 = outputlimiter(m_output2,1); // final output limit (not really needed, is for safety) if(m_output2 > 0) { // uses the calculated output to determine the direction of the motor motor2_rich.write(0); motor2_aan.write(m_output2); } else if(m_output2 < 0) { motor2_rich.write(1); motor2_aan.write(abs(m_output2)); } } // calibrate the emg-signal // works bij taking a certain amount of samples adding them and then normalize to the desired value // STILL BUGGED!!! // went to max-value type. must be tested.! void calibrate_amp() { double max1 = 0; double max2 = 0; for(int i = 0; i<cal_samples; i++) { EMG_filter(); // run filter double input1 = output1; // take data from global variable if(input1>max1){max1 = input1;} // take max input double input2 = output2; if(input2>max2){max2 = input2;} // take max input wait(controlstep); } emg_gain1 = normalize_emg_value/max1; // normalize the amplification so that the maximum signal hits the desired one emg_gain2 = normalize_emg_value/max2; pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2); } ////////////////////////////////////////////////////////////////// //////////// DEFINE GO-FLAG FUNCTIONS /////////////////////////// //////////////////////////////////////////////////////////////// void EMG_activate(){emg_go = true;} void angle_activate(){cart_go = true;} void motor1_activate(){motor1_go = true;} void motor2_activate(){motor2_go = true;} int main() { pc.baud(115200); main_filter.attach(&EMG_activate, controlstep); cartesian.attach(&angle_activate, controlstep); controller1.attach(&motor1_activate, controlstep); // call a go-flag controller2.attach(&motor2_activate, controlstep); send.attach(&mod_send, 1); while(true) { // button press functions // flow buttons if(buttonlinks.read() == 0) { loop_start = !loop_start; wait(buttonlinks.read() == 1); wait(0.3); } if(buttonrechts.read() == 0) { calib_start = !calib_start; wait(buttonrechts.read() == 1); wait(0.3); } // reverse buttons if(switch_xy_button.read() == 0) { switch_xy = !switch_xy; wait(switch_xy_button.read() == 1); led_right.write(!led_right.read()); // turn on led when switched to y control wait(0.2); } if(reset_button.read() == 0) { reset = !reset; wait(reset_button.read() == 1); phi_one_curr = phi_one; phi_two_curr = phi_two; rc1 = 0; rc2 = 0; wait(0.3); } ////////////////////////////////////////////////// // Main Control stuff and options if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops { LED(1,1,0); // turn blue led on if(cart_go) { cart_go = false; det_angles();} if(emg_go) { emg_go = false; EMG_filter();} if(motor1_go) { motor1_go = false; motor1_control();} if(motor2_go) { motor2_go = false; motor2_control();} } // shut off both motors if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);} // c_reference_x = 60; c_reference_y = 0;} // turn green led on // start calibration procedures if(loop_start == false && calib_start == true) { LED(1,0,1); motor1_aan.write(0); motor2_aan.write(0); calibrate_amp(); // 10 second calibration calib_start = false; // turn fork off } // turn red led on if(loop_start == true && calib_start == true) { LED(0,1,1); motor1_aan.write(0); motor2_aan.write(0); } // turn leds off (both buttons false) else { LED(1,1,1);} } }