ARES
Robot's source code
Dependencies: mbed
Diff: main.cpp
- Revision:
- 17:f360e21d3307
- Parent:
- 16:6bd245b26423
- Child:
- 20:2840a749fb55
diff -r 6bd245b26423 -r f360e21d3307 main.cpp
--- a/main.cpp Fri Oct 03 14:53:35 2014 +0000
+++ b/main.cpp Fri Dec 12 12:44:59 2014 +0000
@@ -1,30 +1,72 @@
#include "mbed.h"
#include "QEI.h"
#include "Odometry.h"
-
+#include <iostream>
/*---------------------------------------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------------------------------------*/
/*KalmanFilter*/
#include "EKF.h"
-Mat<double> motion_bicycle2( Mat<double> state, Mat<double> command, double dt = 0.5);
-Mat<double> sensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5 );
-Mat<double> jmotion_bicycle2( Mat<double> state, Mat<double> command, double dt = 0.5);
-Mat<double> jsensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5);
+Mat<double> motion_bicycle3( Mat<double> state, Mat<double> command, double dt = 0.5);
+Mat<double> sensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5 );
+Mat<double> jmotion_bicycle3( Mat<double> state, Mat<double> command, double dt = 0.5);
+Mat<double> jsensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5);
void measurementCallback( Mat<double>* z, Odometry* odometry);
+bool setPWM(PwmOut *servo,float p);
Mat<double> bicycle(3,1);
+int reduc = 16;
/*---------------------------------------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------------------------------------*/
-
+/*----------------------------------------------------------------------------------------------*/
+ /*Serial*/
+ Serial pcs(USBTX, USBRX); // tx, rx
+/*----------------------------------------------------------------------------------------------*/
int main()
{
+
+
+ PwmOut pw1(p22);
+ DigitalOut dir1(p21);
+ PwmOut pw2(p24);
+ DigitalOut dir2(p23);
+
+ //mbuino
+ /*
+ PwmOut pw1(P0_17);
+ DigitalOut dir1(P0_18);
+ PwmOut pw2(P0_23);
+ DigitalOut dir2(P0_19);
+ */
+ /*
+ //nucleo
+ PwmOut pw1(PB_8);
+ DigitalOut dir1(D12);
+ PwmOut pw2(PB_9);
+ DigitalOut dir2(D13);
+ */
+ pw1.period_us(10);
+ pw2.period_us(10);
+
+
+ dir1.write(0);
+ dir2.write(0);
+ pw1.write(1.0);
+ pw2.write(0.8);
+ //setPWM(&pw1,0.9);
+ pcs.printf("mise à jour des pwm.\n");
+ //while(1);
/*----------------------------------------------------------------------------------------------*/
/*Odometry*/
- QEI qei_left(p15,p16,NC,1024,QEI::X4_ENCODING);
- QEI qei_right(p17,p18,NC,1024,QEI::X4_ENCODING);
+ QEI qei_left(p15,p16,NC,1024*reduc,QEI::X4_ENCODING);
+ //QEI qei_left(P0_2,P0_7,NC,1024*reduc,QEI::X4_ENCODING);//mbuino
+ //QEI qei_left(PA_3,PA_2,NC,1024*reduc,QEI::X4_ENCODING);//nucleo
+
+ QEI qei_right(p17,p18,NC,1024*reduc,QEI::X4_ENCODING);
+ //QEI qei_right(P0_8,P0_20,NC,1024*reduc,QEI::X4_ENCODING);//mbuino
+ //QEI qei_right(PA_10,PB_3,NC,1024*reduc,QEI::X4_ENCODING);//nucleo
Odometry odometry(&qei_left,&qei_right,0.07,0.07,0.26);
/*----------------------------------------------------------------------------------------------*/
@@ -41,7 +83,7 @@
int nbrstate = 5;
int nbrcontrol = 2;
- int nbrobs = 3;
+ int nbrobs = 5;
double dt = (double)0.05;
double stdnoise = (double)0.05;
@@ -49,57 +91,94 @@
initX.set( (double)0, 3,1);
bool extended = true;
- EKF<double> instance(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended);
+ bool filterOn = false;
+ EKF<double> instance(&pcs, nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended, filterOn);
- instance.initMotion(motion_bicycle2);
- instance.initSensor(sensor_bicycle2);
- instance.initJMotion(jmotion_bicycle2);
- instance.initJSensor(jsensor_bicycle2);
+ instance.initMotion(motion_bicycle3);
+ instance.initSensor(sensor_bicycle3);
+ instance.initJMotion(jmotion_bicycle3);
+ instance.initJSensor(jsensor_bicycle3);
/*desired State : (x y theta phiright phileft)*/
Mat<double> dX((double)0, nbrstate, 1);
- dX.set( (double)0, 1,1);
- dX.set( (double)100, 2,1);
+ dX.set( (double)100, 1,1);
+ dX.set( (double)0, 2,1);
dX.set( (double)0, 3,1);
dX.set( (double)0, 4,1);
dX.set( (double)0, 5,1);
+ Mat<double> ki((double)0, nbrcontrol, nbrstate);
+ Mat<double> kp((double)0, nbrcontrol, nbrstate);
+ Mat<double> kd((double)0, nbrcontrol, nbrstate);
+ //Mat<double> kdd((double)0.0015, nbrcontrol, nbrstate);
+
+ for(int i=1;i<=nbrstate;i++)
+ {
+ kp.set( (double)0.01, i, i);
+ kd.set( (double)0.0001, i, i);
+ ki.set( (double)0.0001, i, i);
+ }
+
+ instance.setKi(ki);
+ instance.setKp(kp);
+ instance.setKd(kd);
+ //instance.setKdd(kdd);
+
Mat<double> u(transpose( instance.getCommand()) );
/*Observations*/
- Mat<double> z(3,1);
+ /*il nous faut 5 observation :*/
+ Mat<double> z((double)0,5,1);
measurementCallback(&z, &odometry);
/*----------------------------------------------------------------------------------------------*/
-
-
- /*----------------------------------------------------------------------------------------------*/
- /*Serial*/
- Serial pc(USBTX, USBRX); // tx, rx
- /*----------------------------------------------------------------------------------------------*/
-
while(1)
{
- wait(1);
- pc.printf("%f : %f : %f\n",odometry.getX()*100,odometry.getY()*100,odometry.getTheta()*180/3.14);
-
+ //wait(1);
+ pcs.printf("%f : %f : %f\n",odometry.getX()*100,odometry.getY()*100,odometry.getTheta()*180/3.14);
+
/*------------------------------------------------------------------------------------------*/
- /*Asservissement*/
- measurementCallback(&z, &odometry);
- instance.measurement_Callback( z, dX );
+ /*Asservissement*/
+
+ //measurementCallback(&z, &odometry);
+ instance.measurement_Callback( instance.getX(), dX, true );
+
instance.state_Callback();
+
double phi_r = instance.getCommand().get(1,1);
double phi_l = instance.getCommand().get(2,1);
+ double phi_max = 100;
- instance.computeCommand(dX, (double)dt, -1);
- pc.printf("command : \n phi_r = %f \n phi_l = %f \n", phi_r/phi_max*100, phi_l/phi_max*100);
- instance.getX().afficher();
-
- /*------------------------------------------------------------------------------------------*/
+ instance.computeCommand(dX, (double)dt, -2);
+ pcs.printf("command : \n phi_r = %f \n phi_l = %f \n", phi_r/phi_max*100, phi_l/phi_max*100);
+ //instance.getX().afficher();
+
+
+ if(phi_r <= 0)
+ dir1.write(0);
+ else
+ dir1.write(1);
+
+ if(phi_l <= 0)
+ dir2.write(0);
+ else
+ dir2.write(1);
+
+ if(abs(phi_r/phi_max) < 1.0)
+ setPWM(&pw1, (float)abs(phi_r/phi_max));
+ else
+ cout << "P1..." << endl;
+
+ if(abs(phi_l/phi_max) < 1.0)
+ setPWM(&pw2,(float)abs(phi_l/phi_max));
+ else
+ pcs.printf("P2...");
+
+ pcs.printf("\n\n----------------- Commande mise executee. ------------------ \n\n");
}
}
@@ -111,16 +190,16 @@
z->set( (double)/*conversionUnitée rad*/odometry->getTheta(), 3,1);
}
-Mat<double> motion_bicycle2( Mat<double> state, Mat<double> command, double dt)
+Mat<double> motion_bicycle3( Mat<double> state, Mat<double> command, double dt)
{
Mat<double> r(state);
double v = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)+r.get(5,1));
- //double w = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)-r.get(5,1));
+ double w = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)-r.get(5,1));
r.set( r.get(1,1) + v*cos(r.get(3,1))*dt, 1,1);
r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
- double angle = (r.get(3,1) + dt/bicycle.get(3,1)*(r.get(4,1)-r.get(5,1)));
+ double angle = (r.get(3,1) + dt*w);
if( angle < -PI)
{
angle = angle - PI*ceil(angle/PI);
@@ -130,52 +209,79 @@
angle = angle - PI*floor(angle/PI);
}
- r.set( angle, 3,1);
+ r.set( atan21(sin(angle), cos(angle)), 3,1);
- r.set( bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)+command.get(2,1)), 4,1);
- r.set( bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)-command.get(2,1)), 5,1);
+
+ /*----------------------------------------*/
+ /*Modele du moteur*/
+ /*----------------------------------------*/
+ double r1 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)+command.get(2,1));
+ double r2 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)-command.get(2,1));
+
+
+ r.set( r1, 4,1);
+ r.set( r2, 5,1);
+
+
+ /*----------------------------------------*/
+ /*----------------------------------------*/
return r;
}
-Mat<double> sensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
-{
- return extract(state-d_state, 1,1, 3,1);
+Mat<double> sensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
+{
+ return state;
}
-Mat<double> jmotion_bicycle2( Mat<double> state, Mat<double> command, double dt)
+
+Mat<double> jmotion_bicycle3( Mat<double> state, Mat<double> command, double dt)
{
double h = numeric_limits<double>::epsilon()*10e2;
Mat<double> var( (double)0, state.getLine(), state.getColumn());
var.set( h, 1,1);
- Mat<double> G(motion_bicycle2(state, command, dt) - motion_bicycle2(state+var, command,dt));
+ Mat<double> G(motion_bicycle3(state, command, dt) - motion_bicycle3(state+var, command,dt));
for(int i=2;i<=state.getLine();i++)
{
var.set( (double)0, i-1,1);
var.set( h, i,1);
- G = operatorL(G, motion_bicycle2(state, command, dt) - motion_bicycle2(state+var, command,dt) );
+ G = operatorL(G, motion_bicycle3(state, command, dt) - motion_bicycle3(state+var, command,dt) );
}
return (1.0/h)*G;
}
-Mat<double> jsensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
+Mat<double> jsensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
{
double h = numeric_limits<double>::epsilon()*10e2;
Mat<double> var((double)0, state.getLine(), state.getColumn());
var.set( h, 1,1);
- Mat<double> H(sensor_bicycle2(state, command, d_state, dt) - sensor_bicycle2(state+var, command, d_state, dt));
+ Mat<double> H(sensor_bicycle3(state, command, d_state, dt) - sensor_bicycle3(state+var, command, d_state, dt));
for(int i=2;i<=state.getLine();i++)
- {
+ {
var.set( (double)0, i-1,1);
var.set( h, i,1);
- H = operatorL(H, sensor_bicycle2(state, command, d_state, dt) - sensor_bicycle2(state+var, command, d_state, dt) );
+ Mat<double> temp(sensor_bicycle3(state, command, d_state, dt) - sensor_bicycle3(state+var, command, d_state, dt));
+
+ H = operatorL(H, temp );
+ pcs.printf("sensor bicycle %d...\n",i);
}
return (1.0/h)*H;
+}
+
+bool setPWM(PwmOut *servo,float p)
+{
+ if(p <= 1.0f && p >= 0.0f)
+ {
+ servo->write(p);
+ return true;
+ }
+
+ return false;
}
\ 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 |
