Calibration code for the motors
Dependencies: DHT MODSERIAL QEI biquadFilter mbed
Test_cal.cpp@0:84e285dc97bd, 2018-10-30 (annotated)
- Committer:
- Sven_van_Wincoop
- Date:
- Tue Oct 30 12:27:19 2018 +0000
- Revision:
- 0:84e285dc97bd
Calibration of motors with V1 values
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Sven_van_Wincoop | 0:84e285dc97bd | 1 | #include "mbed.h" |
Sven_van_Wincoop | 0:84e285dc97bd | 2 | #include "math.h" |
Sven_van_Wincoop | 0:84e285dc97bd | 3 | #include "MODSERIAL.h" |
Sven_van_Wincoop | 0:84e285dc97bd | 4 | #include "QEI.h" |
Sven_van_Wincoop | 0:84e285dc97bd | 5 | #include "BiQuad.h" |
Sven_van_Wincoop | 0:84e285dc97bd | 6 | |
Sven_van_Wincoop | 0:84e285dc97bd | 7 | //Tickers |
Sven_van_Wincoop | 0:84e285dc97bd | 8 | Ticker TickerMeasureAndControl; |
Sven_van_Wincoop | 0:84e285dc97bd | 9 | Ticker TickerPrintToScreen; |
Sven_van_Wincoop | 0:84e285dc97bd | 10 | |
Sven_van_Wincoop | 0:84e285dc97bd | 11 | //Communication |
Sven_van_Wincoop | 0:84e285dc97bd | 12 | MODSERIAL pc(USBTX,USBRX); |
Sven_van_Wincoop | 0:84e285dc97bd | 13 | QEI Encoder(D10,D11,NC,32); |
Sven_van_Wincoop | 0:84e285dc97bd | 14 | |
Sven_van_Wincoop | 0:84e285dc97bd | 15 | //Global pin variables |
Sven_van_Wincoop | 0:84e285dc97bd | 16 | PwmOut PwmPin(D5); |
Sven_van_Wincoop | 0:84e285dc97bd | 17 | DigitalOut DirectionPin(D4); |
Sven_van_Wincoop | 0:84e285dc97bd | 18 | AnalogIn Potmeter1(A1); |
Sven_van_Wincoop | 0:84e285dc97bd | 19 | AnalogIn Potmeter2(A0); |
Sven_van_Wincoop | 0:84e285dc97bd | 20 | DigitalIn BUT1(D8); //Button for + arm rotation |
Sven_van_Wincoop | 0:84e285dc97bd | 21 | DigitalIn BUT2(D9); //Button for - arm rotation |
Sven_van_Wincoop | 0:84e285dc97bd | 22 | DigitalIn button_set(SW3); //Button to set the 0 config |
Sven_van_Wincoop | 0:84e285dc97bd | 23 | DigitalIn button(SW2); //Button to return to 0-reference config |
Sven_van_Wincoop | 0:84e285dc97bd | 24 | //Global variables |
Sven_van_Wincoop | 0:84e285dc97bd | 25 | volatile bool PrintFlag = false; |
Sven_van_Wincoop | 0:84e285dc97bd | 26 | |
Sven_van_Wincoop | 0:84e285dc97bd | 27 | //Global variables for printing on screen |
Sven_van_Wincoop | 0:84e285dc97bd | 28 | volatile float PosRefPrint; // for printing value on screen |
Sven_van_Wincoop | 0:84e285dc97bd | 29 | volatile float PosMotorPrint; // for printing value on screen |
Sven_van_Wincoop | 0:84e285dc97bd | 30 | volatile float ErrorPrint; |
Sven_van_Wincoop | 0:84e285dc97bd | 31 | |
Sven_van_Wincoop | 0:84e285dc97bd | 32 | //----------------------------------------------------------------------------- |
Sven_van_Wincoop | 0:84e285dc97bd | 33 | //The double-functions |
Sven_van_Wincoop | 0:84e285dc97bd | 34 | |
Sven_van_Wincoop | 0:84e285dc97bd | 35 | //Calibration for arm drive |
Sven_van_Wincoop | 0:84e285dc97bd | 36 | //double GetReferencePosition() |
Sven_van_Wincoop | 0:84e285dc97bd | 37 | //{ |
Sven_van_Wincoop | 0:84e285dc97bd | 38 | // static double PositionRef=0; |
Sven_van_Wincoop | 0:84e285dc97bd | 39 | |
Sven_van_Wincoop | 0:84e285dc97bd | 40 | //if (BUT1==false) { |
Sven_van_Wincoop | 0:84e285dc97bd | 41 | // PositionRef += 0.0005*(6.2830); |
Sven_van_Wincoop | 0:84e285dc97bd | 42 | //} |
Sven_van_Wincoop | 0:84e285dc97bd | 43 | //if (button_set == false) { |
Sven_van_Wincoop | 0:84e285dc97bd | 44 | // PositionRef = 0; |
Sven_van_Wincoop | 0:84e285dc97bd | 45 | // Encoder.reset(); |
Sven_van_Wincoop | 0:84e285dc97bd | 46 | //} |
Sven_van_Wincoop | 0:84e285dc97bd | 47 | //if BUT2 == false){ |
Sven_van_Wincoop | 0:84e285dc97bd | 48 | //PositionRef -= 0.0005*(6.2830); |
Sven_van_Wincoop | 0:84e285dc97bd | 49 | //if (button == false) { |
Sven_van_Wincoop | 0:84e285dc97bd | 50 | // if (PositionRef>=-0.52*(6.2830)) { |
Sven_van_Wincoop | 0:84e285dc97bd | 51 | // PositionRef -=0.0005*(6.2830); |
Sven_van_Wincoop | 0:84e285dc97bd | 52 | //} |
Sven_van_Wincoop | 0:84e285dc97bd | 53 | //} |
Sven_van_Wincoop | 0:84e285dc97bd | 54 | //return PositionRef; |
Sven_van_Wincoop | 0:84e285dc97bd | 55 | //} |
Sven_van_Wincoop | 0:84e285dc97bd | 56 | |
Sven_van_Wincoop | 0:84e285dc97bd | 57 | //Calibration for belt drive |
Sven_van_Wincoop | 0:84e285dc97bd | 58 | double GetReferencePosition() |
Sven_van_Wincoop | 0:84e285dc97bd | 59 | { |
Sven_van_Wincoop | 0:84e285dc97bd | 60 | static double PositionRef=0; |
Sven_van_Wincoop | 0:84e285dc97bd | 61 | |
Sven_van_Wincoop | 0:84e285dc97bd | 62 | if (BUT1==false) { |
Sven_van_Wincoop | 0:84e285dc97bd | 63 | PositionRef += 0.0005*(6.2830); |
Sven_van_Wincoop | 0:84e285dc97bd | 64 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 65 | if (button_set == false) { |
Sven_van_Wincoop | 0:84e285dc97bd | 66 | PositionRef = 0; |
Sven_van_Wincoop | 0:84e285dc97bd | 67 | Encoder.reset(); |
Sven_van_Wincoop | 0:84e285dc97bd | 68 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 69 | if (BUT2 == false){ |
Sven_van_Wincoop | 0:84e285dc97bd | 70 | PositionRef -=0.0005*(6.2830); |
Sven_van_Wincoop | 0:84e285dc97bd | 71 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 72 | if (button == false) { |
Sven_van_Wincoop | 0:84e285dc97bd | 73 | if (PositionRef<=0.733*(6.2830)) { |
Sven_van_Wincoop | 0:84e285dc97bd | 74 | PositionRef +=0.0005*(6.2830); |
Sven_van_Wincoop | 0:84e285dc97bd | 75 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 76 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 77 | return PositionRef; |
Sven_van_Wincoop | 0:84e285dc97bd | 78 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 79 | |
Sven_van_Wincoop | 0:84e285dc97bd | 80 | // actual position of the motor |
Sven_van_Wincoop | 0:84e285dc97bd | 81 | double GetActualPosition() |
Sven_van_Wincoop | 0:84e285dc97bd | 82 | { |
Sven_van_Wincoop | 0:84e285dc97bd | 83 | //This function determines the actual position of the motor |
Sven_van_Wincoop | 0:84e285dc97bd | 84 | //The count:radians relation is 8400:2pi |
Sven_van_Wincoop | 0:84e285dc97bd | 85 | double EncoderCounts = Encoder.getPulses(); //number of counts |
Sven_van_Wincoop | 0:84e285dc97bd | 86 | double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding |
Sven_van_Wincoop | 0:84e285dc97bd | 87 | |
Sven_van_Wincoop | 0:84e285dc97bd | 88 | return PositionMotor; |
Sven_van_Wincoop | 0:84e285dc97bd | 89 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 90 | |
Sven_van_Wincoop | 0:84e285dc97bd | 91 | |
Sven_van_Wincoop | 0:84e285dc97bd | 92 | |
Sven_van_Wincoop | 0:84e285dc97bd | 93 | ///The controller |
Sven_van_Wincoop | 0:84e285dc97bd | 94 | double PID_Controller(double Error) |
Sven_van_Wincoop | 0:84e285dc97bd | 95 | { |
Sven_van_Wincoop | 0:84e285dc97bd | 96 | //Belt drive parameters |
Sven_van_Wincoop | 0:84e285dc97bd | 97 | double Ts = 0.01; //Sampling time 100 Hz |
Sven_van_Wincoop | 0:84e285dc97bd | 98 | double Kp = 11.1; // proportional gain |
Sven_van_Wincoop | 0:84e285dc97bd | 99 | double Ki = 11.2; //Integral gain |
Sven_van_Wincoop | 0:84e285dc97bd | 100 | double Kd = 5.5; //Differential gain |
Sven_van_Wincoop | 0:84e285dc97bd | 101 | |
Sven_van_Wincoop | 0:84e285dc97bd | 102 | //Arm drive parameters |
Sven_van_Wincoop | 0:84e285dc97bd | 103 | //double Ts = 0.01; //Sampling time 100 Hz |
Sven_van_Wincoop | 0:84e285dc97bd | 104 | //double Kp = 19.8; // proportional gain |
Sven_van_Wincoop | 0:84e285dc97bd | 105 | //double Ki = 19.9; //Intergral gain |
Sven_van_Wincoop | 0:84e285dc97bd | 106 | //double Kd = 9.8; //Differential gain. |
Sven_van_Wincoop | 0:84e285dc97bd | 107 | |
Sven_van_Wincoop | 0:84e285dc97bd | 108 | static double ErrorIntegral = 0; |
Sven_van_Wincoop | 0:84e285dc97bd | 109 | static double ErrorPrevious = Error; |
Sven_van_Wincoop | 0:84e285dc97bd | 110 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
Sven_van_Wincoop | 0:84e285dc97bd | 111 | |
Sven_van_Wincoop | 0:84e285dc97bd | 112 | //Proportional part: |
Sven_van_Wincoop | 0:84e285dc97bd | 113 | double u_k = Kp * Error; |
Sven_van_Wincoop | 0:84e285dc97bd | 114 | |
Sven_van_Wincoop | 0:84e285dc97bd | 115 | //Integral part: |
Sven_van_Wincoop | 0:84e285dc97bd | 116 | ErrorIntegral = ErrorIntegral + Error*Ts; |
Sven_van_Wincoop | 0:84e285dc97bd | 117 | double u_i = Ki * ErrorIntegral; |
Sven_van_Wincoop | 0:84e285dc97bd | 118 | |
Sven_van_Wincoop | 0:84e285dc97bd | 119 | //Derivative part: |
Sven_van_Wincoop | 0:84e285dc97bd | 120 | double ErrorDerivative = (Error - ErrorPrevious)/Ts; |
Sven_van_Wincoop | 0:84e285dc97bd | 121 | double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); |
Sven_van_Wincoop | 0:84e285dc97bd | 122 | double u_d = Kd * FilteredErrorDerivative; |
Sven_van_Wincoop | 0:84e285dc97bd | 123 | ErrorPrevious = Error; |
Sven_van_Wincoop | 0:84e285dc97bd | 124 | |
Sven_van_Wincoop | 0:84e285dc97bd | 125 | // sum of parts and return it |
Sven_van_Wincoop | 0:84e285dc97bd | 126 | return u_k + u_i + u_d; //This will become the MotorValue |
Sven_van_Wincoop | 0:84e285dc97bd | 127 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 128 | |
Sven_van_Wincoop | 0:84e285dc97bd | 129 | //Ticker function set motorvalues |
Sven_van_Wincoop | 0:84e285dc97bd | 130 | void SetMotor(double MotorValue) |
Sven_van_Wincoop | 0:84e285dc97bd | 131 | { |
Sven_van_Wincoop | 0:84e285dc97bd | 132 | if (MotorValue >=0) { |
Sven_van_Wincoop | 0:84e285dc97bd | 133 | DirectionPin = 1; |
Sven_van_Wincoop | 0:84e285dc97bd | 134 | } else { |
Sven_van_Wincoop | 0:84e285dc97bd | 135 | DirectionPin = 0; |
Sven_van_Wincoop | 0:84e285dc97bd | 136 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 137 | |
Sven_van_Wincoop | 0:84e285dc97bd | 138 | if (fabs(MotorValue)>1) { // if error more than 1 radian, full duty cycle |
Sven_van_Wincoop | 0:84e285dc97bd | 139 | PwmPin = 1; |
Sven_van_Wincoop | 0:84e285dc97bd | 140 | } else { |
Sven_van_Wincoop | 0:84e285dc97bd | 141 | PwmPin = fabs(MotorValue); |
Sven_van_Wincoop | 0:84e285dc97bd | 142 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 143 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 144 | |
Sven_van_Wincoop | 0:84e285dc97bd | 145 | // ---------------------------------------------------------------------------- |
Sven_van_Wincoop | 0:84e285dc97bd | 146 | //Ticker function |
Sven_van_Wincoop | 0:84e285dc97bd | 147 | void MeasureAndControl(void) |
Sven_van_Wincoop | 0:84e285dc97bd | 148 | { |
Sven_van_Wincoop | 0:84e285dc97bd | 149 | double PositionRef = GetReferencePosition(); |
Sven_van_Wincoop | 0:84e285dc97bd | 150 | double PositionMotor = GetActualPosition(); |
Sven_van_Wincoop | 0:84e285dc97bd | 151 | double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error |
Sven_van_Wincoop | 0:84e285dc97bd | 152 | SetMotor(MotorValue); |
Sven_van_Wincoop | 0:84e285dc97bd | 153 | |
Sven_van_Wincoop | 0:84e285dc97bd | 154 | //for printing on screen |
Sven_van_Wincoop | 0:84e285dc97bd | 155 | PosRefPrint = PositionRef; |
Sven_van_Wincoop | 0:84e285dc97bd | 156 | PosMotorPrint = PositionMotor; |
Sven_van_Wincoop | 0:84e285dc97bd | 157 | ErrorPrint = PositionRef - PositionMotor; |
Sven_van_Wincoop | 0:84e285dc97bd | 158 | |
Sven_van_Wincoop | 0:84e285dc97bd | 159 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 160 | |
Sven_van_Wincoop | 0:84e285dc97bd | 161 | |
Sven_van_Wincoop | 0:84e285dc97bd | 162 | |
Sven_van_Wincoop | 0:84e285dc97bd | 163 | void PrintToScreen() |
Sven_van_Wincoop | 0:84e285dc97bd | 164 | { |
Sven_van_Wincoop | 0:84e285dc97bd | 165 | PrintFlag = true; |
Sven_van_Wincoop | 0:84e285dc97bd | 166 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 167 | |
Sven_van_Wincoop | 0:84e285dc97bd | 168 | |
Sven_van_Wincoop | 0:84e285dc97bd | 169 | //----------------------------------------------------------------------------- |
Sven_van_Wincoop | 0:84e285dc97bd | 170 | int main() |
Sven_van_Wincoop | 0:84e285dc97bd | 171 | { |
Sven_van_Wincoop | 0:84e285dc97bd | 172 | pc.baud(115200); |
Sven_van_Wincoop | 0:84e285dc97bd | 173 | pc.printf("Hello World\n\r"); |
Sven_van_Wincoop | 0:84e285dc97bd | 174 | PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!) |
Sven_van_Wincoop | 0:84e285dc97bd | 175 | TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz |
Sven_van_Wincoop | 0:84e285dc97bd | 176 | TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen |
Sven_van_Wincoop | 0:84e285dc97bd | 177 | |
Sven_van_Wincoop | 0:84e285dc97bd | 178 | while (true) { |
Sven_van_Wincoop | 0:84e285dc97bd | 179 | if(PrintFlag) { // if-statement for printing every second four times to screen |
Sven_van_Wincoop | 0:84e285dc97bd | 180 | double KpPrint = 3.52; |
Sven_van_Wincoop | 0:84e285dc97bd | 181 | double KiPrint = 0.88; |
Sven_van_Wincoop | 0:84e285dc97bd | 182 | double KdPrint = 6.9; |
Sven_van_Wincoop | 0:84e285dc97bd | 183 | pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, Kp = %f, Ki = %f and Kd = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint,KiPrint,KdPrint); |
Sven_van_Wincoop | 0:84e285dc97bd | 184 | PrintFlag = false; |
Sven_van_Wincoop | 0:84e285dc97bd | 185 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 186 | } |
Sven_van_Wincoop | 0:84e285dc97bd | 187 | } |