ARES
Robot's source code
Dependencies: mbed
Diff: Asservissement/Asserv.h
- Revision:
- 94:5c37bcf73d14
- Parent:
- 81:08eacb6179d8
- Child:
- 100:a827a645d6c2
--- a/Asservissement/Asserv.h Fri Apr 24 11:12:44 2015 +0000
+++ b/Asservissement/Asserv.h Thu Apr 30 16:03:55 2015 +0000
@@ -1,7 +1,9 @@
/*KalmanFilter*/
#include "EKF.h"
#include "Odometry.h"
-#define debugAsserv
+//#define debugAsserv
+#define verboseGOAL
+
/*
template<typename T>
Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt = (T)0.5);
@@ -51,14 +53,26 @@
r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
T angle = (r.get(3,1) + dt*w);
- if( angle < -PI)
+
+ while(angle > (T)PI)
{
- angle = angle - PI*ceil(angle/PI);
+ angle -= 2.0f*(T)PI;
+ }
+ while(angle <= -(T)PI)
+ {
+ angle += 2.0f*(T)PI;
}
- else if( angle > PI)
+
+ /*
+ if( angle < -(T)PI)
{
- angle = angle - PI*floor(angle/PI);
+ angle = angle - (T)PI*ceil(angle/PI);
}
+ else if( angle > (T)PI)
+ {
+ angle = angle - (T)PI*floor(angle/PI);
+ }
+ */
r.set( angle, 3,1);
@@ -227,6 +241,7 @@
bool isarrived;
bool isarrivedalpha;
bool isarrivedrho;
+ int behaviour;
Asserv(Odometry* odometry)
{
@@ -260,12 +275,7 @@
instanceEKF->initJSensor(jsensor_bicycle3);
/*desired State : (x y theta phiright phileft)*/
- dX = Mat<T>((T)0, nbrstate, 1);
- dX.set( (T)0, 1,1);
- dX.set( (T)0, 2,1);
- dX.set( (T)0, 3,1);
- dX.set( (T)0, 4,1);
- dX.set( (T)0, 5,1);
+ dX = Mat<T>((T)0, nbrstate, 1);
dXalpha = dX;
dXrho = dX;
@@ -299,6 +309,7 @@
isarrivedalpha = true;
isarrivedrho = true;
execution = false;
+ behaviour = 0; //alpha : 1 : rho : 2 : dX (beta)
}
@@ -309,6 +320,7 @@
void setGoal(T x, T y, T theta)
{
+ instanceEKF->resetPID();
dX.set(x,1,1);
dX.set(y,2,1);
dX.set(theta,3,1);
@@ -318,11 +330,25 @@
isarrived = false;
isarrivedalpha = false;
isarrivedrho = false;
+ behaviour = 0;
- T alpha = atan21((dX.get(2,1)-X.get(2,1)),( dX.get(1,1) - X.get(1,1)));
- alpha = alpha - atan21(sin(X.get(3,1)),cos(X.get(3,1)));
- //alpha = atan21( sin(alpha), cos(alpha));
+ T alpha = atan2((dX.get(2,1)-X.get(2,1)),( dX.get(1,1) - X.get(1,1)));
+ alpha = alpha - X.get(3,1);//atan2(sin(X.get(3,1)),cos(X.get(3,1)));
+
+ while(alpha > (T)PI)
+ {
+ alpha -= 2.0f*(T)PI;
+ }
+ while(alpha <= -(T)PI)
+ {
+ alpha += 2.0f*(T)PI;
+ }
+
+ dXalpha.set( X.get(1,1), 1,1);
+ dXalpha.set( X.get(2,1), 2,1);
dXalpha.set( alpha, 3,1);
+ dXalpha.set((T)0, 4,1);
+ dXalpha.set((T)0, 5,1);
dXrho = dX;
dXrho.set( alpha, 3,1);
@@ -350,14 +376,13 @@
/*Gestion des comportements : alpha --> rho --> beta --> isarrived = true;*/
/*------------------------------------------------------------------------*/
-
- int behaviour = 0; //alpha : 1 : rho : 2 : dX (beta)
- Mat<T> dXbehaviour(dX);
- if(isarrivedalpha && && phi_l+phi_r <= phi_max/10)
+
+ Mat<T> dXbehaviour(dX);
+ if(isarrivedalpha && phi_l+phi_r <= phi_max/20 && behaviour == 0)
{
behaviour=1;
}
- if(isarrivedalpha && isarrivedrho && && phi_l+phi_r <= phi_max/10)
+ if(isarrivedalpha && isarrivedrho && phi_l+phi_r <= phi_max/20 && behaviour == 1)
{
behaviour = 2;
}
@@ -365,7 +390,7 @@
switch(behaviour)
{
case 0:
- if(fabs_(dXalpha.get(3,1)-X.get(3,1) ) <= (T)0.02)
+ if(fabs_(dXalpha.get(3,1)-X.get(3,1) ) <= (T)0.1)
{
behaviour = 1;
dXbehaviour = dXrho;
@@ -394,7 +419,7 @@
break;
default:
- if(norme2(dX-X) <=10)
+ if(norme2(dX-X) <=5)
{
isarrived = true;
}
@@ -405,19 +430,40 @@
dXbehaviour = dX;
break;
}
+
+#ifdef verboseGOAL
+ logger.printf("GOAL : ");
+ transpose(dXbehaviour).afficherMblue();
+ logger.printf("\r\nPOSE : ");
+ transpose(X).afficherMblue();
+#endif
/*------------------------------------------------*/
- if( norme2( extract( dX-X,1,1,2,1)) <= 10 )
- isarrivedrho = true;
+ //if( norme2( extract( dX-X,1,1,2,1)) <= 10 )
+ // isarrivedrho = true;
#ifdef debugAsserv
- logger.printf("BEHAVIOUR ASSERV : %d\r\n", behabiour);
+ logger.printf("BEHAVIOUR ASSERV : %d\r\n", behaviour);
#endif
- instanceEKF->measurement_Callback( z, dXbehaviour, true );
+ instanceEKF->measurement_Callback( z, dXbehaviour, true );
+
+#ifdef debugAsserv
+ logger.printf("MEASUREMENT CALLBACK : DONE.\r\n");
+#endif
//instanceEKF->state_Callback();
instanceEKF->setX(z);
- X = instanceEKF->getX();
+ X = instanceEKF->getX();
- instanceEKF->computeCommand(dXbehaviour, (T)dt, -1);
+#ifdef debugAsserv
+ logger.printf("STATE CALLBACK : DONE. \r\n");
+ transpose(X).afficherMblue();
+ transpose(z).afficherMblue();
+#endif
+
+ //instanceEKF->computeCommand(dXbehaviour, (T)dt, -1);
+ instanceEKF->computeCommand(dXbehaviour, (T)dt, 1);
+#ifdef debugAsserv
+ logger.printf("COMMAND CALLBACK : DONE. \r\n");
+#endif
//instanceEKF->computeCommand(dX, (T)dt, -2);
phi_l = instanceEKF->getCommand().get(1,1);
phi_r = instanceEKF->getCommand().get(2,1);
@@ -433,9 +479,12 @@
{
z->set( (T)/*conversionUnitée mm */this->odometry->getX(), 1,1);
z->set( (T)/*conversionUnitée mm*/this->odometry->getY(), 2,1);
+
T theta = (T)this->odometry->getTheta();
theta = atan21(sin(theta),cos(theta));
+
z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);
+
T vx = (T)this->odometry->getVx();
T vy = (T)this->odometry->getVy();
z->set( sqrt(vx*vx+vy*vy),4,1);
@@ -480,4 +529,4 @@
}
-};
+};
\ No newline at end of file
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 14 Dec 2014 |
| Imports: | 17 |
| Forks: | 0 |
| Commits: | 124 |
| Dependents: | 0 |
| Dependencies: | 1 |
| Followers: | 8 |
