Test_cal.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "MODSERIAL.h"
00004 #include "QEI.h"
00005 #include "BiQuad.h"
00006 
00007 //Tickers
00008 Ticker TickerMeasureAndControl;
00009 Ticker TickerPrintToScreen;
00010 
00011 //Communication
00012 MODSERIAL pc(USBTX,USBRX);
00013 QEI Encoder(D10,D11,NC,32);
00014 
00015 //Global pin variables
00016 PwmOut PwmPin(D5);
00017 DigitalOut DirectionPin(D4);
00018 AnalogIn Potmeter1(A1);
00019 AnalogIn Potmeter2(A0);
00020 DigitalIn BUT1(D8); //Button for + arm rotation
00021 DigitalIn BUT2(D9); //Button for - arm rotation 
00022 DigitalIn button_set(SW3); //Button to set the 0 config
00023 DigitalIn button(SW2); //Button to return to 0-reference config
00024 //Global variables
00025 volatile bool PrintFlag = false;
00026 
00027 //Global variables for printing on screen
00028 volatile float PosRefPrint; // for printing value on screen
00029 volatile float PosMotorPrint; // for printing value on screen
00030 volatile float ErrorPrint;
00031 
00032 //-----------------------------------------------------------------------------
00033 //The double-functions
00034 
00035 //Calibration for arm drive
00036 //double GetReferencePosition()
00037 //{
00038   //  static double PositionRef=0;
00039 
00040     //if (BUT1==false) {
00041       //  PositionRef += 0.0005*(6.2830);
00042     //}
00043     //if (button_set == false) {
00044       //  PositionRef = 0;
00045        // Encoder.reset();
00046     //}
00047     //if BUT2 == false){
00048         //PositionRef -= 0.0005*(6.2830);
00049     //if (button == false) {
00050       //  if (PositionRef>=-0.52*(6.2830)) {
00051         //    PositionRef -=0.0005*(6.2830);
00052         //}
00053     //}
00054     //return PositionRef;
00055 //}
00056 
00057 //Calibration for belt drive
00058 double GetReferencePosition()
00059 {
00060     static double PositionRef=0;
00061 
00062     if (BUT1==false) {
00063         PositionRef += 0.0005*(6.2830);
00064     }
00065     if (button_set == false) {
00066         PositionRef = 0;
00067         Encoder.reset();
00068     }
00069     if (BUT2 == false){
00070         PositionRef -=0.0005*(6.2830);
00071     }
00072     if (button == false) {
00073         if (PositionRef<=0.733*(6.2830)) {
00074             PositionRef +=0.0005*(6.2830);
00075         }
00076     }
00077       return PositionRef;
00078 }
00079 
00080 // actual position of the motor
00081 double GetActualPosition()
00082 {
00083     //This function determines the actual position of the motor
00084     //The count:radians relation is 8400:2pi
00085     double EncoderCounts = Encoder.getPulses();    //number of counts
00086     double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
00087 
00088     return PositionMotor;
00089 }
00090 
00091 
00092 
00093 ///The controller
00094 double PID_Controller(double Error)
00095 {
00096     //Belt drive parameters
00097     double Ts = 0.01; //Sampling time 100 Hz
00098     double Kp = 11.1; // proportional gain
00099     double Ki = 11.2; //Integral gain
00100     double Kd = 5.5; //Differential gain
00101 
00102     //Arm drive parameters
00103     //double Ts = 0.01; //Sampling time 100 Hz
00104     //double Kp = 19.8; // proportional gain
00105     //double Ki = 19.9; //Intergral gain
00106     //double Kd = 9.8; //Differential gain.
00107 
00108     static double ErrorIntegral = 0;
00109     static double ErrorPrevious = Error;
00110     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
00111 
00112     //Proportional part:
00113     double u_k = Kp * Error;
00114 
00115     //Integral part:
00116     ErrorIntegral = ErrorIntegral + Error*Ts;
00117     double u_i = Ki * ErrorIntegral;
00118 
00119     //Derivative part:
00120     double ErrorDerivative = (Error - ErrorPrevious)/Ts;
00121     double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
00122     double u_d = Kd * FilteredErrorDerivative;
00123     ErrorPrevious = Error;
00124 
00125     // sum of parts and return it
00126     return u_k + u_i + u_d; //This will become the MotorValue
00127 }
00128 
00129 //Ticker function set motorvalues
00130 void SetMotor(double MotorValue)
00131 {
00132     if (MotorValue >=0) {
00133         DirectionPin = 1;
00134     } else {
00135         DirectionPin = 0;
00136     }
00137 
00138     if (fabs(MotorValue)>1) { // if error more than 1 radian, full duty cycle
00139         PwmPin = 1;
00140     } else {
00141         PwmPin = fabs(MotorValue);
00142     }
00143 }
00144 
00145 // ----------------------------------------------------------------------------
00146 //Ticker function
00147 void MeasureAndControl(void)
00148 {
00149     double PositionRef = GetReferencePosition();
00150     double PositionMotor = GetActualPosition();
00151     double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
00152     SetMotor(MotorValue);
00153 
00154     //for printing on screen
00155     PosRefPrint = PositionRef;
00156     PosMotorPrint = PositionMotor;
00157     ErrorPrint = PositionRef - PositionMotor;
00158 
00159 }
00160 
00161 
00162 
00163 void PrintToScreen()
00164 {
00165     PrintFlag = true;
00166 }
00167 
00168 
00169 //-----------------------------------------------------------------------------
00170 int main()
00171 {
00172     pc.baud(115200);
00173     pc.printf("Hello World\n\r");
00174     PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
00175     TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz
00176     TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen
00177 
00178     while (true) {
00179         if(PrintFlag) { // if-statement for printing every second four times to screen
00180             double KpPrint = 3.52;
00181             double KiPrint = 0.88;
00182             double KdPrint = 6.9;
00183             pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, Kp = %f, Ki = %f and Kd = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint,KiPrint,KdPrint);
00184             PrintFlag = false;
00185         }
00186     }
00187 }