main.cpp

00001  
00002 /***************************< File comment >***************************/  
00003 /* project:   Lamborghini48                                           */ 
00004 /* project description : mobile robot class                           */ 
00005 /*--------------------------------------------------------------------*/ 
00006 /* version : 0.1                                                      */ 
00007 /* board : NUCLEO-F411RE                                              */ 
00008 /* detail : DC motor control                                          */
00009 /*--------------------------------------------------------------------*/ 
00010 /* create : 26/10/2018                                                */ 
00011 /* programmer : Worasuchad Haomachai                                  */ 
00012 /**********************************************************************/ 
00013  
00014 /*--------------------------------------------------------------------*/ 
00015 /*                           Include file                             */ 
00016 /*--------------------------------------------------------------------*/
00017 #include "math.h"
00018 #include "QEI.h"                        // https://os.mbed.com/cookbook/QEI
00019 
00020 Serial pc(USBTX, USBRX);
00021 
00022 //Use X4 encoding.
00023 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
00024 //Use X2 encoding by default.
00025 QEI wheel (D2, D3, NC, 200, QEI::X4_ENCODING);
00026 
00027 /*--------------------------------------------------------------------*/ 
00028 /*                         Global variable                            */ 
00029 /*--------------------------------------------------------------------*/
00030 PwmOut pwm(D5);                         // this is the PWM pin for the motor for how much we move it to correct for its error
00031 DigitalOut dir1(D6);                    //these pins are to control the direction of the motor (clockwise/counter-clockwise)
00032 DigitalOut dir2(D7);
00033 
00034 double angle = 0;                       //set the angles
00035 
00036 double setpoint = 360;                  // 3072;        // I am setting it to move through 100 degrees
00037 double Kp = 0.05;                       // 0.015;       // you can set these constants however you like depending on trial & error
00038 double Ki = 0.001;                        // 0.000071;    // 0.000109;
00039 double Kd = 0.0;                        // 0.93866;     // 0.1009;
00040 
00041 float last_error = 0;
00042 float _error = 0;
00043 float changeError = 0;
00044 float totalError = 0;
00045 float pidTerm = 0;
00046 float pidTerm_scaled = 0;               // if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|
00047 
00048 /*--------------------------------------------------------------------*/ 
00049 /*                           prototype fun                            */ 
00050 /*--------------------------------------------------------------------*/
00051 void PIDcalculation();              // find PID value
00052 
00053 /*--------------------------------------------------------------------*/ 
00054 /*                              main                                  */ 
00055 /*--------------------------------------------------------------------*/   
00056 int main() {
00057     pc.baud(115200);
00058     while(1)
00059     {
00060         PIDcalculation();               // find PID value
00061         
00062         if (angle < setpoint) 
00063         {
00064             dir1 = 1;                   // Forward motion
00065             dir2 = 0;
00066         } 
00067         else 
00068         {
00069             dir1 = 0;                   // Reverse motion
00070             dir2 = 1;
00071         }
00072     
00073         pwm.write(pidTerm_scaled / 255.00f);
00074         
00075         // Serial.println("WHEEL ANGLE:");
00076         pc.printf("%.3f\t\t", setpoint);
00077         pc.printf("%.3f\n\r", angle);
00078         //pc.printf("%d\n\r", wheel.getPulses());
00079     }
00080 
00081 }
00082 /*--------------------------------------------------------------------*/ 
00083 /*                              PID                                   */ 
00084 /*--------------------------------------------------------------------*/ 
00085 void PIDcalculation(){
00086     /* 
00087     *  angle = ( 0.1171875 * count); // count to angle conversion = 360/(64*12*4)
00088     *  reduction ratio: 64:1 , pulses per revolution: 12CPR
00089     */
00090     
00091     angle = ( 0.004591 * wheel.getPulses());        // 360/(98*200*4)
00092     _error = setpoint - angle;
00093     
00094     changeError = _error - last_error;              //derivative term
00095     totalError += _error;                           //accumalate errors to find integral term
00096     pidTerm = (Kp * _error) + (Ki * totalError) + (Kd * changeError);//total gain
00097     
00098     // pidTerm = constrain(pidTerm, -255, 255);     //constraining to appropriate value
00099     if(pidTerm > 255)
00100     {
00101       pidTerm = 255;
00102     }
00103     else if(pidTerm < -255)
00104     {
00105       pidTerm = -255;  
00106     }
00107     
00108     pidTerm_scaled = abs(pidTerm);                  //make sure it's a positive value
00109     
00110     last_error = _error;
00111 }