ARES
Robot's source code
Dependencies: mbed
Diff: Asservissement/Asserv.h
- Revision:
- 43:87bdce65341f
- Parent:
- 42:fcb48e2fc426
- Child:
- 44:d5f95af61243
--- a/Asservissement/Asserv.h Wed Mar 18 13:47:40 2015 +0000
+++ b/Asservissement/Asserv.h Thu Mar 19 14:35:25 2015 +0000
@@ -1,7 +1,7 @@
/*KalmanFilter*/
#include "EKF.h"
#include "Odometry.h"
-
+/*
template<typename T>
Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt = (T)0.5);
template<typename T>
@@ -14,204 +14,7 @@
Mat<T> jsensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt = 0.5);
template<typename T>
bool setPWM(PwmOut *servo,T p);
-int reduc = 16;
-
-extern Serial logger;
-
-/*---------------------------------------------------------------------------------------------------------*/
-/*---------------------------------------------------------------------------------------------------------*/
-
-template<typename T>
-class Asserv
-{
- private :
-
- Odometry* odometry;
- EKF<T> instanceEKF;
- int nbrstate;
- int nbrcontrol;
- int nbrobs;
- T dt;
-
- T stdnoise;
- Mat<T> X;
- Mat<T> dX;
- Mat<T> initX;
- Mat<T> z;
- Mat<T> u;
- bool extended;
- bool filterOn;
-
- Mat<T> ki;
- Mat<T> kp;
- Mat<T> kd;
-
- public :
-
- T phi_r;
- T phi_l;
- T phi_max;
- bool execution;
- bool isarrived;
-
- Asserv(Odometry* odometry)
- {
- /*Odometry*/
- this->odometry = odometry;
- phi_max = (T)1.0;
-
- /*KalmanFilter*/
- //double phi_max = 100;
- /*en millimetres*/
-
- nbrstate = 5;
- nbrcontrol = 2;
- nbrobs = 5;
- dt = (T)0.05;
- stdnoise = (T)0.05;
-
- initX = Mat<T>((T)0, nbrstate, 1);
- initX.set( (T)0, 3,1);
- X = initX;
-
- extended = true;
- filterOn = true;
-
- instanceEKF = EKF<T>(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended, filterOn);
-
- instanceEKF.initMotion(motion_bicycle3);
- instanceEKF.initSensor(sensor_bicycle3);
- instanceEKF.initJMotion(jmotion_bicycle3);
- //instanceEKF.initJMotionCommand(jmotion_bicycle3_command);
- 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);
-
- ki = Mat<T>((T)0, nbrcontrol, nbrstate);
- kp = Mat<T>((T)0, nbrcontrol, nbrstate);
- kd = Mat<T>((T)0, nbrcontrol, nbrstate);
- //Mat<double> kdd((double)0.0015, nbrcontrol, nbrstate);
-
- for(int i=1;i<=nbrstate;i++)
- {
- kp.set( (T)0.01, i, i);
- kd.set( (T)0.0001, i, i);
- ki.set( (T)0.0001, i, i);
- }
-
- instanceEKF.setKi(ki);
- instanceEKF.setKp(kp);
- instanceEKF.setKd(kd);
- //instance.setKdd(kdd);
-
- u = Mat<T>(transpose( instanceEKF.getCommand()) );
-
- /*Observations*/
- /*il nous faut 5 observation : mais on n'en met à jour que 3...*/
- z = Mat<T>((T)0,5,1);
- this->measurementCallback(&z);
-
- /*----------------------------------------------------------------------------------------------*/
- isarrived = true;
- execution = false;
-
- }
-
- ~Asserv()
- {
-
- }
-
- void setGoal(T x, T y, T theta)
- {
- dX.set(x,1,1);
- dX.set(y,2,1);
- dX.set(theta,3,1);
- dX.set((T)0,4,1);
- dX.set((T)0,5,1);
- execution = true;
- isarrived = false;
-
- }
-
- bool isArrived() { return isarrived;}
-
- void stop()
- {
- execution = false;
- }
-
- void update(T deltat)
- {
- if(execution)
- {
- dt = deltat;
- /*Asservissement*/
- logger.printf("1\r\n");
- this->measurementCallback(&z);
- logger.printf("2\r\n");
- instanceEKF.measurement_Callback( z, dX, true );
- logger.printf("3\r\n");
- instanceEKF.state_Callback();
- //instance.setX(z);
- X = instanceEKF.getX();
- X.afficherMblue();
-
- //instance.computeCommand(dX, (double)dt, -2);
- instanceEKF.computeCommand(dX, (T)dt, -2);
- phi_r = instanceEKF.getCommand().get(1,1);
- phi_l = instanceEKF.getCommand().get(2,1);
- }
- else
- {
- phi_r = (T)0;
- phi_l = (T)0;
- }
- }
-
- void measurementCallback( Mat<T>* z)
- {
- 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);
- z->set( (T)odometry->getW(),5,1);
-
- //z->afficherM();
- }
-
- Mat<T> getX()
- {
- return X;
- }
-
- T getPhiR()
- {
- return phi_r;
- }
-
- T getPhiL()
- {
- return phi_l;
- }
-
- T getPhiMax()
- {
- return phi_max;
- }
-
-
-};
+*/
template<typename T>
bool setPWM(PwmOut *servo,T p)
@@ -227,8 +30,7 @@
template<typename T>
Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt)
-{
- logger.printf("motion\r\n");
+{
Mat<T> bicycle(3,1);
bicycle.set((T)36, 1,1); /*radius*/
bicycle.set((T)36, 2,1);
@@ -295,9 +97,8 @@
template<typename T>
Mat<T> jmotion_bicycle3( Mat<T> state, Mat<T> command, T dt)
-{
- logger.printf("jmotion\r\n");
- T h = sqrt(numeric_limits<T>::epsilon())*norme2(state)+ pow(numeric_limits<T>::epsilon(), (T)0.5);
+{
+ T h = pow(numeric_limits<T>::epsilon(),(T)0.5)*norme2(state)+ pow(numeric_limits<T>::epsilon(), (T)0.5);
Mat<T> var( (T)0, state.getLine(), state.getColumn());
var.set( h, 1,1);
Mat<T> G(motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt));
@@ -316,7 +117,7 @@
template<typename T>
Mat<T> jmotion_bicycle3_command(Mat<T> state, Mat<T> command, T dt)
{
- T h = pow(numeric_limits<T>::epsilon()), (T)0.5)+sqrt(numeric_limits<T>::epsilon())*norme2(state);
+ T h = pow(numeric_limits<T>::epsilon(), (T)0.5)+sqrt(numeric_limits<T>::epsilon())*norme2(state);
Mat<T> var( (T)0, command.getLine(), command.getColumn());
var.set( h, 1,1);
Mat<T> G(motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt));
@@ -367,4 +168,198 @@
return (1.0/(2*h))*H;
*/
-}
\ No newline at end of file
+}
+//int reduc = 16;
+
+extern Serial logger;
+
+/*---------------------------------------------------------------------------------------------------------*/
+/*---------------------------------------------------------------------------------------------------------*/
+
+template<typename T>
+class Asserv
+{
+ private :
+
+ Odometry* odometry;
+ EKF<T>* instanceEKF;
+ int nbrstate;
+ int nbrcontrol;
+ int nbrobs;
+ T dt;
+
+ T stdnoise;
+ Mat<T> X;
+ Mat<T> dX;
+ Mat<T> initX;
+ Mat<T> z;
+ Mat<T> u;
+ bool extended;
+ bool filterOn;
+
+ Mat<T> ki;
+ Mat<T> kp;
+ Mat<T> kd;
+
+ public :
+
+ T phi_r;
+ T phi_l;
+ T phi_max;
+ bool execution;
+ bool isarrived;
+
+ Asserv(Odometry* odometry)
+ {
+ /*Odometry*/
+ this->odometry = odometry;
+ phi_max = (T)1.0;
+
+ /*KalmanFilter*/
+ //double phi_max = 100;
+ /*en millimetres*/
+
+ nbrstate = 5;
+ nbrcontrol = 2;
+ nbrobs = 5;
+ dt = (T)0.05;
+ stdnoise = (T)0.05;
+
+ initX = Mat<T>((T)0, nbrstate, 1);
+ initX.set( (T)0, 3,1);
+ X = initX;
+
+ extended = true;
+ filterOn = true;
+
+ instanceEKF = new EKF<T>(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended, filterOn);
+
+ instanceEKF->initMotion(motion_bicycle3);
+ instanceEKF->initSensor(sensor_bicycle3);
+ instanceEKF->initJMotion(jmotion_bicycle3);
+ //instanceEKF.initJMotionCommand(jmotion_bicycle3_command);
+ 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);
+
+ ki = Mat<T>((T)0, nbrcontrol, nbrstate);
+ kp = Mat<T>((T)0, nbrcontrol, nbrstate);
+ kd = Mat<T>((T)0, nbrcontrol, nbrstate);
+ //Mat<double> kdd((double)0.0015, nbrcontrol, nbrstate);
+
+ for(int i=1;i<=nbrstate;i++)
+ {
+ kp.set( (T)0.01, i, i);
+ kd.set( (T)0.0001, i, i);
+ ki.set( (T)0.0001, i, i);
+ }
+
+ instanceEKF->setKi(ki);
+ instanceEKF->setKp(kp);
+ instanceEKF->setKd(kd);
+ //instance.setKdd(kdd);
+
+ u = Mat<T>(transpose( instanceEKF->getCommand()) );
+
+ /*Observations*/
+ /*il nous faut 5 observation : mais on n'en met à jour que 3...*/
+ z = Mat<T>((T)0,5,1);
+ this->measurementCallback(&z);
+
+ /*----------------------------------------------------------------------------------------------*/
+ isarrived = true;
+ execution = false;
+
+ }
+
+ ~Asserv()
+ {
+ delete instanceEKF;
+ }
+
+ void setGoal(T x, T y, T theta)
+ {
+ dX.set(x,1,1);
+ dX.set(y,2,1);
+ dX.set(theta,3,1);
+ dX.set((T)0,4,1);
+ dX.set((T)0,5,1);
+ execution = true;
+ isarrived = false;
+
+ }
+
+ bool isArrived() { return isarrived;}
+
+ void stop()
+ {
+ execution = false;
+ }
+
+ void update(T deltat)
+ {
+ if(execution)
+ {
+ dt = deltat;
+ /*Asservissement*/
+ this->measurementCallback(&z);
+ instanceEKF->measurement_Callback( z, dX, true );
+ instanceEKF->state_Callback();
+ //instance.setX(z);
+ X = instanceEKF->getX();
+
+ //instance.computeCommand(dX, (double)dt, -2);
+ instanceEKF->computeCommand(dX, (T)dt, -2);
+ phi_r = instanceEKF->getCommand().get(1,1);
+ phi_l = instanceEKF->getCommand().get(2,1);
+ }
+ else
+ {
+ phi_r = (T)0;
+ phi_l = (T)0;
+ }
+ }
+
+ void measurementCallback( Mat<T>* z)
+ {
+ 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);
+ z->set( (T)odometry->getW(),5,1);
+
+ //z->afficherM();
+ }
+
+ Mat<T> getX()
+ {
+ return X;
+ }
+
+ T getPhiR()
+ {
+ return phi_r;
+ }
+
+ T getPhiL()
+ {
+ return phi_l;
+ }
+
+ T getPhiMax()
+ {
+ return phi_max;
+ }
+
+
+};
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 |
