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@11:4b0dbbdc56fb, 2018-10-22 (annotated)

Committer:
Florianhs
Date:
Mon Oct 22 10:01:47 2018 +0000
Revision:
11:4b0dbbdc56fb
Parent:
10:61a7cb3b3aa3 
P(I) controller voor 2 motoren, aangestuurd met potmeters voor positite

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 */
Florianhs 11:4b0dbbdc56fb 7 //motor 1
Florianhs 11:4b0dbbdc56fb 8 double Kp = 5;
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 11:4b0dbbdc56fb 15 double m1_scalar;
Florianhs 10:61a7cb3b3aa3 16 double motor1_ang=0;
Florianhs 11:4b0dbbdc56fb 17 double m1=0;
Florianhs 10:61a7cb3b3aa3 18 double input_motor;
Florianhs 11:4b0dbbdc56fb 19 //motor 2
Florianhs 11:4b0dbbdc56fb 20 double Kp2 = 10;
Florianhs 11:4b0dbbdc56fb 21 double Ki2 = 1;
Florianhs 11:4b0dbbdc56fb 22 double u_k2 = 1;
Florianhs 11:4b0dbbdc56fb 23 double u_i2 = 1;
Florianhs 11:4b0dbbdc56fb 24 double Kswitch2;
Florianhs 11:4b0dbbdc56fb 25 double kp_scalar2 = 1;
Florianhs 11:4b0dbbdc56fb 26 double ki_scalar2 = 1;
Florianhs 11:4b0dbbdc56fb 27 double m2_scalar;
Florianhs 11:4b0dbbdc56fb 28 double motor2_ang=0;
Florianhs 11:4b0dbbdc56fb 29 double m2=0;
Florianhs 11:4b0dbbdc56fb 30 double input_motor2;
Florianhs 11:4b0dbbdc56fb 31
Florianhs 11:4b0dbbdc56fb 32 //rest
Florianhs 10:61a7cb3b3aa3 33 bool input_switch = true;
Florianhs 10:61a7cb3b3aa3 34 const double Ts = 0.01f;
Florianhs 11:4b0dbbdc56fb 35 volatile char input;
Florianhs 10:61a7cb3b3aa3 36
Florianhs 10:61a7cb3b3aa3 37 Ticker Control;
SimonRez 4:651d06e860e7 38
rachieldb 9:466dff9ae128 39 PwmOut motor1_pwm(D5);
rachieldb 9:466dff9ae128 40 PwmOut motor2_pwm(D6);
rachieldb 9:466dff9ae128 41 DigitalOut motor1_dir(D4);
rachieldb 9:466dff9ae128 42 DigitalOut motor2_dir(D7);
SimonRez 6:1ee8795b7578 43 AnalogIn pot1(A0);
rachieldb 9:466dff9ae128 44 AnalogIn pot2(A1);
SimonRez 6:1ee8795b7578 45
rachieldb 9:466dff9ae128 46 QEI motor1_enc(D12,D13,NC,8400,QEI::X4_ENCODING);
rachieldb 9:466dff9ae128 47 QEI motor2_enc(D10,D11,NC,8400,QEI::X4_ENCODING);
SimonRez 5:f07bafaf11d7 48
SimonRez 4:651d06e860e7 49 InterruptIn sw2(SW2);
SimonRez 4:651d06e860e7 50 InterruptIn sw3(SW3);
SimonRez 2:52b3c0b95388 51
vsluiter 0:c8f15874531b 52 MODSERIAL pc(USBTX, USBRX);
vsluiter 0:c8f15874531b 53
Florianhs 10:61a7cb3b3aa3 54 //hoek uitrekenen van de motor
rachieldb 9:466dff9ae128 55 double countstoangle(int counts){
rachieldb 9:466dff9ae128 56 double angle;
Florianhs 10:61a7cb3b3aa3 57 int counts_abs =(counts);
Florianhs 10:61a7cb3b3aa3 58 angle = (2 * M_PI * counts_abs)/8400;
Florianhs 10:61a7cb3b3aa3 59 return angle;
Florianhs 10:61a7cb3b3aa3 60
Florianhs 10:61a7cb3b3aa3 61 }
Florianhs 10:61a7cb3b3aa3 62 void buttonpress()
Florianhs 10:61a7cb3b3aa3 63 {
Florianhs 10:61a7cb3b3aa3 64 input_switch=!input_switch; //verander de potmeter input tussen kp en ki
Florianhs 10:61a7cb3b3aa3 65 }
Florianhs 10:61a7cb3b3aa3 66
Florianhs 11:4b0dbbdc56fb 67 double PID_controller1(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 68 {
Florianhs 10:61a7cb3b3aa3 69 //Proportional part
Florianhs 10:61a7cb3b3aa3 70 u_k = Kp*error;
Florianhs 10:61a7cb3b3aa3 71
Florianhs 10:61a7cb3b3aa3 72 //Integral part
Florianhs 11:4b0dbbdc56fb 73 //static double error_integral = 0;
Florianhs 11:4b0dbbdc56fb 74 //error_integral = error_integral + error * Ts;
Florianhs 11:4b0dbbdc56fb 75 //u_i = Ki * error_integral;
Florianhs 11:4b0dbbdc56fb 76 return u_k; //+ u_i;
Florianhs 10:61a7cb3b3aa3 77 }
Florianhs 10:61a7cb3b3aa3 78
Florianhs 11:4b0dbbdc56fb 79 double PID_controller2(double error2) //bereken de input die naar de motor wordt gestuurd aan de hand van kp en ki en de error op elk moment
Florianhs 11:4b0dbbdc56fb 80 {
Florianhs 11:4b0dbbdc56fb 81 //Proportional part
Florianhs 11:4b0dbbdc56fb 82 u_k2 = Kp2*error2;
Florianhs 11:4b0dbbdc56fb 83
Florianhs 11:4b0dbbdc56fb 84 //Integral part
Florianhs 11:4b0dbbdc56fb 85 //static double error_integral2 = 0;
Florianhs 11:4b0dbbdc56fb 86 //error_integral2 = error_integral2 + error2 * Ts;
Florianhs 11:4b0dbbdc56fb 87 //u_i2 = Ki2 * error_integral2;
Florianhs 11:4b0dbbdc56fb 88 return u_k2; //+ u_i2;
Florianhs 11:4b0dbbdc56fb 89 }
Florianhs 10:61a7cb3b3aa3 90
Florianhs 11:4b0dbbdc56fb 91 void aansturing()
Florianhs 11:4b0dbbdc56fb 92 { //posities voor de motoren bepalen
Florianhs 11:4b0dbbdc56fb 93 // if(input=='a')
Florianhs 11:4b0dbbdc56fb 94 // {if(m1>0.1f)
Florianhs 11:4b0dbbdc56fb 95 // {m1 = m1-0.01f;}}
Florianhs 11:4b0dbbdc56fb 96 // else if(input=='s')
Florianhs 11:4b0dbbdc56fb 97 // {if(m1<2.1f)
Florianhs 11:4b0dbbdc56fb 98 // {m1 = m1+0.01f;}}
Florianhs 11:4b0dbbdc56fb 99 // else if(input=='q')
Florianhs 11:4b0dbbdc56fb 100 // {if(m2>0.01f)
Florianhs 11:4b0dbbdc56fb 101 // {m2 = m2-0.01f;}}
Florianhs 11:4b0dbbdc56fb 102 // else if(input=='w')
Florianhs 11:4b0dbbdc56fb 103 // {if(m2<2.1f)
Florianhs 11:4b0dbbdc56fb 104 // {m2 = m2+0.01f;}}
Florianhs 11:4b0dbbdc56fb 105 m1_scalar=pot1.read();
Florianhs 11:4b0dbbdc56fb 106 m2_scalar=pot2.read();
Florianhs 11:4b0dbbdc56fb 107 m1=m1_scalar*1.5f;
Florianhs 11:4b0dbbdc56fb 108 m2=m2_scalar*1.5f;
Florianhs 11:4b0dbbdc56fb 109 //Kswitch = pot1.read(); //kp en ki bepalen voor motor 1
Florianhs 11:4b0dbbdc56fb 110 //Kswitch2 = pot2.read(); //kp en ki bepalen voor motor 2
Florianhs 11:4b0dbbdc56fb 111
Florianhs 11:4b0dbbdc56fb 112 //kp_scalar=Kswitch;
Florianhs 11:4b0dbbdc56fb 113 //kp_scalar2=Kswitch2;
Florianhs 11:4b0dbbdc56fb 114 // //kijken of er gekozen is voor het uitlezen van de kp of ki van de potmeter
Florianhs 11:4b0dbbdc56fb 115 // if(input_switch==true)
Florianhs 11:4b0dbbdc56fb 116 // {kp_scalar = Kswitch;}
Florianhs 11:4b0dbbdc56fb 117 // else
Florianhs 11:4b0dbbdc56fb 118 // {ki_scalar = Kswitch;}
Florianhs 11:4b0dbbdc56fb 119 //
Florianhs 11:4b0dbbdc56fb 120 // if(input_switch==true)
Florianhs 11:4b0dbbdc56fb 121 // {kp_scalar2 = Kswitch2;}
Florianhs 11:4b0dbbdc56fb 122 // else
Florianhs 11:4b0dbbdc56fb 123 // {ki_scalar2 = Kswitch2;}
Florianhs 11:4b0dbbdc56fb 124
Florianhs 11:4b0dbbdc56fb 125 //waarde voor de inputcontrol value instellen aan de hand van potmeter
Florianhs 11:4b0dbbdc56fb 126 // Kp = kp_scalar*20.0f;
Florianhs 11:4b0dbbdc56fb 127 // Ki = ki_scalar*2.0f;
Florianhs 11:4b0dbbdc56fb 128 // Kp2 = kp_scalar2*20.0f;
Florianhs 11:4b0dbbdc56fb 129 // Ki2 = ki_scalar2*2.0f;
Florianhs 10:61a7cb3b3aa3 130
Florianhs 10:61a7cb3b3aa3 131
Florianhs 11:4b0dbbdc56fb 132
Florianhs 10:61a7cb3b3aa3 133 motor1_ang = countstoangle(motor1_enc.getPulses()); // de hoek van de motor uit bovenstaande formule halen, om de error te kunnen berekenen
Florianhs 11:4b0dbbdc56fb 134 double error = motor1_ang -m1;
Florianhs 10:61a7cb3b3aa3 135
Florianhs 11:4b0dbbdc56fb 136 motor2_ang = countstoangle(motor2_enc.getPulses()); // de hoek van de motor uit bovenstaande formule halen, om de error te kunnen berekenen
Florianhs 11:4b0dbbdc56fb 137 double error2 = motor2_ang -m2;
Florianhs 10:61a7cb3b3aa3 138
Florianhs 11:4b0dbbdc56fb 139 //input voor de motor met de PID controllers bepalen
Florianhs 11:4b0dbbdc56fb 140 double input_motor = PID_controller1(error);
Florianhs 11:4b0dbbdc56fb 141 double input_motor2 = PID_controller2(error2);
Florianhs 10:61a7cb3b3aa3 142
Florianhs 11:4b0dbbdc56fb 143 //signaal naar motor 1 sturen
Florianhs 10:61a7cb3b3aa3 144 if(input_motor>1.0) //pwm signaal kan niet groter zijn dan 1, dus moet gelimiteerd worden
Florianhs 10:61a7cb3b3aa3 145 {input_motor = 1.0;}
Florianhs 10:61a7cb3b3aa3 146 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 147 {input_motor = -1.0;}
Florianhs 10:61a7cb3b3aa3 148
Florianhs 10:61a7cb3b3aa3 149
Florianhs 10:61a7cb3b3aa3 150 if(input_motor<0.0) //bepaal de richting aan de hand van het teken van het pwm signaal.
Florianhs 10:61a7cb3b3aa3 151 {motor1_dir = 0;}
Florianhs 10:61a7cb3b3aa3 152 else
Florianhs 10:61a7cb3b3aa3 153 {motor1_dir = 1;}
Florianhs 10:61a7cb3b3aa3 154 motor1_pwm.write(fabs(input_motor)); // de uiteindelijke input aan de motor geven
Florianhs 11:4b0dbbdc56fb 155
Florianhs 11:4b0dbbdc56fb 156 //signaal naar motor 12 sturen
Florianhs 11:4b0dbbdc56fb 157 if(input_motor2>1.0) //pwm signaal kan niet groter zijn dan 1, dus moet gelimiteerd worden
Florianhs 11:4b0dbbdc56fb 158 {input_motor2 = 1.0;}
Florianhs 11:4b0dbbdc56fb 159 else if(input_motor2<-1.0) //absolute waarde wordt ingevuld, maar de negatieve kant is belangrijk voor de richting van de motor
Florianhs 11:4b0dbbdc56fb 160 {input_motor2 = -1.0;}
Florianhs 11:4b0dbbdc56fb 161
Florianhs 11:4b0dbbdc56fb 162
Florianhs 11:4b0dbbdc56fb 163 if(input_motor2<0.0) //bepaal de richting aan de hand van het teken van het pwm signaal.
Florianhs 11:4b0dbbdc56fb 164 {motor2_dir = 0;}
Florianhs 11:4b0dbbdc56fb 165 else
Florianhs 11:4b0dbbdc56fb 166 {motor2_dir = 1;}
Florianhs 11:4b0dbbdc56fb 167 motor2_pwm.write(fabs(input_motor2)); // de uiteindelijke input aan de motor geven
Florianhs 10:61a7cb3b3aa3 168 // 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 169 }
SimonRez 4:651d06e860e7 170
vsluiter 0:c8f15874531b 171 int main()
vsluiter 0:c8f15874531b 172 {
SimonRez 2:52b3c0b95388 173
Florianhs 10:61a7cb3b3aa3 174 sw2.fall(buttonpress);
Florianhs 10:61a7cb3b3aa3 175 pc.baud(115200);
SimonRez 4:651d06e860e7 176 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 177
rachieldb 9:466dff9ae128 178 motor1_pwm.period_us(60);
rachieldb 9:466dff9ae128 179 motor1_pwm = 0;
rachieldb 9:466dff9ae128 180 motor2_pwm.period_us(60);
rachieldb 9:466dff9ae128 181 motor2_pwm = 0;
Florianhs 10:61a7cb3b3aa3 182
Florianhs 10:61a7cb3b3aa3 183 Control.attach(aansturing, Ts); //de aansturing regelen met een sampletime
Florianhs 10:61a7cb3b3aa3 184
SimonRez 2:52b3c0b95388 185 while (true)
Florianhs 10:61a7cb3b3aa3 186 {
Florianhs 11:4b0dbbdc56fb 187 //input = pc.getc();
Florianhs 10:61a7cb3b3aa3 188 if(input_switch==true)
Florianhs 10:61a7cb3b3aa3 189 {pc.printf("Kp ingeschakeld\r\n");}
Florianhs 10:61a7cb3b3aa3 190 else
Florianhs 10:61a7cb3b3aa3 191 {pc.printf("Ki ingeschakeld\r\n");}
Florianhs 11:4b0dbbdc56fb 192
Florianhs 11:4b0dbbdc56fb 193 double errorprint = motor1_ang-m1;
Florianhs 11:4b0dbbdc56fb 194 double errorprint2 = motor2_ang-m2;
Florianhs 11:4b0dbbdc56fb 195 pc.printf("ki1: %f kp1: %f m1: %f u_k1: %f angle1: %f error1: %f\r\n",Ki, Kp, m1,u_k,motor1_ang,errorprint);
Florianhs 11:4b0dbbdc56fb 196 pc.printf("ki2: %f kp2: %f m2: %f u_k2: %f angle2: %f error2: %f\r\n",Ki2, Kp2, m2,u_k2,motor2_ang,errorprint2);
Florianhs 11:4b0dbbdc56fb 197 pc.printf("toets=%c\r\n",input);
Florianhs 10:61a7cb3b3aa3 198 wait(0.5);
vsluiter 0:c8f15874531b 199 }
vsluiter 0:c8f15874531b 200 }
SimonRez 2:52b3c0b95388 201
SimonRez 2:52b3c0b95388 202