main.cpp

00001 // KINEMATICS + PID + MOTOR CONTROL
00002 
00003 //----------------~INITIATING-------------------------
00004 #include "mbed.h"
00005 
00006 // KINEMATICS          --       DEPENDENCIES
00007 #include "stdio.h"
00008 #define _USE_MATH_DEFINES
00009 #include <math.h>
00010 #define M_PI    3.14159265358979323846 /* pi */
00011 
00012 // PID CONTROLLER      --      DEPENDENCIES
00013 #include "BiQuad.h"
00014 #include "QEI.h"
00015 #include "MODSERIAL.h"
00016 #include "HIDScope.h"
00017 //#include "Math.h"
00018 
00019 // PID  CONTROLLER     --        PIN DEFENITIONS 
00020 AnalogIn button2(A4);
00021 AnalogIn button1(A3);
00022 DigitalIn button3(SW2);
00023 DigitalIn button4(SW3);
00024 
00025 DigitalOut directionpin1(D7);   // motor 1
00026 DigitalOut directionpin2(D4);   // motor 2
00027 DigitalOut directionpin3(D13);  // motor 3
00028 PwmOut pwmpin1(D6);             // motor 1
00029 PwmOut pwmpin2(D5);             // motor 2
00030 PwmOut pwmpin3(D12);            // motor 3
00031 
00032 QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
00033 QEI encoder2 (D11, D10, NC, 8400, QEI::X4_ENCODING); // motor 2
00034 QEI encoder3 (D3, D2, NC, 8400, QEI::X4_ENCODING);  // motor 3
00035 MODSERIAL pc(USBTX, USBRX);
00036 HIDScope scope(2);
00037 
00038 // TICKERS
00039 Ticker ref_rot;
00040 Ticker show_counts;
00041 Ticker Scope_Data;
00042 
00043 //----------------GLOBALS--------------------------
00044 
00045 // CONSTANTS PID CONTROLLER
00046 double PI = M_PI;// CHANGE THIS INTO M_PI
00047 double Kp = 14; //200 , 50
00048 double Ki = 0;   //1, 0.5
00049 double Kd = 3; //200, 10
00050 double Ts = 0.1; // Sample time in seconds
00051 double reference_rotation; //define as radians
00052 double motor_position;
00053 bool AlwaysTrue;
00054 
00055 //CONSTANTS KINEMATICS
00056 // constants
00057 const float la = 0.256;         // lengte actieve arm
00058 const float lp = 0.21;          // lengte passieve arm
00059 const float rp = 0.052;         // straal van midden end effector tot hoekpunt
00060 const float rm = 0.23;          // straal van global midden tot motor
00061 const float a = 0.09;           // zijde van de driehoek
00062 const float xas = 0.40;         // afstand van motor 1 tot motor 3
00063 const float yas = 0.346;        // afstand van xas tot motor 2
00064 const float thetap = 0;         // rotatiehoek van de end effector
00065 
00066 
00067 // motor locatie
00068 const int a1x = 0;              //x locatie motor 1
00069 const int a1y = 0;              //y locatie motor 1
00070 const float a2x = (0.5)*xas;    // x locatie motor 2
00071 const float a2y = yas;          // y locatie motor 2
00072 const float a3x = xas;          // x locatie motor 3
00073 const int a3y = 0;              // y locatie motor 3
00074 
00075 // script voor het bepalen van de desired position aan de hand van emg (1/0)
00076 
00077 //  EMG OUTPUT
00078 int EMGxplus;
00079 int EMGxmin ;
00080 int EMGyplus;
00081 int EMGymin ;
00082 
00083 // Dit moet experimenteel geperfectioneerd worden
00084 float tijdstap = 0.005;      //nu wss heel langzaam, kan miss omhoog KEER V GEEFT VERANDERING IN POSITIE
00085 float v = 0.1;                // snelheid kan wss ook hoger
00086 
00087 float px = 0.2;     //starting x    // BOUNDARIES
00088 float py = 0.155;   // starting y   // BOUNDARIES
00089 
00090 // verschil horizontale as met de actieve arm
00091 float da1 = 1.619685; // verschil a1 hoek en motor
00092 float da2 = -0.609780;
00093 float da3 = 3.372859;
00094 
00095 // limits (since no forward kinematics)
00096 float upperxlim = 0.275; //36, 0.04, 0.315, -0.085niet helemaal naar requierments ff kijken of ie groter kan
00097 float lowerxlim = 0.10;
00098 float upperylim = 0.225;
00099 float lowerylim = 0.03; //0.03 is goed
00100 
00101 
00102 //----------------FUNCTIONS--------------------------
00103 
00104 // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~
00105 
00106 // functie x positie
00107 float positionx(int EMGxplus,int EMGxmin)
00108 {
00109     float EMGx = EMGxplus - EMGxmin;
00110 
00111     float verplaatsingx = EMGx * tijdstap * v;
00112     float pxnieuw = px + verplaatsingx;
00113     // x limit
00114     if (pxnieuw <= upperxlim && pxnieuw >= lowerxlim)
00115         {
00116         px = pxnieuw;
00117         }
00118     else
00119         {
00120         if (pxnieuw >= lowerxlim)
00121         {
00122         px = upperxlim;
00123         }
00124     else
00125         {
00126         px = lowerxlim;
00127         }
00128     }
00129 //printf("X eindpunt (%f) en verplaatsing: (%f)\n\r",px,verplaatsingx);
00130 return px;
00131 }
00132 
00133 
00134 // functie y positie
00135 float positiony(int EMGyplus,int EMGymin)
00136 {
00137     float EMGy = EMGyplus - EMGymin;
00138 
00139     float verplaatsingy = EMGy * tijdstap * v;
00140     float pynieuw = py + verplaatsingy;
00141 
00142     // y limit
00143     if (pynieuw <= upperylim && pynieuw >= lowerylim)
00144         {
00145         py = pynieuw;
00146         }
00147     else
00148         {
00149             if (pynieuw >= lowerylim)
00150             {
00151             py = upperylim;
00152             }
00153             else
00154             {
00155             py = lowerylim;
00156             }
00157         }
00158 //printf("Y eindpunt (%f) en verplaatsing: (%f) \n\r",py,verplaatsingy);
00159 return (py);
00160 }
00161 
00162 
00163 //~~~~~~~~~~~~CALCULATIING MOTOR ANGLES ~~~~~~~~
00164 // arm 1 --> reference angle motor 1
00165 float hoek1(float px, float py) // input: ref x, ref y
00166 {
00167     float c1x =  px - rp * cos(thetap +(M_PI/6));       // x locatie hoekpunt end-effector
00168     float c1y = py - rp*sin(thetap+(M_PI/6));           // y locatie hoekpunt end-effector
00169     float alpha1 = atan2((c1y-a1y),(c1x-a1x));          // hoek tussen horizontaal en lijn van motor naar bijbehorende end-effector punt
00170     float psi1 = acos(( pow(la,2)-pow(lp,2)+pow((c1x-a1x),2)+pow((c1y-a1y),2))/(2*la*sqrt(pow ((c1x-a1x),2)+pow((c1y-a1y),2) ))); //Hoek tussen lijn van motor naar bijbehorende end=effector punt en actieve arm
00171     float a1 = alpha1 + psi1 - da1;                          //hoek tussen horizontaal en actieve arm
00172     //printf("arm 1 = %f \n\r",a1);
00173     return a1;
00174 }
00175 
00176 // arm 2 --> reference angle motor 2
00177 float hoek2(float px, float py)
00178 {
00179     float c2x =  px - rp * cos(thetap -(M_PI/2)); 
00180     float c2y = py - rp*sin(thetap-(M_PI/2)); 
00181     float alpha2 = atan2((c2y-a2y),(c2x-a2x)); 
00182     float psi2 = acos(( pow(la,2)-pow(lp,2)+pow((c2x-a2x),2)+pow((c2y-a2y),2))/(2*la*sqrt(pow ((c2x-a2x),2)+pow((c2y-a2y),2) )));
00183     float a2 = alpha2 + psi2 - da2;   
00184     //printf("arm 2 = %f \n\r",a2);
00185     return a2;
00186 }
00187 
00188 //arm 3 --> reference angle motor 3
00189 float hoek3(float px, float py)
00190 {
00191     float c3x =  px - rp * cos(thetap +(5*M_PI/6)); 
00192     float c3y = py - rp*sin(thetap+(5*M_PI/6)); 
00193     float alpha3 = atan2((c3y-a3y),(c3x-a3x)); 
00194     float psi3 = acos(( pow(la,2)-pow(lp,2)+pow((c3x-a3x),2)+pow((c3y-a3y),2))/(2*la*sqrt(pow ((c3x-a3x),2)+pow((c3y-a3y),2) ))); 
00195     float a3 = alpha3 + psi3 - da3;   
00196     //printf("arm 3 = %f \n\r",a3);
00197     return a3;
00198 }
00199 
00200 // ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~
00201 
00202 double PID_controller(double error)
00203 {
00204     static double error_integral = 0;
00205     static double error_prev = error; // initialization with this value only done once!
00206     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
00207 
00208     // Proportional part:
00209     double u_k = Kp * error;
00210 
00211     // Integral part
00212     error_integral = error_integral + error * Ts;
00213     double u_i = Ki * error_integral;
00214 
00215     // Derivative part
00216     double error_derivative = (error - error_prev)/Ts;
00217     double filtered_error_derivative = LowPassFilter.step(error_derivative);
00218     double u_d = Kd * filtered_error_derivative;
00219     error_prev = error;
00220 
00221     // Sum all parts and return it
00222     return u_k + u_i + u_d;
00223 }
00224 
00225 
00226 // DIRECTON AND SPEED CONTROL
00227 void moter_control(double u)
00228 {
00229     
00230     directionpin1= u > 0.0f; //eithertrueor false
00231     if (fabs(u)> 0.7f){
00232         u = 0.7f;
00233         }
00234     else {
00235         u= u;
00236         }      
00237     pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
00238 }
00239 
00240 void moter2_control(double u)
00241 {
00242     directionpin2= u > 0.0f; //eithertrueor false
00243         if (fabs(u)> 0.7f){
00244         u = 0.7f;
00245         }
00246     else {
00247         u= u;
00248         }  
00249     pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
00250 }
00251 
00252 void moter3_control(double u)
00253 {
00254     directionpin3= u > 0.0f; //eithertrueor false
00255     if (fabs(u)> 0.7f){
00256         u = 0.7f;
00257         }
00258     else {
00259         u= u;
00260         }  
00261     pwmpin3 = fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
00262 }
00263 
00264 // CONTROLLING THE MOTOR
00265 void Motor_mover()
00266 {
00267     double motor_position = encoder1.getPulses(); //output in counts
00268     double reference_rotation = hoek1(px, py);
00269     double error = reference_rotation - motor_position*(2*PI)/8400;
00270     double u = PID_controller(error);
00271     moter_control(u);
00272     
00273     double motor_position2 = encoder2.getPulses(); //output in counts
00274     double reference_rotation2 = hoek2(px, py);
00275     double error_2 = reference_rotation2 - motor_position2*(2*PI)/8400;
00276     double u_2 = PID_controller(error_2);
00277     moter2_control(u_2);
00278     
00279     double motor_position3 = encoder3.getPulses(); //output in counts
00280     double reference_rotation3 = hoek3(px, py);
00281     double error_3 = reference_rotation3 - motor_position3*(2*PI)/8400;
00282     double u_3 = PID_controller(error_3);
00283     moter3_control(u_3);  
00284     
00285     
00286 }
00287 
00288 //PRINT TICKER
00289 void PrintFlag()
00290 {
00291     AlwaysTrue = true;
00292 }
00293 
00294 // HIDSCOPE
00295 void ScopeData()
00296 {
00297     double y = encoder1.getPulses();
00298     scope.set(0, y);
00299     scope.send();
00300 }
00301 
00302 
00303 //----------------------MAIN---------------------------------
00304 int main()
00305 {
00306     // ~~~~~~~~~~~~~~~~ INITIATING ~~~~~~~~~~~~
00307         pwmpin1.period_us(60); // setup motor
00308         
00309         // setup printing service
00310         pc.baud(9600);
00311         pc.printf("test");
00312         
00313         // Tickers
00314         //show_counts.attach(PrintFlag, 0.2);
00315         ref_rot.attach(Motor_mover, 0.001);
00316         //Scope_Data.attach(ScopeData, 0.01);
00317         
00318 
00319 while(true){ 
00320  
00321         
00322 if (button2 == false)
00323 {
00324    wait(0.01f);
00325         
00326         // berekenen positie
00327            float px = positionx(1,0);  // EMG: +x, -x
00328             float py = positiony(0,0);  // EMG: +y, -y
00329             //printf("positie (%f,%f)\n\r",px,py);
00330                 }        
00331 
00332 if (button1 == false){
00333     wait(0.01f);
00334             // berekenen positie
00335            float px = positionx(0,1);  // EMG: +x, -x
00336             float py = positiony(0,0);  // EMG: +y, -y
00337             //printf("positie (%f,%f)\n\r",px,py);
00338             }
00339 
00340 if (button3 == false){
00341     wait(0.01f);
00342             // berekenen positie
00343            float px = positionx(0,0);  // EMG: +x, -x
00344             float py = positiony(1,0);  // EMG: +y, -y
00345             //printf("positie (%f,%f)\n\r",px,py);
00346             }
00347 
00348 if (button4 == false){
00349     wait(0.01f);
00350             // berekenen positie
00351            float px = positionx(0,0);  // EMG: +x, -x
00352             float py = positiony(0,1);  // EMG: +y, -y
00353             //printf("positie (%f,%f)\n\r",px,py);
00354             }
00355 
00356 }
00357 
00358 
00359 // berekenen hoeken
00360 /*
00361 float a1 = hoek1(px, py);
00362 float a2 = hoek2(px, py);
00363 float a3 = hoek3(px, py);
00364 
00365 //printf("hoek(%f,%f,%f)\n\r",a1,a2,a3);
00366 
00367  return 0;
00368 */
00369 }