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 = 1;
00043 
00044 //Sample time (motor-step)
00045     const double m2_Ts = 0.001, m1_Ts = 0.01;
00046 
00047 //Controller gain Motor 2 & 1
00048     const double m2_Kp = 2.1/57,m2_Ki = 3.9/57, m2_Kd = 0.1/57;
00049     const double m1_Kp = 2.1/57,m1_Ki = 3.9/57, m1_Kd = 0.1/57;
00050     double m2_err_int = 0, m2_prev_err = 0;
00051     double m1_err_int = 0, m1_prev_err = 0;
00052     
00053     const double BiGainEMG_L1= 0.231938;
00054     const double EMGL1_a1 = -0.25537145181, EMGL1_a2 = 0.18312488356, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = 2.00000000000*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
00055     
00056     biquadFilter m2_lowpass (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2);
00057 
00058     double x;
00059 //--------------------------------------------------------------------------------------------------------------------------//
00060 // General Functions
00061 //--------------------------------------------------------------------------------------------------------------------------//
00062 
00063 //HIDScope
00064     void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
00065     {
00066         scope.set(0, reference2 - position2);
00067         scope.set(1, position2);
00068         scope.set(2, reference1 - position1);    
00069         scope.set(3, position1);
00070         scope.set(4, x);
00071         scope.send();
00072     
00073     }  
00074 
00075 
00076 // Reusable PID controller
00077     double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev)
00078     {
00079     // Derivative
00080         double e_der = (e-e_prev)/Ts;
00081         e_der = m2_lowpass.step(e_der);
00082         e_prev = e;
00083     // Integral
00084         e_int = e_int + Ts*e;
00085     // PID
00086         x = Kp * e + Ki*e_int + Kd*e_der;
00087         return x;
00088     }
00089 //--------------------------------------------------------------------------------------------------------------------------//
00090 // Motor control functions
00091 //--------------------------------------------------------------------------------------------------------------------------//
00092 
00093 // Motor2 control
00094     void motor2_Controller() 
00095     {
00096         // Setpoint motor 2
00097             reference2 = m2_ref;                           // Reference in degrees
00098             position2 = 1.0*Encoder2.getPulses()*360.0/(32.0*131.0); // Position in degrees
00099         // Speed control
00100             double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err);
00101             double m2_P2 = m2_P1; // Filter of motorspeed input
00102             motor2speed = abs(m2_P2); 
00103         // Direction control
00104             if(m2_P2 > 0) 
00105             {    
00106                 motor2direction = 0;
00107             }
00108             else
00109             {
00110                 motor2direction = 1;
00111             }
00112     }   
00113 
00114 // Motor1 control
00115     void motor1_Controller() 
00116     {
00117         // Setpoint Motor 1
00118             reference1 = m1_ref;                           // Reference in degrees
00119             position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
00120         // Speed control
00121             double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err); 
00122             double m1_P2 = m1_P1; 
00123             motor1speed = abs(m1_P2); 
00124         // Direction control    
00125             if(m1_P2 > 0)
00126             {  
00127                 motor1direction = 1;
00128             }
00129             else
00130             {
00131                 motor1direction = 0;
00132             }
00133     }
00134 
00135 //--------------------------------------------------------------------------------------------------------------------------//
00136 // Main function
00137 //--------------------------------------------------------------------------------------------------------------------------//
00138 int main()
00139 {  
00140 //--------------------------------------------------------------------------------------------------------------------------//
00141 // Initalizing
00142 //--------------------------------------------------------------------------------------------------------------------------// 
00143     //LEDs OFF
00144         LedR = LedB = LedG = 1;
00145     
00146     //PC connection & check
00147         pc.baud(115200);
00148         pc.printf("Tot aan loop werkt\n");
00149     
00150     // Tickers
00151         ScopeTime.attach(&ScopeSend, 0.01f);                    // 100 Hz, Scope
00152         myControllerTicker2.attach(&motor2_Controller, m2_Ts ); // 1000 Hz, Motor 2
00153         myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
00154     
00155 //--------------------------------------------------------------------------------------------------------------------------//
00156 // Control Program
00157 //--------------------------------------------------------------------------------------------------------------------------//
00158     while(true)
00159     {
00160         char c = pc.getc();
00161     // 1 Program UP
00162         if(c == 'e') 
00163         {
00164             count = count + 1;
00165             if(count > 2)
00166                 {
00167                     count = 2;
00168                 }
00169 
00170         }
00171      // 1 Program DOWN
00172         if(c == 'd')
00173         {
00174             count = count - 1;
00175             if(count < 0)
00176                 {
00177                     count = 0;
00178                 }
00179         }
00180     // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED      
00181         if(count == 0)
00182         {
00183                 
00184                 LedR = LedB = 1;
00185                 LedG = 0;
00186                 if(c == 'r')
00187                 {
00188                     m2_ref = m2_ref + Stapgrootte;
00189                     m1_ref = m1_ref + Stapgrootte;
00190                     if (m2_ref > Grens2)
00191                     {
00192                         m2_ref = Grens2;
00193                         m1_ref = Grens1;
00194                     }
00195                 }
00196                 if(c == 'f')
00197                 {
00198                     m2_ref = m2_ref - Stapgrootte;
00199                     m1_ref = m1_ref - Stapgrootte;
00200                     if (m2_ref < -1*Grens2)
00201                     {
00202                         m2_ref = -1*Grens2;
00203                         m1_ref = -1*Grens1;
00204                     }
00205                 }
00206         }
00207     // PROGRAM 1: Motor 1 control, Red LED
00208         if(count == 1) 
00209         {
00210                 LedG = LedB = 1;
00211                 LedR = 0;
00212                 if(c == 't')
00213                 {
00214                     m1_ref = m1_ref + Stapgrootte;
00215                     if (m1_ref > Grens1)
00216                     {
00217                         m1_ref = Grens1;
00218                     }
00219                 }
00220                 if(c == 'g')
00221                 {
00222                     m1_ref = m1_ref - Stapgrootte;
00223                     if (m1_ref < -1*Grens1)
00224                     {
00225                         m1_ref = -1*Grens1;
00226                     }
00227                 }
00228         }
00229     // PROGRAM 2: Firing mechanism & Reset, Blue LED
00230         if(count == 2) 
00231         {
00232 
00233                 LedR = LedG = 1;
00234                 LedB = 0;
00235                 //VUUUUR!! (To Do)
00236                 wait(1);
00237                 m2_ref = 0;
00238                 m1_ref = 0;   
00239         }
00240     }
00241 
00242 }