ProjectGroep23 / 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_moter1and2_ticker by Florian Stevens

main.cpp@12:505e5ea9f639, 2018-10-25 (annotated)

Committer:
Florianhs
Date:
Thu Oct 25 13:01:31 2018 +0000
Revision:
12:505e5ea9f639
Parent:
11:4b0dbbdc56fb 
andere toetsenbordaansturing;

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