main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 #include "QEI.h"
00004 #include "math.h"
00005 #define _USE_MATH_DEFINES
00006 # define M_PI           3.14159265358979323846  /* pi */
00007 
00008 double Kp = 1;
00009 double Ki = 1;
00010 double u_k = 1;
00011 double u_i = 1;
00012 double Kswitch;
00013 double kp_scalar = 1;
00014 double ki_scalar = 1;
00015 double y_ref=0;
00016 double motor1_ang=0;
00017 double y_ref_scalar = 0;
00018 double input_motor;
00019 bool input_switch = true;
00020 const double Ts = 0.01f;
00021 
00022 Ticker Control;
00023 
00024 PwmOut  motor1_pwm(D5);
00025 PwmOut  motor2_pwm(D6);
00026 DigitalOut motor1_dir(D4);
00027 DigitalOut motor2_dir(D7);
00028 AnalogIn pot1(A0);
00029 AnalogIn pot2(A1);
00030 
00031 QEI motor1_enc(D12,D13,NC,8400,QEI::X4_ENCODING);
00032 QEI motor2_enc(D10,D11,NC,8400,QEI::X4_ENCODING);  
00033 
00034 InterruptIn  sw2(SW2);
00035 InterruptIn  sw3(SW3);
00036 
00037 MODSERIAL pc(USBTX, USBRX);
00038 
00039 //hoek uitrekenen van de motor
00040 double countstoangle(int counts){
00041     double angle;
00042     int counts_abs =(counts);
00043     angle = (2 * M_PI * counts_abs)/8400;
00044     return angle;
00045     
00046 }
00047 void buttonpress()
00048 {
00049     input_switch=!input_switch;             //verander de potmeter input tussen kp en ki
00050 }
00051 
00052 double PID_controller(double error)         //bereken de input die naar de motor wordt gestuurd aan de hand van kp en ki en de error op elk moment
00053             {
00054             //Proportional part
00055             u_k = Kp*error;
00056             
00057             //Integral part
00058             static double error_integral = 0;
00059             error_integral = error_integral + error * Ts;
00060             u_i = Ki * error_integral;
00061             return u_k + u_i;
00062             }
00063 
00064 
00065 void aansturing()
00066     {       
00067             y_ref_scalar = pot2.read();     //altijd de potmeters uitlezen, buiten de ticker
00068             Kswitch = pot1.read();
00069             
00070             //kijken of er gekozen is voor het uitlezen van de kp of ki van de potmeter
00071             if(input_switch==true)
00072             {kp_scalar = Kswitch;}
00073             else
00074             {ki_scalar = Kswitch;}
00075             
00076             
00077             //waarde voor de inputcontrol value instellen aan de hand van potmeter
00078             Kp = kp_scalar*20.0f;
00079             Ki = ki_scalar*2.0f;
00080             
00081             y_ref = y_ref_scalar*2.0f*M_PI;                          // de postitie waar je naartoe wilt instellen tussen 0 en 2pi aan de hand van de potmeter waarde
00082             motor1_ang = countstoangle(motor1_enc.getPulses());      // de hoek van de motor uit bovenstaande formule halen, om de error te kunnen berekenen
00083             double error = motor1_ang -y_ref;
00084             
00085             
00086             double input_motor = PID_controller(error);
00087             
00088             if(input_motor>1.0)                 //pwm signaal kan niet groter zijn dan 1, dus moet gelimiteerd worden
00089                 {input_motor = 1.0;}
00090             else if(input_motor<-1.0)           //absolute waarde wordt ingevuld, maar de negatieve kant is belangrijk voor de richting van de motor
00091                 {input_motor = -1.0;}
00092             
00093                     
00094             if(input_motor<0.0)                 //bepaal de richting aan de hand van het teken van het pwm signaal.
00095                 {motor1_dir = 0;}
00096             else                                
00097                 {motor1_dir = 1;}                      
00098             motor1_pwm.write(fabs(input_motor));    // de uiteindelijke input aan de motor geven 
00099  //   pc.printf("ki: %f  kp: %f  yref: %f     error: %f   u_k: %f  angle: %f\r\n",Ki, Kp, y_ref,error,u_k,motor1_ang);
00100     }
00101 
00102 int main()
00103 {
00104     
00105     sw2.fall(buttonpress);
00106     pc.baud(115200);
00107     pc.printf("\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n ~~~A$$De$troyer69~~~ \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n");
00108     
00109     motor1_pwm.period_us(60);
00110     motor1_pwm = 0;  
00111     motor2_pwm.period_us(60);
00112     motor2_pwm = 0;  
00113     
00114     Control.attach(aansturing, Ts);         //de aansturing regelen met een sampletime
00115     
00116     while (true)
00117     {       
00118             if(input_switch==true)
00119             {pc.printf("Kp ingeschakeld\r\n");}
00120             else
00121             {pc.printf("Ki ingeschakeld\r\n");}
00122             double errorprint = motor1_ang-y_ref;
00123                 pc.printf("ki: %f   kp: %f  yref: %f    u_k: %f angle: %f   error: %f\r\n",Ki, Kp, y_ref,u_k,motor1_ang,errorprint);
00124 
00125             wait(0.5);  
00126     }
00127 }
00128 
00129