main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 #include "encoder.h"
00004 #include "HIDScope.h"
00005 
00006 Serial pc(USBTX,USBRX);
00007 HIDScope scope(3);          // definieerd het aantal kanalen van de scope
00008 
00009 // Define Tickers and control frequencies
00010 Ticker          controller1, controller2;        // definieer de ticker die controler1 doet
00011     // Go flag variables
00012     volatile bool motor1_go = false, motor2_go = false;
00013 
00014     // Frequency control
00015     double controlfreq = 50 ;    // controlloops frequentie (Hz)
00016     double controlstep = 1/controlfreq; // timestep derived from controlfreq
00017 
00018 
00019 //MOTOR OUTPUTPINS
00020 // motor 1
00021     PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
00022     DigitalOut motor1_rich(D7); // digitaal signaal voor richting
00023 // motor 2
00024     PwmOut motor2_aan(D5);
00025     DigitalOut motor2_rich(D4);
00026 
00027 // ENCODER INPUTPINS
00028 Encoder motor1_enc(D12,D11);        // encoder outputpins
00029 Encoder motor2_enc(D10,D9);
00030 
00031 int reference1 = 0;         // set the reference position of the encoders
00032 int reference2 = 0;
00033 
00034 
00035 // EXTRA INPUTS AND REQUIRED VARIABLES
00036 //POTMETERS
00037     AnalogIn potright(A0);      // define the potmeter outputpins
00038     AnalogIn potleft(A1);
00039 
00040 // BUTTONS 
00041     // control flow             
00042     DigitalIn   buttonlinks(PTA4);       
00043     DigitalIn   buttonrechts(PTC6);
00044         // init values
00045         bool loop_start = false;
00046         bool calib_start = false;
00047         
00048     // direction control
00049     DigitalIn   reverse_button_links(D0);
00050     DigitalIn   reverse_button_rechts(D1); 
00051         // init values
00052         bool reverse_links = false;
00053         bool reverse_rechts = false;
00054 
00055 // LED
00056     DigitalOut ledred(LED1);
00057     DigitalOut ledgreen(LED2);
00058     DigitalOut ledblue(LED3);
00059 
00060 // REFERENCE SIGNAL SETTINGS
00061     double input_threshold = 0.25;   // the minimum value the signal must have to change the reference.
00062     // Define signal amplification  (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!)    ??
00063     double signalamp1 = 1;
00064     double signalamp2 = 1;
00065  // Define storage variables for reference values
00066     double c_reference1 = 0;
00067     double c_reference2 = 0;
00068 // limit  angles
00069     // motor1
00070     const double limlow1 = 0.5;             // min height
00071     const double limhigh1 = 4.5;            // max height
00072     // motor 2
00073     const double limlow2 = -4.5;            // maximum height, motor has been inversed due to transmission
00074     const double limhigh2 = 2;              // minimum height
00075     
00076 // Define the maximum rate of change for the reference (velocity)
00077     double Vmax = 3;            // rad/sec
00078 
00079 // CONTROLLER SETTINGS
00080     // motor 1
00081     const double m1_Kp = 2;         // Proportional constant     
00082     const double m1_Ki = 0.05;         // integration constant
00083     const double m1_Kd = 0;         // differentiation constant
00084     // motor 2
00085     const double m2_Kp = 0.25;
00086     const double m2_Ki = 0.01;
00087     const double m2_Kd = 0;
00088 // storage variables
00089     // motor 1
00090     double m1_err_int = 0; 
00091     double m1_prev_err = 0;
00092     // motor 2
00093     double m2_err_int = 0;
00094     double m2_prev_err = 0;
00095 
00096 
00097 //// FILTER VARIABLES
00098 // storage variables
00099         // differential action filter, same is used for both controllers
00100         double m_f_v1 = 0;
00101         double m_f_v2 = 0;
00102 
00103 // Filter coefficients
00104 // differential action filter
00105     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.
00106     
00107 // tweede orde notch filter 50 Hz
00108 // biquad 1 coefficienten
00109     const double numnotch50biq1_1 = 1;
00110     const double numnotch50biq1_2 = -1.61816178466632;
00111     const double numnotch50biq1_3 = 1.00000006127058;
00112     const double dennotch50biq1_2 = -1.59325742941798;
00113     const double dennotch50biq1_3 = 0.982171881701431;
00114 // biquad 2 coefficienten
00115     const double numnotch50biq2_1 = 1;
00116     const double numnotch50biq2_2 = -1.61816171933244;
00117     const double numnotch50biq2_3 = 0.999999938729428;
00118     const double dennotch50biq2_2 = -1.61431180968071;
00119     const double dennotch50biq2_3 = 0.982599066293075;
00120 // highpass filter 20 Hz coefficienten
00121     const double numhigh20_1 = 0.837089190566345;
00122     const double numhigh20_2 = -1.67417838113269;
00123     const double numhigh20_3 = 0.837089190566345;
00124     const double denhigh20_2 = -1.64745998107698;
00125     const double denhigh20_3 = 0.700896781188403;
00126 // lowpass 5 Hz coefficienten
00127     const double numlow5_1 =0.000944691843840162;
00128     const double numlow5_2 =0.00188938368768032;
00129     const double numlow5_3 =0.000944691843840162;
00130     const double denlow5_2 =-1.91119706742607;
00131     const double denlow5_3 =0.914975834801434;
00132 
00133 // Define the storage variables and filter coeicients for two filters
00134 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;
00135 
00136 
00137 
00138 ////////////////////////////////////////////////////////////////
00139 /////////////////// START OF SIDE FUNCTIONS ////////////////////
00140 //////////////////////////////////////////////////////////////
00141 // these functions are tailored to perform 1 specific function
00142 
00143 // this funtion flips leds on and off accordin to input with 0 being on
00144 void LED(int red,int green,int blue)
00145 {
00146     ledred.write(red);
00147     ledgreen.write(green);
00148     ledblue.write(blue);
00149 }    
00150     
00151 // counts 2 radians
00152 // this function takes counts from the encoder and converts it to the amount of radians from the zero position. 
00153 // It has been set up for standard 2X DECODING!!!!!!
00154 double get_radians(double counts)       
00155 {
00156     double pi = 3.14159265359;
00157     double radians = (counts/4200)*2*pi;        // 2X DECODING!!!!! ((32 counts/rotation, extra warning)
00158     return radians;
00159 }
00160 
00161 
00162 // This functions takes a 0->1 input, uses passing by reference (&c_reference)
00163 // to create a reference that moves with a variable speed. It is now optimised for 0->1 values
00164 double reference_f(double input, double &c_reference, double limlow, double limhigh)
00165 {
00166     double reference = c_reference + input * controlstep * Vmax ;
00167             if(reference < limlow){reference = limlow;}
00168             if(reference > limhigh){reference = limhigh;}
00169            c_reference = reference; // change the global variable to the latest location.
00170     return reference;
00171 }
00172 
00173 
00174 // This function takes the controller outputvalue and ensures it is between -1 and 1
00175 // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
00176 // needs more work to use it for the wind-up prevention.
00177 double outputlimiter (double output, double limit)
00178 {
00179     if(output> limit)                                
00180     {
00181         output = 1;
00182     }
00183     else if(output < limit && output > 0)
00184     {
00185         output = output;
00186     }
00187     else if(output > -limit && output < 0)
00188     {
00189         output = output;
00190     }
00191     else if(output < -limit)
00192     {
00193         (output = -1);
00194     }
00195     return output;
00196 }
00197 
00198 
00199 // BIQUADFILTER CODE GIVEN IN SHEETS 
00200 // used for d-control
00201 double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
00202 {
00203     double v = u - a1*v1 - a2*v2;
00204     double y = b0*v + b1*v1 + b2*v2;
00205     v2 = v1;
00206     v1 = v;
00207     return y;
00208     }
00209 
00210 double EMG_Filter(double u1)
00211 {
00212     // double u1 = potright.read();
00213     double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
00214     double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
00215     double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
00216     double y4 = abs(y3);
00217     double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
00218     return y5;
00219 }
00220 
00221 // PID Controller given in sheets
00222 // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!)
00223 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
00224 {
00225 // Proportional
00226 double P = Kp * e;
00227 // pc.printf("P = %f, ",P);
00228 // Integral
00229 e_int = e_int + Ts * e;
00230 double I = e_int * Ki;
00231 // pc.printf("I = %f, ",I);
00232 // Derivative   (Disabled for now because of NaN problem from filter
00233 double e_derr = (e - e_prev)/Ts;
00234 // pc.printf("e_derr %f, ",e_derr);
00235 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);
00236 // 
00237 pc.printf("fil_e_derr %f, ",e_derr);    // check for NaN                ??
00238 e_prev = e;
00239 double D = Kd* e_derr;
00240 
00241 // PID
00242 double output = P + I + D;
00243 return output;
00244 }
00245 
00246 
00247 
00248 // this function is a simple K control called by the motor function
00249 double K_control(double error,double K)
00250 {
00251     double output = K*error;                            // controller output K*e
00252     output = outputlimiter(output,1);             // limit the output to -1->1
00253     return output;
00254 }
00255 
00256 /////////////////////////////////////////////////////////////////////
00257 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
00258 ///////////////////////////////////////////////////////////////////
00259 // these functions are used to control all aspects of a single electric motor and are called by the main function
00260 
00261 // MOTOR 1
00262 void motor1_control()
00263 {
00264     
00265     double input1 = potright.read()*signalamp1; // this line must be seperated for emg usage
00266     //pc.printf("input = %f ",input1);
00267     // first data limiter
00268     if(input1 < input_threshold) {input1 = 0;}
00269     if(input1 > 1) {input1 = 1;}
00270     // reverse direction
00271     if(reverse_rechts == true) {input1 = -input1;}
00272 
00273     double reference1 = reference_f(input1, c_reference1,limlow1,limhigh1);      // determine the reference that has been set by the inputsignal
00274     scope.set(0,reference1);
00275     double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
00276     scope.set(1,rads1);
00277     double error1 = (reference1 - rads1);                       // determine the error (reference - position)
00278     double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
00279     //pc.printf("output 1 %f \n",output1);
00280     scope.set(2,output1);
00281     scope.send();
00282     output1 = outputlimiter(output1,1);
00283     if(output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
00284         motor1_rich.write(0);
00285         motor1_aan.write(output1);
00286     } else if(output1 < 0) {
00287         motor1_rich.write(1);
00288         motor1_aan.write(abs(output1));
00289     }
00290 }
00291 
00292 // MOTOR 2
00293 void motor2_control()
00294 {
00295     double input2 = potleft.read()*signalamp2;      // replace potleft with filter
00296     // first input limiter
00297     if(input2 < input_threshold) {input2 = 0;}
00298     if(input2 > 1) {input2 = 1;}
00299     // reverse direction
00300     if(reverse_links == true) {input2 = -input2;}
00301     double reference2 = reference_f(input2, c_reference2,limlow2,limhigh2);      // determine the reference that has been set by the clamped inputsignal
00302     double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
00303     double error2 = (reference2 - rads2);                       // determine the error (reference - position)
00304     //scope.set(1,error2);
00305     //scope.send();
00306     double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
00307     output2 = outputlimiter(output2,1);
00308     if(output2 > 0) {                    // uses the calculated output to determine the direction  of the motor
00309         motor2_rich.write(0);
00310         motor2_aan.write(output2);
00311     } else if(output2 < 0) {
00312         motor2_rich.write(1);
00313         motor2_aan.write(abs(output2));
00314     }
00315 }
00316 
00317 
00318 //////////////////////////////////////////////////////////////////
00319 //////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
00320 ////////////////////////////////////////////////////////////////
00321 
00322 void motor1_activate()
00323 { 
00324     motor1_go = true; 
00325 }
00326  
00327 void motor2_activate()
00328 { 
00329     motor2_go = true; 
00330 }
00331 
00332 int main()
00333 {
00334     pc.baud(115200);
00335     controller1.attach(&motor1_activate, controlstep);      // call a go-flag
00336     controller2.attach(&motor2_activate, controlstep);   // Disabled while debugging PID-controller ??
00337     while(true) 
00338     {
00339         // button press functions
00340         // flow buttons
00341         if(buttonlinks.read() == 0)
00342         {
00343             loop_start = !loop_start;     
00344             wait(buttonlinks.read() == 1);
00345             wait(0.3);             
00346         }
00347         if(buttonrechts.read() == 0)
00348         {
00349             calib_start = !calib_start;     
00350             wait(buttonrechts.read() == 1);
00351             wait(0.3);             
00352         }
00353         // reverse buttons
00354         if(reverse_button_links.read() == 0)
00355         {
00356            reverse_links = !reverse_links;     
00357            wait(reverse_button_links.read() == 1);
00358            wait(0.3);             
00359         }
00360          if(reverse_button_rechts.read() == 0)
00361         {
00362             reverse_rechts = !reverse_rechts;     
00363             wait(reverse_button_rechts.read() == 1);
00364             wait(0.3);             
00365         }
00366         // Main Control stuff and options
00367         if(loop_start == true && calib_start == false)        // check if start button = true then start the main control loops
00368         {
00369             LED(1,1,0); // turn blue led on
00370             if(motor1_go) { motor1_go = false; motor1_control();}
00371             if(motor2_go) { motor2_go = false; motor2_control();}
00372         }
00373         // turn green led on // start calibration procedures
00374         if(loop_start == false && calib_start == true) { LED(1,0,1); motor1_aan.write(0); motor2_aan.write(0);}
00375         // turn red led on
00376         if(loop_start == true && calib_start == true) { LED(0,1,1);}
00377         // turn leds off (both buttons false)
00378         else { LED(1,1,1);}
00379     }
00380 }