main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "BiQuad.h"
00004 #include "MODSERIAL.h"
00005 #include "QEI.h"
00006 #include "FastPWM.h"
00007 
00008 // in gebruik: D(0,1,4,5,6,7,8,10,11,12,13) A(0,1,2)
00009 
00010 MODSERIAL pc(USBTX, USBRX);
00011 HIDScope scope(3); // the amount of scopes to send to the pc
00012 
00013 //Define objects
00014 
00015 //Define the EMG inputs
00016 AnalogIn    emg1( A0 );
00017 AnalogIn    emg2( A1 );
00018 AnalogIn    emg3( A2 );
00019 
00020 //Define motor outputs
00021 DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
00022 FastPWM motor1(D6);     // speed of motor 1
00023 FastPWM motor2(D5);     //speed of motor 2
00024 DigitalOut motor2dir(D4);   //direction of motor 2, attach at m2, set to 0: ccw
00025 
00026 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
00027 QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
00028 
00029 DigitalIn button(PTA4);
00030 
00031 //Define the Tickers
00032 Ticker      pos_timer;                      // the timer which is used to print the position every second
00033 Ticker      sample_timer;                   // the timer which is used to decide when a sample needs to be taken
00034 Ticker      control;                        // Ticker for processing encoder input
00035 
00036 //Initialize all variables
00037 volatile bool sampletimer = false;          // a variable which is changed when a sample needs to be taken
00038 volatile bool controller_go=false;
00039 bool button_pressed=false;
00040 
00041 double threshold = 0.04;                    // the threshold which the emg signals need to surpass to do something
00042 double samplefreq=0.002;                    // every 0.002 sec a sample will be taken this is a frequency of 500 Hz
00043 double emg02;                               // the first emg signal
00044 double emg12;                               // the second emg signal
00045 double emg22;                               // the third emg signal
00046 double ref_x=0.0000;                        // the x reference position
00047 double ref_y=0.0000;                        // the y reference position
00048 double old_ref_x;                           // the old x reference
00049 double old_ref_y;                           // the old y reference
00050 double speed=0.00002;                       // the variable with which a speed is reached of 1cm/s
00051 double theta=0.0;                           // angle of the arm
00052 double radius=0.0;                          // radius of the arm
00053 
00054 double minRadius=0.3;                 // minimum radius of arm
00055 const double maxRadius=0.6;                 // maximum radius of arm
00056 const double min_Radius=0.3;
00057 const double minAngle=-1.25;                // minimum angle for limiting controller
00058 const double min_y=-0.28;                   // minimum height which the spatula can reach
00059 char key;                                   // variable to place the keyboard input
00060 
00061 double m1_pwm=0;    //variable for PWM control motor 1
00062 double m2_pwm=0;    //variable for PWM control motor 2
00063 
00064 const double m1_Kp = 9.974, m1_Ki = 16.49, m1_Kd = 1.341, m1_N = 100; // controller constants motor 1
00065 double m1_v1 = 0, m1_v2 = 0; // Memory variables
00066 const double m1_Ts = 0.01; // Controller sample time
00067 
00068 const double m2_Kp = 9.974, m2_Ki = 16.49, m2_Kd = 1.341, m2_N = 100; // controller constants motor 2
00069 double m2_v1 = 0, m2_v2 = 0; // Memory variables
00070 const double m2_Ts = 0.01; // Controller sample time
00071 
00072 const double pi=3.14159265359;
00073 const double res = 64/(1/131.25*2*pi);     // resolution on gearbox shaft per pulse
00074 const double V_max=9.0;                    // maximum voltage supplied by trafo
00075 const double pulleyRadius=0.0398/2;        // pulley diameter
00076 
00077 //Define the needed Biquad chains
00078 BiQuadChain bqc11;
00079 BiQuadChain bqc13;
00080 BiQuadChain bqc21;
00081 BiQuadChain bqc23;
00082 BiQuadChain bqc31;
00083 BiQuadChain bqc33;
00084 
00085 //Define the BiQuads for the filter of the first emg signal
00086 //Notch filter
00087 BiQuad bq111(0.9795,   -1.5849,    0.9795,    1.0000,   -1.5849,    0.9589);
00088 BiQuad bq112(0.9833,   -1.5912,    0.9833,    1.0000,   -1.5793,    0.9787);
00089 BiQuad bq113(0.9957,   -1.6111,    0.9957,    1.0000,   -1.6224,    0.9798);
00090 //High pass filter
00091 //BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
00092 BiQuad bq121( 0.8956,   -1.7911,    0.8956,    1.0000,   -1.7814,    0.7941);
00093 BiQuad bq122( 0.9192,   -1.8385,    0.9192,    1.0000,   -1.8319,    0.8450);
00094 BiQuad bq123( 0.9649,   -1.9298,    0.9649,    1.0000,   -1.9266,    0.9403);
00095 //Low pass filter
00096 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
00097 
00098 //Define the BiQuads for the filter of the second emg signal
00099 //Notch filter
00100 BiQuad bq211 = bq111;
00101 BiQuad bq212 = bq112;
00102 BiQuad bq213 = bq113;
00103 //High pass filter
00104 BiQuad bq221 = bq121;
00105 BiQuad bq222 = bq122;
00106 BiQuad bq223 = bq123;
00107 //Low pass filter
00108 BiQuad bq231 = bq131;
00109 
00110 //Define the BiQuads for the filter of the third emg signal
00111 //notch filter
00112 BiQuad bq311 = bq111;
00113 BiQuad bq312 = bq112;
00114 BiQuad bq313 = bq113;
00115 //High pass filter
00116 BiQuad bq321 = bq121;
00117 BiQuad bq323 = bq122;
00118 BiQuad bq322 = bq123;
00119 //low pass filter
00120 BiQuad bq331 = bq131;
00121 
00122 void sampleflag()
00123 {
00124     if (sampletimer==true) {
00125         // this if statement is used to see if the code takes too long before it is called again
00126         pc.printf("rate too high error in sampleflag\n\r");
00127     }
00128     //This sets the go flag for when the function sample needs to be called
00129     sampletimer=true;
00130 }
00131 
00132 void activate_controller()
00133 {
00134     if (controller_go==true) {
00135         // this if statement is used to see if the code takes too long before it is called again
00136         pc.printf("rate too high error in activate_controller()\n\r");
00137     }
00138     controller_go=true;   //activate go flag
00139 }
00140 
00141 void sample()
00142 {
00143     //This checks if a key is pressed and changes the variable key in the pressed key
00144     if (pc.readable()==1) {
00145         key=pc.getc();
00146     }
00147     //Read the emg signals and filter it
00148 
00149     emg02=bqc13.step(fabs(bqc11.step(emg1.read())));    //filtered signal 0
00150     emg12=bqc23.step(fabs(bqc21.step(emg2.read())));    //filtered signal 1
00151     emg22=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 2
00152 
00153     //remember what the reference was
00154     old_ref_x=ref_x;
00155     old_ref_y=ref_y;
00156     //look if the emg signals go over the threshold and change the reference accordingly
00157     if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
00158         ref_x=ref_x-speed;
00159         ref_y=ref_y-speed;
00160 
00161     } else if (emg02>threshold&&emg12>threshold || key == 'a' ) {
00162         ref_x=ref_x-speed;
00163 
00164     } else if (emg02>threshold&&emg22>threshold || key == 's') {
00165         ref_y=ref_y-speed;
00166 
00167     } else if (emg12>threshold&&emg22>threshold || key == 'e' ) {
00168         ref_x=ref_x+speed;
00169         ref_y=ref_y+speed;
00170 
00171     } else if (emg12>threshold || key == 'q' ) {
00172         ref_x=ref_x+speed;
00173 
00174     } else if (emg22>threshold || key == 'w') {
00175         ref_y=ref_y+speed;
00176     }
00177 
00178     // convert the x and y reference to the theta and radius reference
00179     theta=atan(ref_y/(ref_x+min_Radius));
00180     radius=sqrt(pow(ref_x+min_Radius,2)+pow(ref_y,2));
00181 
00182     //look if the new reference is outside the possible range and revert back to the old reference if it is outside  the range
00183     if (theta < minAngle) {
00184         ref_x=old_ref_x;
00185         ref_y=old_ref_y;
00186 
00187     } else if (radius < minRadius) {
00188         ref_x=old_ref_x;
00189         ref_y=old_ref_y;
00190 
00191     } else if ( radius > maxRadius) {
00192         ref_x=old_ref_x;
00193         ref_y=old_ref_y;
00194     } else if (ref_y<min_y) {
00195         ref_x=old_ref_x;
00196         ref_y=old_ref_y;
00197     }
00198     theta=atan(ref_y/(ref_x+min_Radius));
00199     radius=sqrt(pow(ref_x+min_Radius,2)+pow(ref_y,2));
00200 }
00201 
00202 double PID( double err, const double Kp, const double Ki, const double Kd,
00203             const double Ts, const double N, double &v1, double &v2 )   //discrete PIDF filter
00204 {
00205     const double a1 =-4/(N*Ts+2),
00206                  a2=-(N*Ts-2)/(N*Ts+2),
00207                  b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
00208                  b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
00209                  b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
00210 
00211     double v=err-a1*v1-a2*v2;
00212     double u=b0*v+b1*v1+b2*v2;
00213     v2=v1;
00214     v1=v;
00215     return u;
00216 }
00217 
00218 void controller()  //function for executing controller action
00219 {
00220 
00221     //converting radius and theta to gearbox angle
00222     double ref_angle1=16*theta;
00223     double ref_angle2=(-radius+min_Radius)/pulleyRadius;
00224 
00225     double angle1 = Encoder1.getPulses()/res;   //get number of pulses (counterclockwise is positive)
00226     double angle2 = Encoder2.getPulses()/res;   //get number of pulses
00227     m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
00228     //divide by voltage to get pwm duty cycle percentage)
00229     m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;
00230 
00231     //limit pwm value and change motor direction when pwm becomes either negative or positive
00232     if (m1_pwm >=0.0f && m1_pwm <=1.0f) {
00233         motor1dir=0;
00234         motor1.write(m1_pwm);
00235     } else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) {
00236         motor1dir=1;
00237         motor1.write(-m1_pwm);
00238     }
00239 
00240     if (m2_pwm >=0.0f && m2_pwm <=1.0f) {
00241         motor2dir=0;
00242         motor2.write(m2_pwm);
00243     } else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) {
00244         motor2dir=1;
00245         motor2.write(-m2_pwm);
00246     }
00247 
00248     //hidsopce to check what the code does exactly
00249     scope.set(0,ref_angle2-angle2); //error
00250     scope.set(1,angle2);
00251     scope.set(2,m2_pwm);
00252     scope.send();
00253 }
00254 
00255 void my_pos()
00256 {
00257     //This function is attached to a ticker so that the reference position is printed every second.
00258     pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta);
00259 
00260 }
00261 
00262 int main()
00263 {
00264     pc.printf("RESET\n\r");
00265     pc.baud(115200);
00266 
00267     //Attach the Biquads to the Biquad chains
00268     bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
00269     bqc13.add( &bq131);
00270     bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
00271     bqc23.add( &bq231);
00272     bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
00273     bqc33.add( &bq331);
00274 
00275     motor1.period(0.02f); //period of pwm signal for motor 1
00276     motor2.period(0.02f); // period of pwm signal for motor 2
00277     motor1dir=0; // setting direction to ccw
00278     motor2dir=0; // setting direction to ccw
00279 
00280     //Attach the 'sample' function to the timer 'sample_timer'.
00281     //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
00282     sample_timer.attach(&sampleflag, samplefreq);
00283 
00284     //Attach the function my_pos to the timer pos_timer.
00285     //This ensures that the position is printed every second.
00286     pos_timer.attach(&my_pos, 1);
00287     control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
00288 
00289 
00290     while(1) {
00291         //Only take a sample when the go flag is true.
00292         if (button==0) {
00293             button_pressed=true;
00294             minRadius=-50.0;
00295         } else {
00296             if (button_pressed==true) {
00297                 ref_x=0.0;
00298                 ref_y=0.0;
00299                 button_pressed=false;
00300             }
00301             minRadius=0.3;
00302         }
00303 
00304         if (sampletimer==true) {
00305             sample();
00306             sampletimer = false;            //change sampletimer to false if sample() is finished
00307         }
00308 
00309         if(controller_go) { // go flag
00310             controller();
00311             controller_go=false;
00312         }
00313     }
00314 }