control flow doet nog niks
Dependencies: Encoder HIDScope MODSERIAL mbed
main.cpp@1:2f23368e8638, 2015-10-07 (annotated)
- Committer:
- Zeekat
- Date:
- Wed Oct 07 18:19:07 2015 +0000
- Revision:
- 1:2f23368e8638
- Parent:
- 0:f6306e179750
- Child:
- 2:2563df4ee829
untested PID controller with P-controller settings. output is not yet limited
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Zeekat | 0:f6306e179750 | 1 | #include "mbed.h" |
Zeekat | 0:f6306e179750 | 2 | #include "MODSERIAL.h" |
Zeekat | 0:f6306e179750 | 3 | #include "encoder.h" |
Zeekat | 0:f6306e179750 | 4 | // #include "HIDScope.h" |
Zeekat | 0:f6306e179750 | 5 | |
Zeekat | 0:f6306e179750 | 6 | Serial pc(USBTX,USBRX); |
Zeekat | 0:f6306e179750 | 7 | // HIDScope scope(4); // definieerd het aantal kanalen van de scope |
Zeekat | 0:f6306e179750 | 8 | |
Zeekat | 0:f6306e179750 | 9 | // Define Tickers and stuff |
Zeekat | 0:f6306e179750 | 10 | Ticker controller1, controller2; // definieer de ticker die controler1 doet |
Zeekat | 0:f6306e179750 | 11 | double controlfreq = 10 ; // controlloops frequentie (Hz) |
Zeekat | 0:f6306e179750 | 12 | double controlstep = 1/controlfreq; // timestep derived from controlfreq |
Zeekat | 0:f6306e179750 | 13 | |
Zeekat | 0:f6306e179750 | 14 | //MOTOR INPUTPINS |
Zeekat | 0:f6306e179750 | 15 | // motor 1 |
Zeekat | 0:f6306e179750 | 16 | PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets) |
Zeekat | 0:f6306e179750 | 17 | DigitalOut motor1_rich(D7); // digitaal signaal voor richting |
Zeekat | 0:f6306e179750 | 18 | // motor 2 |
Zeekat | 0:f6306e179750 | 19 | PwmOut motor2_aan(D5); |
Zeekat | 0:f6306e179750 | 20 | DigitalOut motor2_rich(D4); |
Zeekat | 0:f6306e179750 | 21 | |
Zeekat | 0:f6306e179750 | 22 | // ENCODER |
Zeekat | 0:f6306e179750 | 23 | Encoder motor1_enc(D12,D11); // encoder outputpins |
Zeekat | 0:f6306e179750 | 24 | Encoder motor2_enc(D10,D9); |
Zeekat | 0:f6306e179750 | 25 | |
Zeekat | 0:f6306e179750 | 26 | //POTMETERS |
Zeekat | 0:f6306e179750 | 27 | AnalogIn potright(A0); // define the potmeter outputpins |
Zeekat | 0:f6306e179750 | 28 | AnalogIn potleft(A1); |
Zeekat | 0:f6306e179750 | 29 | |
Zeekat | 0:f6306e179750 | 30 | // RESETBUTTON |
Zeekat | 0:f6306e179750 | 31 | DigitalIn button(PTA4); // defines the button used for a encoder reset |
Zeekat | 0:f6306e179750 | 32 | int button_pressed = 0; |
Zeekat | 0:f6306e179750 | 33 | |
Zeekat | 0:f6306e179750 | 34 | int reference1 = 0; // set the reference position of the encoders |
Zeekat | 0:f6306e179750 | 35 | int reference2 = 0; |
Zeekat | 0:f6306e179750 | 36 | |
Zeekat | 0:f6306e179750 | 37 | // CONTROLLER SETTINGS |
Zeekat | 1:2f23368e8638 | 38 | // motor 1 |
Zeekat | 1:2f23368e8638 | 39 | const double m1_Kp = 1; // Proportional constant |
Zeekat | 1:2f23368e8638 | 40 | const double m1_Ki = 0; // integration constant |
Zeekat | 1:2f23368e8638 | 41 | const double m1_Kd = 0; // differentiation constant |
Zeekat | 1:2f23368e8638 | 42 | // motor 2 |
Zeekat | 1:2f23368e8638 | 43 | const double m2_Kp = 1; |
Zeekat | 1:2f23368e8638 | 44 | const double m2_Ki = 0; |
Zeekat | 1:2f23368e8638 | 45 | const double m2_Kd = 0; |
Zeekat | 1:2f23368e8638 | 46 | // storage variables |
Zeekat | 1:2f23368e8638 | 47 | // motor 1 |
Zeekat | 1:2f23368e8638 | 48 | double m1_err_int = 0; |
Zeekat | 1:2f23368e8638 | 49 | double m1_prev_err = 0; |
Zeekat | 1:2f23368e8638 | 50 | // motor 2 |
Zeekat | 1:2f23368e8638 | 51 | double m2_err_int = 0; |
Zeekat | 1:2f23368e8638 | 52 | double m2_prev_err = 0; |
Zeekat | 0:f6306e179750 | 53 | |
Zeekat | 0:f6306e179750 | 54 | |
Zeekat | 0:f6306e179750 | 55 | // SETPOINT SETTINGS |
Zeekat | 0:f6306e179750 | 56 | // define storage variables for setpoint values |
Zeekat | 1:2f23368e8638 | 57 | double c_setpoint1 = 0; |
Zeekat | 1:2f23368e8638 | 58 | double c_setpoint2 = 0; |
Zeekat | 0:f6306e179750 | 59 | // define the maximum rate of change for the setpoint (velocity) |
Zeekat | 1:2f23368e8638 | 60 | double Vmax = 5; // rad/sec |
Zeekat | 1:2f23368e8638 | 61 | |
Zeekat | 1:2f23368e8638 | 62 | |
Zeekat | 1:2f23368e8638 | 63 | //// FILTER VARIABLES |
Zeekat | 1:2f23368e8638 | 64 | // storage variables |
Zeekat | 1:2f23368e8638 | 65 | // differential action filter, same is used for both controllers |
Zeekat | 1:2f23368e8638 | 66 | double m_f_v1 = 0; |
Zeekat | 1:2f23368e8638 | 67 | double m_f_v2 = 0; |
Zeekat | 1:2f23368e8638 | 68 | |
Zeekat | 1:2f23368e8638 | 69 | // Filter coefficients |
Zeekat | 1:2f23368e8638 | 70 | // differential action filter |
Zeekat | 1:2f23368e8638 | 71 | const double m_f_a1 = 1.0, m_f_a2 = 2.0, m_f_b0 = 1.0, m_f_b1 = 3.0, m_f_b2 = 4.0; // coefficients from sheets are used as first test. |
Zeekat | 0:f6306e179750 | 72 | |
Zeekat | 0:f6306e179750 | 73 | |
Zeekat | 0:f6306e179750 | 74 | |
Zeekat | 0:f6306e179750 | 75 | //////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 76 | /////////////////// START OF SIDE FUNCTIONS //////////////////// |
Zeekat | 0:f6306e179750 | 77 | ////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 78 | // these functions are tailored to perform 1 specific function |
Zeekat | 0:f6306e179750 | 79 | |
Zeekat | 0:f6306e179750 | 80 | |
Zeekat | 0:f6306e179750 | 81 | // counts 2 radians |
Zeekat | 0:f6306e179750 | 82 | // this function takes counts from the encoder and converts it to the amount of radians from the zero position. |
Zeekat | 0:f6306e179750 | 83 | // It has been set up for standard 2X DECODING!!!!!! |
Zeekat | 0:f6306e179750 | 84 | double get_radians(double counts) |
Zeekat | 0:f6306e179750 | 85 | { |
Zeekat | 0:f6306e179750 | 86 | double pi = 3.14159265359; |
Zeekat | 0:f6306e179750 | 87 | double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, extra warning) |
Zeekat | 0:f6306e179750 | 88 | return radians; |
Zeekat | 0:f6306e179750 | 89 | } |
Zeekat | 0:f6306e179750 | 90 | |
Zeekat | 0:f6306e179750 | 91 | |
Zeekat | 0:f6306e179750 | 92 | // This functions takes a 0->1 input converts it to -1->1 and uses passing by reference (&c_setpoint) |
Zeekat | 0:f6306e179750 | 93 | // to create a reference that moves with a variable speed. It is now set up for potmeter values. |
Zeekat | 0:f6306e179750 | 94 | double setpoint_f(double input, double &c_setpoint) |
Zeekat | 0:f6306e179750 | 95 | { |
Zeekat | 0:f6306e179750 | 96 | double offset = 0.5, gain = 2; |
Zeekat | 0:f6306e179750 | 97 | double potset = (input-offset)*gain; |
Zeekat | 0:f6306e179750 | 98 | double setpoint = c_setpoint + potset * controlstep * Vmax ; |
Zeekat | 0:f6306e179750 | 99 | c_setpoint = setpoint; |
Zeekat | 0:f6306e179750 | 100 | return setpoint; |
Zeekat | 0:f6306e179750 | 101 | } |
Zeekat | 0:f6306e179750 | 102 | |
Zeekat | 0:f6306e179750 | 103 | |
Zeekat | 0:f6306e179750 | 104 | // This function takes the inputvalue and ensures it is between -1 and 1 |
Zeekat | 0:f6306e179750 | 105 | // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction). |
Zeekat | 0:f6306e179750 | 106 | // I am aware it has unnecesary steps but it works. |
Zeekat | 0:f6306e179750 | 107 | double outputlimiter (double output) |
Zeekat | 0:f6306e179750 | 108 | { |
Zeekat | 0:f6306e179750 | 109 | if(output>1) |
Zeekat | 0:f6306e179750 | 110 | { |
Zeekat | 0:f6306e179750 | 111 | output = 1; |
Zeekat | 0:f6306e179750 | 112 | } |
Zeekat | 0:f6306e179750 | 113 | else if(output < 1 && output > 0) |
Zeekat | 0:f6306e179750 | 114 | { |
Zeekat | 0:f6306e179750 | 115 | output = output; |
Zeekat | 0:f6306e179750 | 116 | } |
Zeekat | 0:f6306e179750 | 117 | else if(output > -1 && output < 0) |
Zeekat | 0:f6306e179750 | 118 | { |
Zeekat | 0:f6306e179750 | 119 | output = output; |
Zeekat | 0:f6306e179750 | 120 | } |
Zeekat | 0:f6306e179750 | 121 | else if(output < -1) |
Zeekat | 0:f6306e179750 | 122 | { |
Zeekat | 0:f6306e179750 | 123 | (output = -1); |
Zeekat | 0:f6306e179750 | 124 | } |
Zeekat | 0:f6306e179750 | 125 | return output; |
Zeekat | 0:f6306e179750 | 126 | } |
Zeekat | 0:f6306e179750 | 127 | |
Zeekat | 0:f6306e179750 | 128 | |
Zeekat | 1:2f23368e8638 | 129 | // BIQUADFILTER CODE GIVEN IN SHEETS |
Zeekat | 1:2f23368e8638 | 130 | // used for d-control |
Zeekat | 1:2f23368e8638 | 131 | double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) |
Zeekat | 1:2f23368e8638 | 132 | { |
Zeekat | 1:2f23368e8638 | 133 | double v = u- a1*v1-a2*v2; |
Zeekat | 1:2f23368e8638 | 134 | double y = b0*v+b1*v1+b2*v2; |
Zeekat | 1:2f23368e8638 | 135 | v2 = v1; |
Zeekat | 1:2f23368e8638 | 136 | v1 = v; |
Zeekat | 1:2f23368e8638 | 137 | return y; |
Zeekat | 1:2f23368e8638 | 138 | } |
Zeekat | 1:2f23368e8638 | 139 | |
Zeekat | 1:2f23368e8638 | 140 | |
Zeekat | 1:2f23368e8638 | 141 | // PID Controller given in sheets |
Zeekat | 1:2f23368e8638 | 142 | // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!) |
Zeekat | 1:2f23368e8638 | 143 | double PID( double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev) |
Zeekat | 1:2f23368e8638 | 144 | { |
Zeekat | 1:2f23368e8638 | 145 | // Proportional |
Zeekat | 1:2f23368e8638 | 146 | double P = Kp * e; |
Zeekat | 1:2f23368e8638 | 147 | |
Zeekat | 1:2f23368e8638 | 148 | // Derivative |
Zeekat | 1:2f23368e8638 | 149 | double e_derr = (e - e_prev)/Ts; |
Zeekat | 1:2f23368e8638 | 150 | e_derr = biquadfilter(e_derr, m_f_v1, m_f_v2, m_f_a1, m_f_a2, m_f_b0, m_f_b1, m_f_b2); |
Zeekat | 1:2f23368e8638 | 151 | e_prev = e; |
Zeekat | 1:2f23368e8638 | 152 | double D = Kd* e_derr; |
Zeekat | 1:2f23368e8638 | 153 | |
Zeekat | 1:2f23368e8638 | 154 | // Integral |
Zeekat | 1:2f23368e8638 | 155 | e_int = e_int + Ts * e; |
Zeekat | 1:2f23368e8638 | 156 | double I = e_int * Ki; |
Zeekat | 1:2f23368e8638 | 157 | |
Zeekat | 1:2f23368e8638 | 158 | // PID |
Zeekat | 1:2f23368e8638 | 159 | double output = P + I + D; |
Zeekat | 1:2f23368e8638 | 160 | return output; |
Zeekat | 1:2f23368e8638 | 161 | } |
Zeekat | 1:2f23368e8638 | 162 | |
Zeekat | 1:2f23368e8638 | 163 | |
Zeekat | 1:2f23368e8638 | 164 | |
Zeekat | 0:f6306e179750 | 165 | // this function is a simple K control called by the motor function |
Zeekat | 0:f6306e179750 | 166 | double K_control(double error,double K) |
Zeekat | 0:f6306e179750 | 167 | { |
Zeekat | 0:f6306e179750 | 168 | double output = K*error; // controller output K*e |
Zeekat | 0:f6306e179750 | 169 | output = outputlimiter(output); // limit the output to -1->1 |
Zeekat | 0:f6306e179750 | 170 | return output; |
Zeekat | 0:f6306e179750 | 171 | } |
Zeekat | 0:f6306e179750 | 172 | |
Zeekat | 0:f6306e179750 | 173 | ///////////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 174 | ////////////////// PRIMARY CONTROL FUNCTIONS /////////////////////// |
Zeekat | 0:f6306e179750 | 175 | /////////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 176 | // these functions are used to control all aspects of a single electric motor and are called by the main function |
Zeekat | 0:f6306e179750 | 177 | |
Zeekat | 0:f6306e179750 | 178 | // MOTOR 1 |
Zeekat | 0:f6306e179750 | 179 | void motor1_control() |
Zeekat | 0:f6306e179750 | 180 | { |
Zeekat | 0:f6306e179750 | 181 | double setpoint1 = setpoint_f(potright.read(), c_setpoint1); // determine the setpoint that has been set by the inputsignal |
Zeekat | 0:f6306e179750 | 182 | double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor |
Zeekat | 0:f6306e179750 | 183 | double error1 = (setpoint1 - rads1); // determine the error (reference - position) |
Zeekat | 0:f6306e179750 | 184 | // scope.set(0,setpoint1); // disabled because HIDScope is acting strangely |
Zeekat | 0:f6306e179750 | 185 | // scope.set(1,rads1); |
Zeekat | 0:f6306e179750 | 186 | // scope.send(); |
Zeekat | 1:2f23368e8638 | 187 | |
Zeekat | 1:2f23368e8638 | 188 | // double output1 = K_control(error1, m1_Kp); // bereken de controller output voor motor 1(zie hierboven) |
Zeekat | 1:2f23368e8638 | 189 | double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err); |
Zeekat | 0:f6306e179750 | 190 | if(output1 > 0) { // uses the calculated output to determine the direction of the motor |
Zeekat | 0:f6306e179750 | 191 | motor1_rich.write(0); |
Zeekat | 0:f6306e179750 | 192 | motor1_aan.write(output1); |
Zeekat | 0:f6306e179750 | 193 | } else if(output1 < 0) { |
Zeekat | 0:f6306e179750 | 194 | motor1_rich.write(1); |
Zeekat | 0:f6306e179750 | 195 | motor1_aan.write(abs(output1)); |
Zeekat | 0:f6306e179750 | 196 | } |
Zeekat | 0:f6306e179750 | 197 | } |
Zeekat | 0:f6306e179750 | 198 | |
Zeekat | 0:f6306e179750 | 199 | // MOTOR 2 |
Zeekat | 0:f6306e179750 | 200 | void motor2_control() |
Zeekat | 0:f6306e179750 | 201 | { |
Zeekat | 0:f6306e179750 | 202 | double setpoint2 = setpoint_f(potleft.read(), c_setpoint2); // determine the setpoint that has been set by the inputsignal |
Zeekat | 0:f6306e179750 | 203 | double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor |
Zeekat | 0:f6306e179750 | 204 | double error2 = (setpoint2 - rads2); // determine the error (reference - position) |
Zeekat | 0:f6306e179750 | 205 | // scope.set(3,setpoint2); // disabled because HIDScope is acting strangely |
Zeekat | 0:f6306e179750 | 206 | // scope.set(4,rads2); |
Zeekat | 0:f6306e179750 | 207 | // scope.send(); |
Zeekat | 1:2f23368e8638 | 208 | |
Zeekat | 1:2f23368e8638 | 209 | // double output2 = K_control(error2, m2_Kp); // bereken de controller output voor motor 1(zie hierboven) |
Zeekat | 1:2f23368e8638 | 210 | double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err); |
Zeekat | 0:f6306e179750 | 211 | if(output2 > 0) { // uses the calculated output to determine the direction of the motor |
Zeekat | 0:f6306e179750 | 212 | motor2_rich.write(0); |
Zeekat | 0:f6306e179750 | 213 | motor2_aan.write(output2); |
Zeekat | 0:f6306e179750 | 214 | } else if(output2 < 0) { |
Zeekat | 0:f6306e179750 | 215 | motor2_rich.write(1); |
Zeekat | 0:f6306e179750 | 216 | motor2_aan.write(abs(output2)); |
Zeekat | 0:f6306e179750 | 217 | } |
Zeekat | 0:f6306e179750 | 218 | } |
Zeekat | 0:f6306e179750 | 219 | |
Zeekat | 0:f6306e179750 | 220 | |
Zeekat | 0:f6306e179750 | 221 | int main() |
Zeekat | 0:f6306e179750 | 222 | { |
Zeekat | 1:2f23368e8638 | 223 | // Ticker calling the primary functions. If neccessary use the ticker to change bools and execute in the main loop as shown in the sheets. |
Zeekat | 0:f6306e179750 | 224 | controller1.attach(&motor1_control, controlstep); |
Zeekat | 0:f6306e179750 | 225 | controller2.attach(&motor2_control, controlstep); |
Zeekat | 0:f6306e179750 | 226 | while(true) |
Zeekat | 0:f6306e179750 | 227 | { |
Zeekat | 0:f6306e179750 | 228 | if(button.read() == button_pressed) // reset the encoder to reference position |
Zeekat | 0:f6306e179750 | 229 | { |
Zeekat | 0:f6306e179750 | 230 | motor1_enc.setPosition(reference1); |
Zeekat | 0:f6306e179750 | 231 | motor2_enc.setPosition(reference2); |
Zeekat | 0:f6306e179750 | 232 | } |
Zeekat | 0:f6306e179750 | 233 | } |
Zeekat | 0:f6306e179750 | 234 | } |