werkend x-y control
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 0:a643b1b38abe
- Child:
- 1:f3910e46b831
diff -r 000000000000 -r a643b1b38abe main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Oct 18 17:27:35 2015 +0000 @@ -0,0 +1,379 @@ +#include "mbed.h" +#include "MODSERIAL.h" +#include "encoder.h" +#include "HIDScope.h" + +Serial pc(USBTX,USBRX); +HIDScope scope(3); // definieerd het aantal kanalen van de scope + +// Define Tickers and control frequencies +Ticker controller1, controller2; // definieer de ticker die controler1 doet + // Go flag variables + volatile bool motor1_go = false, motor2_go = false; + + // Frequency control + double controlfreq = 50 ; // controlloops frequentie (Hz) + double controlstep = 1/controlfreq; // timestep derived from controlfreq + + +//MOTOR OUTPUTPINS +// motor 1 + PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets) + DigitalOut motor1_rich(D7); // digitaal signaal voor richting +// motor 2 + PwmOut motor2_aan(D5); + DigitalOut motor2_rich(D4); + +// ENCODER INPUTPINS +Encoder motor1_enc(D12,D11); // encoder outputpins +Encoder motor2_enc(D10,D9); + +int reference1 = 0; // set the reference position of the encoders (not used) +int reference2 = 0; + + +// EXTRA INPUTS AND REQUIRED VARIABLES +//POTMETERS + AnalogIn potright(A0); // define the potmeter outputpins + AnalogIn potleft(A1); + +// BUTTONS + // control flow + DigitalIn buttonlinks(PTA4); + DigitalIn buttonrechts(PTC6); + // init values + bool loop_start = false; + bool calib_start = false; + + // direction control + DigitalIn reverse_button_links(D0); + DigitalIn reverse_button_rechts(D1); + // init values + bool reverse_links = false; + bool reverse_rechts = false; + +// LED + DigitalOut ledred(LED1); + DigitalOut ledgreen(LED2); + DigitalOut ledblue(LED3); + +// REFERENCE SIGNAL SETTINGS + double input_threshold = 0.25; // the minimum value the signal must have to change the reference. + // Define signal amplification (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!) ?? + double signalamp1 = 1; + double signalamp2 = 1; + // Define storage variables for reference values + double c_reference1 = 0; + double c_reference2 = 0; +// limit angles (in radians) + // motor1 + const double limlow1 = 0.5; // min height + const double limhigh1 = 4.5; // max height + // motor 2 + const double limlow2 = -4.5; // maximum height, motor has been inverted due to transmission + const double limhigh2 = 2; // minimum height + +// Define the maximum rate of change for the reference (velocity) + double Vmax = 3; // rad/sec + +// 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 = 2; + const double m2_Ki = 0; + const double m2_Kd = 0.1; +// storage variables + // motor 1 + double m1_err_int = 0; + double m1_prev_err = 0; + // motor 2 + double m2_err_int = 0; + double m2_prev_err = 0; + + +//// 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; + double r2_f_v1 = 0, r2_f_v2 = 0; + +// Filter coefficients +// differential action filter (lowpass 5Hz at 50samples) + 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. +// input filter (lowpass 1Hz at 50samples) + 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; + +// tweede orde notch filter 50 Hz +// biquad 1 coefficienten + const double numnotch50biq1_1 = 1; + const double numnotch50biq1_2 = -1.61816178466632; + const double numnotch50biq1_3 = 1.00000006127058; + const double dennotch50biq1_2 = -1.59325742941798; + const double dennotch50biq1_3 = 0.982171881701431; +// biquad 2 coefficienten + const double numnotch50biq2_1 = 1; + const double numnotch50biq2_2 = -1.61816171933244; + const double numnotch50biq2_3 = 0.999999938729428; + const double dennotch50biq2_2 = -1.61431180968071; + const double dennotch50biq2_3 = 0.982599066293075; +// highpass filter 20 Hz coefficienten + const double numhigh20_1 = 0.837089190566345; + const double numhigh20_2 = -1.67417838113269; + const double numhigh20_3 = 0.837089190566345; + const double denhigh20_2 = -1.64745998107698; + const double denhigh20_3 = 0.700896781188403; +// lowpass 5 Hz coefficienten + const double numlow5_1 =0.000944691843840162; + const double numlow5_2 =0.00188938368768032; + const double numlow5_3 =0.000944691843840162; + const double denlow5_2 =-1.91119706742607; + const double denlow5_3 =0.914975834801434; + +// Define the storage variables and filter coeficients for four filters +double f1_v1 = 0, f1_v2 = 0; +double f2_v1 = 0, f2_v2 = 0; +double f3_v1 = 0, f3_v2 = 0; +double f4_v1 = 0, f4_v2 = 0; + + + +//////////////////////////////////////////////////////////////// +/////////////////// 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 pi = 3.14159265359; + 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 0->1 values +double reference_f(double input, double &c_reference, double limlow, double limhigh) +{ + 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). +// needs more work to use it for the wind-up prevention. +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 +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; + } + +double EMG_Filter(double u1) +{ + // double u1 = potright.read(); // legacy test code + 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); + return y5; +} + +// PID Controller given in sheets +// aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!) +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; +} +///////////////////////////////////////////////////////////////////// +////////////////// PRIMARY CONTROL FUNCTIONS /////////////////////// +/////////////////////////////////////////////////////////////////// +// these functions are used to control all aspects of a single electric motor and are called by the main function from tickers + +// MOTOR 1 +void motor1_control() +{ + + double input1 = potright.read()*signalamp1; // this line must be seperated for emg usage + //input1 = 0.4505; + + // first input edit (limit signal between threshold and 1, and reverse if wanted + if(input1 < input_threshold) {input1 = 0;} + if(input1 > 1) {input1 = 1;} + if(reverse_rechts == true) {input1 = -input1;} + input1 = biquadfilter(input1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2); //biquad with diff-filter coefficients to smooth input + + + double reference1 = reference_f(input1, c_reference1,limlow1,limhigh1); // determine the reference that has been set by the inputsignal + scope.set(0,reference1); + double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor + scope.set(1,rads1); + scope.send(); + double error1 = (reference1 - rads1); // determine the error (reference - position) + double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err); + + output1 = outputlimiter(output1,1); // relimit the output for safety + if(output1 > 0) { // uses the calculated output to determine the direction of the motor + motor1_rich.write(0); + motor1_aan.write(output1); + } else if(output1 < 0) { + motor1_rich.write(1); + motor1_aan.write(abs(output1)); + } +} + +// MOTOR 2 +void motor2_control() +{ + double input2 = potleft.read()*signalamp2; // replace potleft with filter + // first input limiter + if(input2 < input_threshold) {input2 = 0;} + if(input2 > 1) {input2 = 1;} + if(reverse_links == false) {input2 = -input2;} + input2 = biquadfilter(input2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2); + + + double reference2 = reference_f(input2, c_reference2,limlow2,limhigh2); // determine the reference that has been set by the clamped inputsignal + double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor + double error2 = (reference2 - rads2); // determine the error (reference - position) + double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err); + + output2 = outputlimiter(output2,1); + if(output2 > 0) { // uses the calculated output to determine the direction of the motor + motor2_rich.write(0); + motor2_aan.write(output2); + } else if(output2 < 0) { + motor2_rich.write(1); + motor2_aan.write(abs(output2)); + } +} + + +////////////////////////////////////////////////////////////////// +//////////// DEFINE GO-FLAG FUNCTIONS /////////////////////////// +//////////////////////////////////////////////////////////////// + +void motor1_activate() +{ + motor1_go = true; +} + +void motor2_activate() +{ + motor2_go = true; +} + +int main() +{ + pc.baud(115200); + controller1.attach(&motor1_activate, controlstep); // call a go-flag + controller2.attach(&motor2_activate, controlstep); + 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(reverse_button_links.read() == 0) + { + reverse_links = !reverse_links; + wait(reverse_button_links.read() == 1); + wait(0.3); + } + if(reverse_button_rechts.read() == 0) + { + reverse_rechts = !reverse_rechts; + wait(reverse_button_rechts.read() == 1); + 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(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);} + + // 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);} + + // 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);} + } +} \ No newline at end of file