Diff: main.cpp

Revision:

0:a643b1b38abe

Child:

1:f3910e46b831

--- /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