Robot's source code
Dependencies: mbed
Diff: main.cpp
- Revision:
- 17:f360e21d3307
- Parent:
- 16:6bd245b26423
- Child:
- 20:2840a749fb55
--- 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