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
main.cpp
- Committer:
- Florianhs
- Date:
- 2018-10-25
- Revision:
- 12:505e5ea9f639
- Parent:
- 11:4b0dbbdc56fb
File content as of revision 12:505e5ea9f639:
#include "mbed.h" #include "MODSERIAL.h" #include "QEI.h" #include "math.h" #define _USE_MATH_DEFINES # define M_PI 3.14159265358979323846 /* pi */ //motor 1 double Kp = 10; double Ki = 1; double u_k = 1; double u_i = 1; double Kswitch; double kp_scalar = 1; double ki_scalar = 1; double m1_scalar; double motor1_ang=0; double m1=0; double input_motor; //motor 2 double Kp2 = 12; double Ki2 = 1; double u_k2 = 1; double u_i2 = 1; double Kswitch2; double kp_scalar2 = 1; double ki_scalar2 = 1; double m2_scalar; double motor2_ang=0; double m2=0; double input_motor2; //rest bool input_switch = true; const double Ts = 0.01f; volatile char input; Ticker Control; Ticker serialEvent; PwmOut motor1_pwm(D5); PwmOut motor2_pwm(D6); DigitalOut motor1_dir(D4); DigitalOut motor2_dir(D7); AnalogIn pot1(A0); AnalogIn pot2(A1); QEI motor1_enc(D12,D13,NC,8400,QEI::X4_ENCODING); QEI motor2_enc(D10,D11,NC,8400,QEI::X4_ENCODING); InterruptIn sw2(SW2); InterruptIn sw3(SW3); MODSERIAL pc(USBTX, USBRX); //hoek uitrekenen van de motor double countstoangle(int counts){ double angle; int counts_abs =(counts); angle = (2 * M_PI * counts_abs)/8400; return angle; } void buttonpress() { input_switch=!input_switch; //verander de potmeter input tussen kp en ki } 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 { //Proportional part u_k = Kp*error; //Integral part //static double error_integral = 0; //error_integral = error_integral + error * Ts; //u_i = Ki * error_integral; return u_k; //+ u_i; } 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 { //Proportional part u_k2 = Kp2*error2; //Integral part //static double error_integral2 = 0; //error_integral2 = error_integral2 + error2 * Ts; //u_i2 = Ki2 * error_integral2; return u_k2; //+ u_i2; } void aansturing() { //posities voor de motoren bepalen if(input=='a') {if(m1>0) {m1 = m1-0.01f;}} else if(input=='s') {if(m1<1.5f) {m1 = m1+0.01f;}} else if(input=='q') {if(m2>0) {m2 = m2-0.01f;}} else if(input=='w') {if(m2<4.0f) {m2 = m2+0.01f;}} input= NULL; //m1_scalar=pot1.read(); //m2_scalar=pot2.read(); //m1=m1_scalar*1.5f; //m2=m2_scalar*1.5f; Kswitch = pot1.read(); //kp en ki bepalen voor motor 1 Kswitch2 = pot2.read(); //kp en ki bepalen voor motor 2 kp_scalar=Kswitch; kp_scalar2=Kswitch2; // //kijken of er gekozen is voor het uitlezen van de kp of ki van de potmeter if(input_switch==true) {kp_scalar = Kswitch;} else {ki_scalar = Kswitch;} if(input_switch==true) {kp_scalar2 = Kswitch2;} else {ki_scalar2 = Kswitch2;} //waarde voor de inputcontrol value instellen aan de hand van potmeter Kp = kp_scalar*5.0f; Ki = ki_scalar*5.0f; Kp2 = kp_scalar2*5.0f; Ki2 = ki_scalar2*5.0f; motor1_ang = countstoangle(motor1_enc.getPulses()); // de hoek van de motor uit bovenstaande formule halen, om de error te kunnen berekenen double error = motor1_ang -m1; motor2_ang = countstoangle(motor2_enc.getPulses()); // de hoek van de motor uit bovenstaande formule halen, om de error te kunnen berekenen double error2 = motor2_ang -m2; //input voor de motor met de PID controllers bepalen double input_motor = PID_controller1(error); double input_motor2 = PID_controller2(error2); //signaal naar motor 1 sturen if(input_motor>1.0) //pwm signaal kan niet groter zijn dan 1, dus moet gelimiteerd worden {input_motor = 1.0;} else if(input_motor<-1.0) //absolute waarde wordt ingevuld, maar de negatieve kant is belangrijk voor de richting van de motor {input_motor = -1.0;} if(input_motor<0.0) //bepaal de richting aan de hand van het teken van het pwm signaal. {motor1_dir = 0;} else {motor1_dir = 1;} motor1_pwm.write(fabs(input_motor)); // de uiteindelijke input aan de motor geven //signaal naar motor 12 sturen if(input_motor2>1.0) //pwm signaal kan niet groter zijn dan 1, dus moet gelimiteerd worden {input_motor2 = 1.0;} else if(input_motor2<-1.0) //absolute waarde wordt ingevuld, maar de negatieve kant is belangrijk voor de richting van de motor {input_motor2 = -1.0;} if(input_motor2<0.0) //bepaal de richting aan de hand van het teken van het pwm signaal. {motor2_dir = 0;} else {motor2_dir = 1;} motor2_pwm.write(fabs(input_motor2)); // de uiteindelijke input aan de motor geven // 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); } void serialSampler(){ if(pc.readable()){ input = pc.getc(); pc.printf("Input = %c\r\n",input); }else if(input_switch==true){ pc.printf("Kp ingeschakeld\r\n"); }else{ pc.printf("Ki ingeschakeld\r\n"); } } int main() { sw2.fall(buttonpress); pc.baud(115200); motor1_pwm.period_us(60); motor1_pwm = 0; motor2_pwm.period_us(60); motor2_pwm = 0; Control.attach(aansturing, Ts); //de aansturing regelen met een sampletime serialEvent.attach(&serialSampler, 0.05); while (true) { double errorprint = motor1_ang-m1; double errorprint2 = motor2_ang-m2; 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); 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); pc.printf("toets=%c\r\n",input); wait(0.5); } }