main.cpp

00001  //--------------------------------------------------------------------------------------------------------------------------//
00002  // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7
00003  //--------------------------------------------------------------------------------------------------------------------------//
00004  // Libraries
00005  //--------------------------------------------------------------------------------------------------------------------------//
00006 #include "mbed.h"
00007 #include "MODSERIAL.h"
00008 #include "HIDScope.h"
00009 #include "QEI.h"
00010 #include "biquadFilter.h"
00011 
00012 //--------------------------------------------------------------------------------------------------------------------------//
00013 // Constanten/Inputs/Outputs
00014 //--------------------------------------------------------------------------------------------------------------------------//
00015     MODSERIAL pc(USBTX, USBRX);             // To/From PC
00016     QEI Encoder2(D3, D2, NC, 32);           // Encoder Motor 2
00017     QEI Encoder1(D13,D12,NC, 32);           // Encoder Motor 1
00018     HIDScope scope(4);                      // Scope, 4 channels
00019 
00020 // LEDs
00021     DigitalOut LedR(LED_RED);
00022     DigitalOut LedG(LED_GREEN);
00023     DigitalOut LedB(LED_BLUE);
00024 
00025 // Motor
00026     DigitalOut motor1direction(D7);         // Motor 1, Direction & Speed
00027     PwmOut motor1speed(D6);
00028     DigitalOut motor2direction(D4);         // Motor 2, Direction & Speed
00029     PwmOut motor2speed(D5);
00030 
00031 // Tickers
00032     Ticker ScopeTime;
00033     Ticker myControllerTicker2;
00034     Ticker myControllerTicker1;
00035 
00036 // Constants
00037     double reference2, reference1;
00038     double position2 = 0, position1 = 0;
00039     double m2_ref = 0, m1_ref = 0;
00040     int count = 0;
00041     double Grens2 = 90, Grens1 = 90;
00042     double Stapgrootte = 5;
00043 
00044 //Sample time (motor-step)
00045     const double m2_Ts = 0.01, m1_Ts = 0.01;
00046 
00047 //Controller gain Motor 2 & 1
00048     const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 2;
00049     const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2;
00050     double m2_err_int = 0, m2_prev_err = 0;
00051     double m1_err_int = 0, m1_prev_err = 0;
00052 
00053 //Derivative filter coeffs Motor 2 & 1
00054     const double BiGain2 = 0.016955, BiGain1 = 0.016955;
00055     const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2;
00056     const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1;
00057 
00058 // Filter variables
00059     double m2_f_v1 = 0, m2_f_v2 = 0;
00060     double m1_f_v1 = 0, m1_f_v2 = 0;
00061 
00062 //--------------------------------------------------------------------------------------------------------------------------//
00063 // General Functions
00064 //--------------------------------------------------------------------------------------------------------------------------//
00065 
00066 //HIDScope
00067     void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
00068     {
00069         scope.set(0, reference2 - position2);
00070         scope.set(1, position2);
00071         scope.set(2, reference1 - position1);    
00072         scope.set(3, position1);
00073         scope.send();
00074     
00075     }
00076 
00077 // Biquad filter
00078     double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
00079     {
00080         double v = u - a1*v1 - a2*v2;
00081         double y = b0*v + b1*v1 + b2*v2;
00082         v2 = v1; v1 = v;
00083         return y;
00084     }    
00085 
00086 
00087 // Reusable PID controller
00088     double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2,
00089         const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
00090     {
00091     // Derivative
00092         double e_der = (e-e_prev)/Ts;
00093         e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
00094         e_prev = e;
00095     // Integral
00096         e_int = e_int + Ts*e;
00097     // PID
00098         return Kp * e + Ki*e_int + Kd*e_der;
00099     }
00100 //--------------------------------------------------------------------------------------------------------------------------//
00101 // Motor control functions
00102 //--------------------------------------------------------------------------------------------------------------------------//
00103 
00104 // Motor2 control
00105     void motor2_Controller() 
00106     {
00107         // Setpoint motor 2
00108             reference2 = m2_ref;                           // Reference in degrees
00109             position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
00110         // Speed control
00111             double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
00112                 m2_f_b0, m2_f_b1, m2_f_b2);
00113             double m2_P2 = biquad(m2_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); // Filter of motorspeed input
00114             motor2speed = abs(m2_P2); 
00115         // Direction control
00116             if(m2_P2 > 0) 
00117             {    
00118                 motor2direction = 0;
00119             }
00120             else
00121             {
00122                 motor2direction = 1;
00123             }
00124     }   
00125 
00126 // Motor1 control
00127     void motor1_Controller() 
00128     {
00129         // Setpoint Motor 1
00130             reference1 = m1_ref;                           // Reference in degrees
00131             position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
00132         // Speed control
00133             double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, 
00134                 m1_f_b0, m1_f_b1, m1_f_b2); 
00135             double m1_P2 = biquad(m1_P1, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2); 
00136             motor1speed = abs(m1_P2); 
00137         // Direction control    
00138             if(m1_P2 > 0)
00139             {  
00140                 motor1direction = 1;
00141             }
00142             else
00143             {
00144                 motor1direction = 0;
00145             }
00146     }
00147 
00148 //--------------------------------------------------------------------------------------------------------------------------//
00149 // Main function
00150 //--------------------------------------------------------------------------------------------------------------------------//
00151 int main()
00152 {  
00153 //--------------------------------------------------------------------------------------------------------------------------//
00154 // Initalizing
00155 //--------------------------------------------------------------------------------------------------------------------------// 
00156     //LEDs OFF
00157         LedR = LedB = LedG = 1;
00158     
00159     //PC connection & check
00160         pc.baud(115200);
00161         pc.printf("Tot aan loop werkt\n");
00162     
00163     // Tickers
00164         ScopeTime.attach(&ScopeSend, 0.01f);                    // 100 Hz, Scope
00165         myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
00166         myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
00167     
00168 //--------------------------------------------------------------------------------------------------------------------------//
00169 // Control Program
00170 //--------------------------------------------------------------------------------------------------------------------------//
00171     while(true)
00172     {
00173         char c = pc.getc();
00174     // 1 Program UP
00175         if(c == 'e') 
00176         {
00177             count = count + 1;
00178             if(count > 2)
00179                 {
00180                     count = 2;
00181                 }
00182 
00183         }
00184      // 1 Program DOWN
00185         if(c == 'd')
00186         {
00187             count = count - 1;
00188             if(count < 0)
00189                 {
00190                     count = 0;
00191                 }
00192         }
00193     // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED      
00194         if(count == 0)
00195         {
00196                 
00197                 LedR = LedB = 1;
00198                 LedG = 0;
00199                 if(c == 'r')
00200                 {
00201                     m2_ref = m2_ref + Stapgrootte;
00202                     m1_ref = m1_ref - Stapgrootte;
00203                     if (m2_ref > Grens2)
00204                     {
00205                         m2_ref = Grens2;
00206                         m1_ref = -1*Grens1;
00207                     }
00208                 }
00209                 if(c == 'f')
00210                 {
00211                     m2_ref = m2_ref - Stapgrootte;
00212                     m1_ref = m1_ref + Stapgrootte;
00213                     if (m2_ref < -1*Grens2)
00214                     {
00215                         m2_ref = -1*Grens2;
00216                         m1_ref = Grens1;
00217                     }
00218                 }
00219         }
00220     // PROGRAM 1: Motor 1 control, Red LED
00221         if(count == 1) 
00222         {
00223                 LedG = LedB = 1;
00224                 LedR = 0;
00225                 if(c == 't')
00226                 {
00227                     m1_ref = m1_ref + Stapgrootte;
00228                     if (m1_ref > Grens1)
00229                     {
00230                         m1_ref = Grens1;
00231                     }
00232                 }
00233                 if(c == 'g')
00234                 {
00235                     m1_ref = m1_ref - Stapgrootte;
00236                     if (m1_ref < -1*Grens1)
00237                     {
00238                         m1_ref = -1*Grens1;
00239                     }
00240                 }
00241         }
00242     // PROGRAM 2: Firing mechanism & Reset, Blue LED
00243         if(count == 2) 
00244         {
00245 
00246                 LedR = LedG = 1;
00247                 LedB = 0;
00248                 //VUUUUR!! (To Do)
00249                 wait(1);
00250                 m2_ref = 0;
00251                 m1_ref = 0;   
00252         }
00253     }
00254 
00255 }