Jorn-Jan van de Beld / Mbed 2 deprecated PI_standaardsignalen

kom op

Dependencies:   QEI mbed

main.cpp@3:3b5b85a32c9a, 2017-11-02 (annotated)

Committer:
JornJan
Date:
Thu Nov 02 13:01:45 2017 +0000
Revision:
3:3b5b85a32c9a
Parent:
2:0686b2132556
Child:
4:e98ad06f227c
niet werkend control expert aanwezig;

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 3:3b5b85a32c9a 18 Ticker kinmat;
JornJan 0:921402a95675 19
JornJan 0:921402a95675 20 double ref1 = 0;
JornJan 0:921402a95675 21 double refrad1;
JornJan 0:921402a95675 22 double refrad2;
JornJan 0:921402a95675 23 double ref2 = 0;
JornJan 0:921402a95675 24 double Kp1 = 0.002; //kp motor 2 = 0.0001
JornJan 0:921402a95675 25 double Ki1 = 0;
JornJan 0:921402a95675 26 double Kp2 = 0.0001;
JornJan 0:921402a95675 27 double Ki2 = 0;
JornJan 0:921402a95675 28 int q1_puls;
JornJan 0:921402a95675 29 int q2_puls;
JornJan 3:3b5b85a32c9a 30 double rad2pulses=(2100/pi);
JornJan 2:0686b2132556 31 double pulses2rad = (2*pi)/4200;
JornJan 0:921402a95675 32 double ts = 0.001;
JornJan 0:921402a95675 33 double PI1;
JornJan 0:921402a95675 34 int n;
JornJan 0:921402a95675 35
JornJan 0:921402a95675 36 double error1_1;
JornJan 0:921402a95675 37 double error2_1 = 0;
JornJan 0:921402a95675 38 double error_I_1;
JornJan 0:921402a95675 39 double error_I2_1 = 0;
JornJan 0:921402a95675 40 double error1_2;
JornJan 0:921402a95675 41 double error2_2 = 0;
JornJan 0:921402a95675 42 double error_I_2;
JornJan 0:921402a95675 43 double error_I2_2 = 0;
JornJan 0:921402a95675 44 double PI2;
JornJan 0:921402a95675 45
JornJan 2:0686b2132556 46 // kinematica gegevens
JornJan 2:0686b2132556 47 // lengte armen
JornJan 2:0686b2132556 48 double L1 = 0.250;
JornJan 2:0686b2132556 49 double L2 = 0.355;
JornJan 2:0686b2132556 50 double L3 = 0.150;
JornJan 2:0686b2132556 51
JornJan 2:0686b2132556 52 // reference position
JornJan 2:0686b2132556 53 double q1=0; // positie q1 in radialen
JornJan 2:0686b2132556 54 double q2=0; // positie q2 in radialen
JornJan 2:0686b2132556 55 double q1_pos;
JornJan 2:0686b2132556 56 double q2_pos;
JornJan 2:0686b2132556 57
JornJan 2:0686b2132556 58 // EMG Input_k
JornJan 2:0686b2132556 59 double vx;
JornJan 2:0686b2132556 60 double vy;
JornJan 2:0686b2132556 61
JornJan 2:0686b2132556 62
JornJan 2:0686b2132556 63 void kinematica()
JornJan 2:0686b2132556 64 {
JornJan 2:0686b2132556 65 // encoders uitlezen
JornJan 3:3b5b85a32c9a 66 q1_puls = -q1_enc.getPulses();
JornJan 3:3b5b85a32c9a 67 q1_pos = q1_puls*pulses2rad; // berekent positie q1 in radialen
JornJan 2:0686b2132556 68 q2_puls = q2_enc.getPulses();
JornJan 3:3b5b85a32c9a 69 q2_pos = q2_puls*pulses2rad; // berekent positie q2 in radialen
JornJan 2:0686b2132556 70
JornJan 3:3b5b85a32c9a 71 vx = 0.04f*sin( t.read()/20 );
JornJan 2:0686b2132556 72 vy = 0;
JornJan 2:0686b2132556 73
JornJan 3:3b5b85a32c9a 74 q1 = ((-(L3 + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vx + ((L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * 5 + q1_pos;
JornJan 3:3b5b85a32c9a 75 q2 = (((L3 - L1*sin(q1_pos) + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos))*vx) + ((L1*cos(q1_pos) - L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * 5 + q2_pos;
JornJan 2:0686b2132556 76
JornJan 2:0686b2132556 77 ref1 = q1*rad2pulses;
JornJan 2:0686b2132556 78 ref2 = q2*rad2pulses;
JornJan 3:3b5b85a32c9a 79
JornJan 3:3b5b85a32c9a 80 if (ref1 >=0)
JornJan 3:3b5b85a32c9a 81 {
JornJan 3:3b5b85a32c9a 82 ref1 = ceil(ref1);
JornJan 3:3b5b85a32c9a 83 }
JornJan 3:3b5b85a32c9a 84 else if (ref1<0)
JornJan 3:3b5b85a32c9a 85 {
JornJan 3:3b5b85a32c9a 86 ref1 = floor(ref1);
JornJan 3:3b5b85a32c9a 87 }
JornJan 3:3b5b85a32c9a 88 if (ref1 >=0)
JornJan 3:3b5b85a32c9a 89 {
JornJan 3:3b5b85a32c9a 90 ref2 = ceil(ref2);
JornJan 3:3b5b85a32c9a 91 }
JornJan 3:3b5b85a32c9a 92 else if (ref1<0)
JornJan 3:3b5b85a32c9a 93 {
JornJan 3:3b5b85a32c9a 94 ref2 = floor(ref2);
JornJan 3:3b5b85a32c9a 95 }
JornJan 2:0686b2132556 96 }
JornJan 2:0686b2132556 97
JornJan 2:0686b2132556 98
JornJan 0:921402a95675 99 void controller()
JornJan 3:3b5b85a32c9a 100 {
JornJan 0:921402a95675 101
JornJan 0:921402a95675 102 //PID 1
JornJan 0:921402a95675 103 error1_1 = ref1 - q1_puls;
JornJan 0:921402a95675 104 error_I_1 = error_I2_1 + ts*((error1_1 + error2_1)/2);
JornJan 0:921402a95675 105
JornJan 0:921402a95675 106 PI1 = Kp1*error1_1 + Ki1*error_I_1;
JornJan 0:921402a95675 107
JornJan 0:921402a95675 108 error2_1 = error1_1; // opslaan variabelen voor integraal onderdeel
JornJan 0:921402a95675 109 error_I2_1 = error_I_1;
JornJan 0:921402a95675 110
JornJan 0:921402a95675 111 //PID 2
JornJan 0:921402a95675 112 error1_2 = ref2 - q2_puls;
JornJan 0:921402a95675 113 error_I_2 = error_I2_2 + ts*((error1_2 + error2_2)/2);
JornJan 0:921402a95675 114
JornJan 0:921402a95675 115 PI2 = Kp2*error1_2 + Ki2*(error_I_2);
JornJan 0:921402a95675 116
JornJan 0:921402a95675 117
JornJan 0:921402a95675 118 if (PI1<=0)
JornJan 0:921402a95675 119 {
JornJan 0:921402a95675 120 motor1DirectionPin = 1;
JornJan 0:921402a95675 121 motor1MagnitudePin = fabs(PI1);
JornJan 0:921402a95675 122 }
JornJan 0:921402a95675 123 else if (PI1>0)
JornJan 0:921402a95675 124 {
JornJan 0:921402a95675 125 motor1DirectionPin = 0;
JornJan 0:921402a95675 126 motor1MagnitudePin = fabs(PI1);
JornJan 0:921402a95675 127 }
JornJan 0:921402a95675 128
JornJan 0:921402a95675 129 if (PI2>=0)
JornJan 0:921402a95675 130 {
JornJan 0:921402a95675 131 motor2DirectionPin = 1;
JornJan 3:3b5b85a32c9a 132 motor2MagnitudePin = fabs(PI2);
JornJan 0:921402a95675 133 }
JornJan 0:921402a95675 134 else if(PI2<0)
JornJan 0:921402a95675 135 {
JornJan 0:921402a95675 136 motor2DirectionPin = 0;
JornJan 3:3b5b85a32c9a 137 motor2MagnitudePin = fabs(PI2);
JornJan 0:921402a95675 138 }
JornJan 0:921402a95675 139
JornJan 0:921402a95675 140 if(n==500){
JornJan 3:3b5b85a32c9a 141 printf("PI1 = %f PI2 = %f ref1 = %f ref2 = %f\n\r", PI1, PI2, ref1, ref2);
JornJan 0:921402a95675 142 n=0;
JornJan 0:921402a95675 143 }
JornJan 0:921402a95675 144 else{
JornJan 0:921402a95675 145 n=n+1;
JornJan 0:921402a95675 146 }
JornJan 0:921402a95675 147
JornJan 0:921402a95675 148 }
JornJan 0:921402a95675 149
JornJan 0:921402a95675 150 void stapfunc()
JornJan 0:921402a95675 151 {
JornJan 0:921402a95675 152 if (ref1==0){
JornJan 0:921402a95675 153 ref1 = 1000;
JornJan 0:921402a95675 154 }
JornJan 0:921402a95675 155 else if (ref1==1000)
JornJan 0:921402a95675 156 {
JornJan 0:921402a95675 157 ref1=0;
JornJan 0:921402a95675 158 }
JornJan 0:921402a95675 159 }
JornJan 0:921402a95675 160
JornJan 0:921402a95675 161 int main()
JornJan 0:921402a95675 162 {
JornJan 0:921402a95675 163 pc.baud(115200);
JornJan 0:921402a95675 164 t.start();
JornJan 0:921402a95675 165 aansturing.attach_us(&controller, 1000);
JornJan 3:3b5b85a32c9a 166 kinmat.attach(&kinematica, 0.1);
JornJan 0:921402a95675 167 //stepres.attach(&stapfunc, 3);
JornJan 0:921402a95675 168
JornJan 0:921402a95675 169 while(true){
JornJan 0:921402a95675 170
JornJan 0:921402a95675 171 }
JornJan 0:921402a95675 172
JornJan 0:921402a95675 173
JornJan 0:921402a95675 174 }