Gabriel das Neves
controle pendulo roda reacao
Dependencies: mbed MatrixMath QEI Matrix USBDevice
Revision 0:200d9e41a3cf, committed 2020-03-11
- Comitter:
- gabrielpdn
- Date:
- Wed Mar 11 16:50:23 2020 +0000
- Commit message:
- versao 1
Changed in this revision
diff -r 000000000000 -r 200d9e41a3cf Matrix.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Matrix.lib Wed Mar 11 16:50:23 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/Yo_Robot/code/Matrix/#a4014ab0a8cf
diff -r 000000000000 -r 200d9e41a3cf MatrixMath.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MatrixMath.lib Wed Mar 11 16:50:23 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/Yo_Robot/code/MatrixMath/#93948a9bbde2
diff -r 000000000000 -r 200d9e41a3cf QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Wed Mar 11 16:50:23 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/gabrielpdn/code/QEI/#ce2f5a0d50b6
diff -r 000000000000 -r 200d9e41a3cf USBDevice.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBDevice.lib Wed Mar 11 16:50:23 2020 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/USBDevice/#53949e6131f6
diff -r 000000000000 -r 200d9e41a3cf main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Mar 11 16:50:23 2020 +0000
@@ -0,0 +1,195 @@
+#include "mbed.h"
+#include "math.h"
+#include "USBSerial.h"
+#include "QEI.h"
+#include "Matrix.h"
+#include "MatrixMath.h"
+
+
+#define Ts 0.01
+#define en2rad_roda 0.006698491798699
+//#define en2rad_pend 0.001591485640116
+//#define en2rad_roda 0.014660765717029
+#define en2rad_pend 0.001570796326795
+#define rad2graus 57.295779513082320
+#define pi 3.141592653589793
+#define omega_nd 210
+
+USBSerial PC;
+Ticker tick;
+
+QEI enc_roda (PTB17, PTD0, NC, 469);
+QEI enc_pend (PTA12, PTA13, NC, 1000);
+//QEI enc_roda (PTC2, PTC1, NC, 341.2);
+//QEI enc_pend (PTD6, PTD5, NC, 1974);
+
+DigitalOut OnBoardLed (D13);
+int aux = 0;
+float pac = 0.0;
+
+// Variaveis encoders
+float theta = 0.0, theta_1 = 0.0, dtheta = 0.0;
+float alpha = 0.0, alpha_1 = 0.0, dalpha = 0.0;
+float angulo = 0.0;
+
+float xc1 = 0.0, xc2 = 0.0, xc3 = 0.0, xc4 = 0.0;
+float dxc1 = 0.0, dxc2 = 0.0, dxc3 = 0.0, dxc4 = 0.0;
+
+// Motor
+DigitalOut InA(PTD5);
+DigitalOut InB(PTC1);
+PwmOut V(PTD6);
+float th5=0.0;
+float th5_1=0.0;
+float dth5=0.0;
+
+// Alimentacao encoder motor
+DigitalOut vcc_enc (PTB0);
+
+// Variaveis de medida
+int N=0;
+const int tempo = 15/Ts;
+float th[tempo], dth[tempo], alp[tempo], dalp[tempo], u[tempo];
+
+
+// variavel controle
+Matrix Z1(1,3);
+Matrix Z2(1,3);
+Matrix Z3(1,3);
+Matrix Y1(3,3);
+Matrix Y2(3,3);
+Matrix Y3(3,3);
+Matrix Z(1,3);
+Matrix Y(3,3);
+Matrix K(1,3);
+
+
+float lim_1 = -0.261799387799149;
+float lim_2 = 0.261799387799149;
+float rho1 = 0.0, rho2 = 0.0;
+float u_pwm = 0.0;
+
+// drive motor
+void drive(float u)
+{
+ int direction=0;
+ if (u>0)direction=1;
+ if (u<0)direction=2;
+ if(u==0)direction=0;
+ switch(direction) {
+ case 0:
+ V.write(u);
+ InA=0;
+ InB=0;
+ break;
+ case 1:
+ V.write(u);
+ InA=1;
+ InB=0;
+ break;
+ case 2:
+ V.write(-u);
+ InA=0;
+ InB=1;
+ break;
+ default:
+ V.write(0.0);
+ InA=0;
+ InB=0;
+ break;
+ }
+}
+
+// Interrupção
+void inverte() {
+ aux=1;
+ //N++;
+}
+
+int main ()
+{
+Z1 << 0.0528635583471 << 55.1775667591881 << -334.6107239814101;
+Z2 << 2.2178052729892 << 89.4963037908543 << -632.0606939025414;
+Z3 << 0.0527550080233 << 55.1818544517557 << -334.6407739018230;
+Y1 << 52.229273 << -293.202697 << -1000.523522
+ << -351.739401 << 8417.246435 << -49726.191528
+ << -487.165029 << -53707.951308 << 505666.562609;
+Y2 << 79.768194 << -492.424621 << -1138.974682
+ << -454.513497 << 15400.080131 << -103976.618630
+ << -1476.885923 << -101235.956011 << 992352.224275;
+Y3 << 52.229040 << -293.201469 << -1000.518230
+ << -351.740390 << 8417.269581 << -49726.325976
+ << -487.140469 << -53708.234657 << 505667.351490;
+
+// LQR contuinuo
+float K1 = 6.307583719273756;
+float K2 = 0.808578597390600;
+float K3 = 0.091291588353526;
+
+ vcc_enc = 1;
+ OnBoardLed = 1;
+ V.period(0.0001);
+ wait(5);
+ tick.attach(&inverte,Ts);
+
+ while (1)
+ {
+ if(aux == 1){
+ aux = 0;
+
+ theta=enc_roda.getPulses()*en2rad_roda;
+ dtheta=(theta-theta_1)/Ts;
+ theta_1=theta;
+
+ angulo=enc_pend.getPulses()*en2rad_pend;
+ alpha = angulo - pi*angulo/abs(angulo);
+ dalpha=(alpha-alpha_1)/Ts;
+ alpha_1=alpha;
+
+ // Disturbio
+ /*if (N>7/Ts && N<7.2/Ts){
+ alpha = alpha-7*pi/180;
+ }*/
+
+ // Variavel LPV
+ rho1 = (alpha - lim_1)/(lim_2-lim_1);
+ rho2 = 1-rho1;
+
+ // Controle
+ Y = rho1*rho1*Y1+rho1*rho2*Y2+rho2*rho2*Y3;
+ Z = rho1*rho1*Z1+rho1*rho2*Z2+rho2*rho2*Z3;
+ K = Z*MatrixMath::Inv(Y);
+ u_pwm = K.getNumber(1,1)*alpha + K.getNumber(1,2)*dalpha + K.getNumber(1,3)*dtheta;
+ //u_pwm = K1*alpha + K2*dalpha + K3*dtheta;
+
+
+ // Saturação da roda
+ if (u_pwm>1.0)
+ u_pwm=1.0;
+ if (u_pwm<-1.0)
+ u_pwm=-1.0;
+
+ drive(u_pwm);
+
+
+ if (N<tempo) {
+ th[N] = theta;
+ dth[N] = dtheta;
+ alp[N] = alpha;
+ dalp[N] = dalpha;
+ u[N] = u_pwm;
+ }
+
+ if (N == tempo) {
+ tick.detach();
+ drive(0.0);
+ for (N=0; N<tempo; N++)
+ PC.printf("%f %f %f %f %f\n\r", alp[N], dalp[N], th[N], dth[N], u[N]);
+ }
+
+ //PC.printf( "%f \n\r",alpha);
+ //PC.printf( "\n" );
+
+ }
+ }
+}
diff -r 000000000000 -r 200d9e41a3cf mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Mar 11 16:50:23 2020 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e7ca05fa8600 \ No newline at end of file