Jorn-Jan van de Beld / Mbed 2 deprecated PI_standaardsignalen

kom op

Dependencies:   QEI mbed

main.cpp@1:7969189824ff, 2017-11-02 (annotated)

Committer:
JornJan
Date:
Thu Nov 02 09:20:15 2017 +0000
Revision:
1:7969189824ff
Parent:
0:921402a95675
Child:
2:0686b2132556
voor kinimatica erin;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JornJan 0:921402a95675 1 #include "mbed.h"
JornJan 0:921402a95675 2 #include "Serial.h"
JornJan 0:921402a95675 3 #include "math.h"
JornJan 0:921402a95675 4 #include "QEI.h"
JornJan 0:921402a95675 5
JornJan 0:921402a95675 6 Serial pc(USBTX, USBRX); //Serial PC connectie
JornJan 0:921402a95675 7 DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord)
JornJan 0:921402a95675 8 PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord)
JornJan 0:921402a95675 9 DigitalOut motor2DirectionPin(D7); //Motorrichting op D4 (connected op het bord)
JornJan 0:921402a95675 10 PwmOut motor2MagnitudePin(D6); //Motorkracht op D5 (connected op het bord)
JornJan 0:921402a95675 11 QEI q1_enc(D13, D12, NC, 32); //encoder motor 1 instellen
JornJan 0:921402a95675 12 QEI q2_enc(D11, D10, NC, 32); // encoder motor 2 instellen
JornJan 0:921402a95675 13 const double pi = 3.1415926535897; // waarde voor pi aanmaken
JornJan 0:921402a95675 14
JornJan 0:921402a95675 15 Timer t;
JornJan 0:921402a95675 16 Ticker aansturing;
JornJan 0:921402a95675 17 Ticker stepres;
JornJan 0:921402a95675 18
JornJan 0:921402a95675 19 double ref1 = 0;
JornJan 0:921402a95675 20 double refrad1;
JornJan 0:921402a95675 21 double refrad2;
JornJan 0:921402a95675 22 double ref2 = 0;
JornJan 0:921402a95675 23 double Kp1 = 0.002; //kp motor 2 = 0.0001
JornJan 0:921402a95675 24 double Ki1 = 0;
JornJan 0:921402a95675 25 double Kp2 = 0.0001;
JornJan 0:921402a95675 26 double Ki2 = 0;
JornJan 0:921402a95675 27 int q1_puls;
JornJan 0:921402a95675 28 int q2_puls;
JornJan 0:921402a95675 29 double rad2pulses=4200/(2*pi);
JornJan 0:921402a95675 30 double ts = 0.001;
JornJan 0:921402a95675 31 double PI1;
JornJan 0:921402a95675 32 int n;
JornJan 0:921402a95675 33
JornJan 0:921402a95675 34 double error1_1;
JornJan 0:921402a95675 35 double error2_1 = 0;
JornJan 0:921402a95675 36 double error_I_1;
JornJan 0:921402a95675 37 double error_I2_1 = 0;
JornJan 0:921402a95675 38 double error1_2;
JornJan 0:921402a95675 39 double error2_2 = 0;
JornJan 0:921402a95675 40 double error_I_2;
JornJan 0:921402a95675 41 double error_I2_2 = 0;
JornJan 0:921402a95675 42 double PI2;
JornJan 0:921402a95675 43
JornJan 0:921402a95675 44 void controller()
JornJan 0:921402a95675 45 {
JornJan 0:921402a95675 46 refrad2 = 1.57f*sin(t.read()); //motor 2
JornJan 0:921402a95675 47 ref2 = refrad2 * rad2pulses;
JornJan 1:7969189824ff 48 refrad1 = -0.5f*cos(t.read()) + 0.6f; //motor 1
JornJan 0:921402a95675 49 ref1 = refrad2 * rad2pulses;
JornJan 0:921402a95675 50
JornJan 0:921402a95675 51 // encoders uitlezen
JornJan 0:921402a95675 52 q1_puls = q1_enc.getPulses();
JornJan 0:921402a95675 53 q2_puls = q2_enc.getPulses();
JornJan 0:921402a95675 54
JornJan 0:921402a95675 55 //PID 1
JornJan 0:921402a95675 56 error1_1 = ref1 - q1_puls;
JornJan 0:921402a95675 57 error_I_1 = error_I2_1 + ts*((error1_1 + error2_1)/2);
JornJan 0:921402a95675 58
JornJan 0:921402a95675 59 PI1 = Kp1*error1_1 + Ki1*error_I_1;
JornJan 0:921402a95675 60
JornJan 0:921402a95675 61 error2_1 = error1_1; // opslaan variabelen voor integraal onderdeel
JornJan 0:921402a95675 62 error_I2_1 = error_I_1;
JornJan 0:921402a95675 63
JornJan 0:921402a95675 64 //PID 2
JornJan 0:921402a95675 65 error1_2 = ref2 - q2_puls;
JornJan 0:921402a95675 66 error_I_2 = error_I2_2 + ts*((error1_2 + error2_2)/2);
JornJan 0:921402a95675 67
JornJan 0:921402a95675 68 PI2 = Kp2*error1_2 + Ki2*(error_I_2);
JornJan 0:921402a95675 69
JornJan 0:921402a95675 70
JornJan 0:921402a95675 71 if (PI1<=0)
JornJan 0:921402a95675 72 {
JornJan 0:921402a95675 73 motor1DirectionPin = 1;
JornJan 0:921402a95675 74 motor1MagnitudePin = fabs(PI1);
JornJan 0:921402a95675 75 }
JornJan 0:921402a95675 76 else if (PI1>0)
JornJan 0:921402a95675 77 {
JornJan 0:921402a95675 78 motor1DirectionPin = 0;
JornJan 0:921402a95675 79 motor1MagnitudePin = fabs(PI1);
JornJan 0:921402a95675 80 }
JornJan 0:921402a95675 81
JornJan 0:921402a95675 82 if (PI2>=0)
JornJan 0:921402a95675 83 {
JornJan 0:921402a95675 84 motor2DirectionPin = 1;
JornJan 1:7969189824ff 85 //motor2MagnitudePin = fabs(PI2);
JornJan 0:921402a95675 86 }
JornJan 0:921402a95675 87 else if(PI2<0)
JornJan 0:921402a95675 88 {
JornJan 0:921402a95675 89 motor2DirectionPin = 0;
JornJan 1:7969189824ff 90 //motor2MagnitudePin = fabs(PI2);
JornJan 0:921402a95675 91 }
JornJan 0:921402a95675 92
JornJan 0:921402a95675 93 if(n==500){
JornJan 1:7969189824ff 94 printf("error1 = %f error2 = %f\n\r", error1_1, error1_2);
JornJan 0:921402a95675 95 n=0;
JornJan 0:921402a95675 96 }
JornJan 0:921402a95675 97 else{
JornJan 0:921402a95675 98 n=n+1;
JornJan 0:921402a95675 99 }
JornJan 0:921402a95675 100
JornJan 0:921402a95675 101 }
JornJan 0:921402a95675 102
JornJan 0:921402a95675 103 void stapfunc()
JornJan 0:921402a95675 104 {
JornJan 0:921402a95675 105 if (ref1==0){
JornJan 0:921402a95675 106 ref1 = 1000;
JornJan 0:921402a95675 107 }
JornJan 0:921402a95675 108 else if (ref1==1000)
JornJan 0:921402a95675 109 {
JornJan 0:921402a95675 110 ref1=0;
JornJan 0:921402a95675 111 }
JornJan 0:921402a95675 112 }
JornJan 0:921402a95675 113
JornJan 0:921402a95675 114 int main()
JornJan 0:921402a95675 115 {
JornJan 0:921402a95675 116 pc.baud(115200);
JornJan 0:921402a95675 117 t.start();
JornJan 0:921402a95675 118 aansturing.attach_us(&controller, 1000);
JornJan 0:921402a95675 119 //stepres.attach(&stapfunc, 3);
JornJan 0:921402a95675 120
JornJan 0:921402a95675 121 while(true){
JornJan 0:921402a95675 122
JornJan 0:921402a95675 123 }
JornJan 0:921402a95675 124
JornJan 0:921402a95675 125
JornJan 0:921402a95675 126 }