Florian Stevens / Mbed 2 deprecated PI_moter1and2_ticker

pot controller voor positie, kp handmatig instellen, werkt met motor 1 en 2

Dependencies:   MODSERIAL QEI mbed-dsp mbed

Fork of PI_controller_verbeteringen by ProjectGroep23

main.cpp@10:61a7cb3b3aa3, 2018-10-17 (annotated)

Committer:
Florianhs
Date:
Wed Oct 17 13:50:35 2018 +0000
Revision:
10:61a7cb3b3aa3
Parent:
9:466dff9ae128
Child:
11:4b0dbbdc56fb
Werkend programma om de PI controller motor aan te sturen en met een switch te kiezen tussen Kp en Ki;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c8f15874531b 1 #include "mbed.h"
vsluiter 0:c8f15874531b 2 #include "MODSERIAL.h"
SimonRez 6:1ee8795b7578 3 #include "QEI.h"
rachieldb 9:466dff9ae128 4 #include "math.h"
rachieldb 9:466dff9ae128 5 #define _USE_MATH_DEFINES
rachieldb 9:466dff9ae128 6 # define M_PI 3.14159265358979323846 /* pi */
SimonRez 2:52b3c0b95388 7
Florianhs 10:61a7cb3b3aa3 8 double Kp = 1;
Florianhs 10:61a7cb3b3aa3 9 double Ki = 1;
Florianhs 10:61a7cb3b3aa3 10 double u_k = 1;
Florianhs 10:61a7cb3b3aa3 11 double u_i = 1;
Florianhs 10:61a7cb3b3aa3 12 double Kswitch;
Florianhs 10:61a7cb3b3aa3 13 double kp_scalar = 1;
Florianhs 10:61a7cb3b3aa3 14 double ki_scalar = 1;
Florianhs 10:61a7cb3b3aa3 15 double y_ref=0;
Florianhs 10:61a7cb3b3aa3 16 double motor1_ang=0;
Florianhs 10:61a7cb3b3aa3 17 double y_ref_scalar = 0;
Florianhs 10:61a7cb3b3aa3 18 double input_motor;
Florianhs 10:61a7cb3b3aa3 19 bool input_switch = true;
Florianhs 10:61a7cb3b3aa3 20 const double Ts = 0.01f;
Florianhs 10:61a7cb3b3aa3 21
Florianhs 10:61a7cb3b3aa3 22 Ticker Control;
SimonRez 4:651d06e860e7 23
rachieldb 9:466dff9ae128 24 PwmOut motor1_pwm(D5);
rachieldb 9:466dff9ae128 25 PwmOut motor2_pwm(D6);
rachieldb 9:466dff9ae128 26 DigitalOut motor1_dir(D4);
rachieldb 9:466dff9ae128 27 DigitalOut motor2_dir(D7);
SimonRez 6:1ee8795b7578 28 AnalogIn pot1(A0);
rachieldb 9:466dff9ae128 29 AnalogIn pot2(A1);
SimonRez 6:1ee8795b7578 30
rachieldb 9:466dff9ae128 31 QEI motor1_enc(D12,D13,NC,8400,QEI::X4_ENCODING);
rachieldb 9:466dff9ae128 32 QEI motor2_enc(D10,D11,NC,8400,QEI::X4_ENCODING);
SimonRez 5:f07bafaf11d7 33
SimonRez 4:651d06e860e7 34 InterruptIn sw2(SW2);
SimonRez 4:651d06e860e7 35 InterruptIn sw3(SW3);
SimonRez 2:52b3c0b95388 36
vsluiter 0:c8f15874531b 37 MODSERIAL pc(USBTX, USBRX);
vsluiter 0:c8f15874531b 38
Florianhs 10:61a7cb3b3aa3 39 //hoek uitrekenen van de motor
rachieldb 9:466dff9ae128 40 double countstoangle(int counts){
rachieldb 9:466dff9ae128 41 double angle;
Florianhs 10:61a7cb3b3aa3 42 int counts_abs =(counts);
Florianhs 10:61a7cb3b3aa3 43 angle = (2 * M_PI * counts_abs)/8400;
Florianhs 10:61a7cb3b3aa3 44 return angle;
Florianhs 10:61a7cb3b3aa3 45
Florianhs 10:61a7cb3b3aa3 46 }
Florianhs 10:61a7cb3b3aa3 47 void buttonpress()
Florianhs 10:61a7cb3b3aa3 48 {
Florianhs 10:61a7cb3b3aa3 49 input_switch=!input_switch; //verander de potmeter input tussen kp en ki
Florianhs 10:61a7cb3b3aa3 50 }
Florianhs 10:61a7cb3b3aa3 51
Florianhs 10:61a7cb3b3aa3 52 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
Florianhs 10:61a7cb3b3aa3 53 {
Florianhs 10:61a7cb3b3aa3 54 //Proportional part
Florianhs 10:61a7cb3b3aa3 55 u_k = Kp*error;
Florianhs 10:61a7cb3b3aa3 56
Florianhs 10:61a7cb3b3aa3 57 //Integral part
Florianhs 10:61a7cb3b3aa3 58 static double error_integral = 0;
Florianhs 10:61a7cb3b3aa3 59 error_integral = error_integral + error * Ts;
Florianhs 10:61a7cb3b3aa3 60 u_i = Ki * error_integral;
Florianhs 10:61a7cb3b3aa3 61 return u_k + u_i;
Florianhs 10:61a7cb3b3aa3 62 }
Florianhs 10:61a7cb3b3aa3 63
Florianhs 10:61a7cb3b3aa3 64
Florianhs 10:61a7cb3b3aa3 65 void aansturing()
Florianhs 10:61a7cb3b3aa3 66 {
Florianhs 10:61a7cb3b3aa3 67 y_ref_scalar = pot2.read(); //altijd de potmeters uitlezen, buiten de ticker
Florianhs 10:61a7cb3b3aa3 68 Kswitch = pot1.read();
Florianhs 10:61a7cb3b3aa3 69
Florianhs 10:61a7cb3b3aa3 70 //kijken of er gekozen is voor het uitlezen van de kp of ki van de potmeter
Florianhs 10:61a7cb3b3aa3 71 if(input_switch==true)
Florianhs 10:61a7cb3b3aa3 72 {kp_scalar = Kswitch;}
Florianhs 10:61a7cb3b3aa3 73 else
Florianhs 10:61a7cb3b3aa3 74 {ki_scalar = Kswitch;}
Florianhs 10:61a7cb3b3aa3 75
Florianhs 10:61a7cb3b3aa3 76
Florianhs 10:61a7cb3b3aa3 77 //waarde voor de inputcontrol value instellen aan de hand van potmeter
Florianhs 10:61a7cb3b3aa3 78 Kp = kp_scalar*20.0f;
Florianhs 10:61a7cb3b3aa3 79 Ki = ki_scalar*2.0f;
Florianhs 10:61a7cb3b3aa3 80
Florianhs 10:61a7cb3b3aa3 81 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
Florianhs 10:61a7cb3b3aa3 82 motor1_ang = countstoangle(motor1_enc.getPulses()); // de hoek van de motor uit bovenstaande formule halen, om de error te kunnen berekenen
Florianhs 10:61a7cb3b3aa3 83 double error = motor1_ang -y_ref;
Florianhs 10:61a7cb3b3aa3 84
Florianhs 10:61a7cb3b3aa3 85
Florianhs 10:61a7cb3b3aa3 86 double input_motor = PID_controller(error);
Florianhs 10:61a7cb3b3aa3 87
Florianhs 10:61a7cb3b3aa3 88 if(input_motor>1.0) //pwm signaal kan niet groter zijn dan 1, dus moet gelimiteerd worden
Florianhs 10:61a7cb3b3aa3 89 {input_motor = 1.0;}
Florianhs 10:61a7cb3b3aa3 90 else if(input_motor<-1.0) //absolute waarde wordt ingevuld, maar de negatieve kant is belangrijk voor de richting van de motor
Florianhs 10:61a7cb3b3aa3 91 {input_motor = -1.0;}
Florianhs 10:61a7cb3b3aa3 92
Florianhs 10:61a7cb3b3aa3 93
Florianhs 10:61a7cb3b3aa3 94 if(input_motor<0.0) //bepaal de richting aan de hand van het teken van het pwm signaal.
Florianhs 10:61a7cb3b3aa3 95 {motor1_dir = 0;}
Florianhs 10:61a7cb3b3aa3 96 else
Florianhs 10:61a7cb3b3aa3 97 {motor1_dir = 1;}
Florianhs 10:61a7cb3b3aa3 98 motor1_pwm.write(fabs(input_motor)); // de uiteindelijke input aan de motor geven
Florianhs 10:61a7cb3b3aa3 99 // 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);
rachieldb 9:466dff9ae128 100 }
SimonRez 4:651d06e860e7 101
vsluiter 0:c8f15874531b 102 int main()
vsluiter 0:c8f15874531b 103 {
SimonRez 2:52b3c0b95388 104
Florianhs 10:61a7cb3b3aa3 105 sw2.fall(buttonpress);
Florianhs 10:61a7cb3b3aa3 106 pc.baud(115200);
SimonRez 4:651d06e860e7 107 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");
SimonRez 2:52b3c0b95388 108
rachieldb 9:466dff9ae128 109 motor1_pwm.period_us(60);
rachieldb 9:466dff9ae128 110 motor1_pwm = 0;
rachieldb 9:466dff9ae128 111 motor2_pwm.period_us(60);
rachieldb 9:466dff9ae128 112 motor2_pwm = 0;
Florianhs 10:61a7cb3b3aa3 113
Florianhs 10:61a7cb3b3aa3 114 Control.attach(aansturing, Ts); //de aansturing regelen met een sampletime
Florianhs 10:61a7cb3b3aa3 115
SimonRez 2:52b3c0b95388 116 while (true)
Florianhs 10:61a7cb3b3aa3 117 {
Florianhs 10:61a7cb3b3aa3 118 if(input_switch==true)
Florianhs 10:61a7cb3b3aa3 119 {pc.printf("Kp ingeschakeld\r\n");}
Florianhs 10:61a7cb3b3aa3 120 else
Florianhs 10:61a7cb3b3aa3 121 {pc.printf("Ki ingeschakeld\r\n");}
Florianhs 10:61a7cb3b3aa3 122 double errorprint = motor1_ang-y_ref;
Florianhs 10:61a7cb3b3aa3 123 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);
Florianhs 10:61a7cb3b3aa3 124
Florianhs 10:61a7cb3b3aa3 125 wait(0.5);
vsluiter 0:c8f15874531b 126 }
vsluiter 0:c8f15874531b 127 }
SimonRez 2:52b3c0b95388 128
SimonRez 2:52b3c0b95388 129