ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@35:54b13e154801, 2015-03-08 (annotated)

Committer:
Near32
Date:
Sun Mar 08 15:15:59 2015 +0000
Revision:
35:54b13e154801
Parent:
34:95b9e61c7dae
Child:
36:54f86bc6fd80
Asserv class : TODO : bicycle problem thingy...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 0:41149573d577 1 #include "mbed.h"
Jagang 0:41149573d577 2 #include "QEI.h"
Jagang 23:228317fe0457 3 #include "Map.h"
Jagang 0:41149573d577 4
Near32 35:54b13e154801 5 #include "Asserv.h"
Near32 17:f360e21d3307 6 /*----------------------------------------------------------------------------------------------*/
Near32 17:f360e21d3307 7 /*Serial*/
Near32 17:f360e21d3307 8 Serial pcs(USBTX, USBRX); // tx, rx
Near32 17:f360e21d3307 9 /*----------------------------------------------------------------------------------------------*/
Jagang 21:5443f93819db 10
Jagang 23:228317fe0457 11 /* --- Initialisation de la liste des obstable --- */
Jagang 23:228317fe0457 12 int Obstacle::lastId = 0;
Jagang 23:228317fe0457 13
Jagang 0:41149573d577 14 int main()
Jagang 0:41149573d577 15 {
Near32 24:3c0422e1ebd6 16 pcs.printf("demarrage");
Near32 24:3c0422e1ebd6 17 //mbed
Near32 24:3c0422e1ebd6 18 /*
Near32 17:f360e21d3307 19 PwmOut pw1(p22);
Near32 17:f360e21d3307 20 DigitalOut dir1(p21);
Near32 17:f360e21d3307 21 PwmOut pw2(p24);
Near32 17:f360e21d3307 22 DigitalOut dir2(p23);
Near32 24:3c0422e1ebd6 23 */
Near32 17:f360e21d3307 24
Near32 17:f360e21d3307 25 //mbuino
Near32 17:f360e21d3307 26 /*
Near32 17:f360e21d3307 27 PwmOut pw1(P0_17);
Near32 17:f360e21d3307 28 DigitalOut dir1(P0_18);
Near32 17:f360e21d3307 29 PwmOut pw2(P0_23);
Near32 17:f360e21d3307 30 DigitalOut dir2(P0_19);
Near32 17:f360e21d3307 31 */
Near32 17:f360e21d3307 32 /*
Near32 17:f360e21d3307 33 //nucleo
Near32 17:f360e21d3307 34 PwmOut pw1(PB_8);
Near32 17:f360e21d3307 35 DigitalOut dir1(D12);
Near32 17:f360e21d3307 36 PwmOut pw2(PB_9);
Near32 17:f360e21d3307 37 DigitalOut dir2(D13);
Near32 17:f360e21d3307 38 */
Near32 24:3c0422e1ebd6 39 //nucleo
Near32 24:3c0422e1ebd6 40 PwmOut pw1(PA_0);
Near32 24:3c0422e1ebd6 41 DigitalOut dir1(PB_8);
Near32 24:3c0422e1ebd6 42 PwmOut pw2(PA_1);
Near32 24:3c0422e1ebd6 43 DigitalOut dir2(PB_9);
Near32 17:f360e21d3307 44 pw1.period_us(10);
Near32 17:f360e21d3307 45 pw2.period_us(10);
Near32 17:f360e21d3307 46
Near32 17:f360e21d3307 47
Near32 17:f360e21d3307 48 dir1.write(0);
Near32 17:f360e21d3307 49 dir2.write(0);
Near32 24:3c0422e1ebd6 50
Near32 26:54e1afed58b2 51 pw1.write(0.0);
Near32 26:54e1afed58b2 52 pw2.write(0.0);
Near32 17:f360e21d3307 53 //setPWM(&pw1,0.9);
Near32 24:3c0422e1ebd6 54 pcs.printf("mise a jour des pwm.\n");
Near32 26:54e1afed58b2 55 Timer t;
Near32 24:3c0422e1ebd6 56
Near32 3:573a0dc8383f 57 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 58 /*Odometry*/
Jagang 32:148147c0755e 59 QEI qei_left(D2,D3,NC,1024*4,QEI::X4_ENCODING);
Jagang 32:148147c0755e 60 QEI qei_right(D4,D5,NC,1024*4,QEI::X4_ENCODING);
Jagang 32:148147c0755e 61 Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,280);
Near32 34:95b9e61c7dae 62 bool testOdo = true;
Near32 35:54b13e154801 63 Asserv<float> instanceAsserv(&odometry);
Near32 3:573a0dc8383f 64
Near32 3:573a0dc8383f 65 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 66
Near32 34:95b9e61c7dae 67 while(1)
Near32 26:54e1afed58b2 68 {
Near32 26:54e1afed58b2 69 t.start();
Near32 35:54b13e154801 70 //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);
Near32 35:54b13e154801 71 instanceAsserv.update();
Near32 26:54e1afed58b2 72 //pcs.printf("command : \n phi_r = %f \n phi_l = %f \n", ((double)phi_r/phi_max), ((double)phi_l/phi_max));
Near32 30:33e970ba1fe5 73 //(instance.getCommand()).afficher();
Near32 34:95b9e61c7dae 74 //blue.printf("State : \n\r");
Near32 34:95b9e61c7dae 75 //(instance.getX()).afficherMblue();
Near32 34:95b9e61c7dae 76 //blue.printf("Odometry : \n\r");
Near32 34:95b9e61c7dae 77 //z.afficherMblue();
Near32 35:54b13e154801 78 Mat<float> X( instanceAsserv.getX() );
Near32 35:54b13e154801 79 float phi_r = instanceAsserv.getPhiR();
Near32 35:54b13e154801 80 float phi_l = instanceAsserv.getPhiL();
Near32 35:54b13e154801 81 float phi_max = instanceAsserv.getPhiMax();
Near32 34:95b9e61c7dae 82 blue.printf(" x : %f ;\t y : %f ;\t theta : %f ;\t phiD = %f ;\t phiG = %f \n\r",X.get(1,1),X.get(2,1),X.get(3,1), X.get(4,1), X.get(5,1) );
Near32 30:33e970ba1fe5 83 //pcs.printf("Sigma : \n\r");
Near32 30:33e970ba1fe5 84 //(instance.getSigma()).afficher();
Near32 34:95b9e61c7dae 85 //pcs.printf("Kalman Gain : \n\r");
Near32 34:95b9e61c7dae 86 //(instance.getKGain()).afficherM();
Near32 17:f360e21d3307 87
Near32 17:f360e21d3307 88
Near32 24:3c0422e1ebd6 89 if(phi_r >= 0)
Near32 24:3c0422e1ebd6 90 dir1.write(1);
Near32 24:3c0422e1ebd6 91 else
Near32 17:f360e21d3307 92 dir1.write(0);
Near32 17:f360e21d3307 93
Near32 17:f360e21d3307 94 if(phi_l <= 0)
Near32 17:f360e21d3307 95 dir2.write(0);
Near32 17:f360e21d3307 96 else
Near32 17:f360e21d3307 97 dir2.write(1);
Near32 17:f360e21d3307 98
Near32 31:e60cd1c984f4 99 if(!testOdo)
Near32 31:e60cd1c984f4 100 {
Near32 31:e60cd1c984f4 101 if(abs(phi_r/phi_max) <= 1.0)
Near32 31:e60cd1c984f4 102 {
Near32 31:e60cd1c984f4 103 //pcs.printf( "VALUE PWM 1 : %f \n", (float)abs((double)phi_r/phi_max)) ;
Near32 31:e60cd1c984f4 104 setPWM(&pw1, (float)abs((double)phi_r/phi_max));
Near32 31:e60cd1c984f4 105 }
Near32 31:e60cd1c984f4 106 else
Near32 31:e60cd1c984f4 107 {
Near32 31:e60cd1c984f4 108 pcs.printf("P1...");
Near32 31:e60cd1c984f4 109 setPWM(&pw1, (float)abs((double)phi_max/phi_max));
Near32 31:e60cd1c984f4 110 }
Near32 31:e60cd1c984f4 111
Near32 31:e60cd1c984f4 112 if(abs(phi_l/phi_max) <= 1.0)
Near32 31:e60cd1c984f4 113 {
Near32 31:e60cd1c984f4 114 //pcs.printf( "VALUE PWM 2 : %f \n", (float)abs((double)phi_l/phi_max)) ;
Near32 31:e60cd1c984f4 115 setPWM(&pw2,(float)abs((double)phi_l/phi_max));
Near32 31:e60cd1c984f4 116 }
Near32 31:e60cd1c984f4 117 else
Near32 31:e60cd1c984f4 118 {
Near32 31:e60cd1c984f4 119 pcs.printf("P2...");
Near32 31:e60cd1c984f4 120 setPWM(&pw2, (float)abs((double)phi_max/phi_max));
Near32 31:e60cd1c984f4 121 }
Near32 30:33e970ba1fe5 122 }
Near32 17:f360e21d3307 123
Near32 26:54e1afed58b2 124 //pcs.printf("\n\n----------------- Commande mise executee. ------------------ \n");
Near32 26:54e1afed58b2 125 t.stop();
Near32 26:54e1afed58b2 126 pcs.printf("%f s \n\r", t.read());
Near32 26:54e1afed58b2 127 t.reset();
Near32 30:33e970ba1fe5 128 t.start();
Near32 26:54e1afed58b2 129
Near32 26:54e1afed58b2 130 }
Near32 3:573a0dc8383f 131
Near32 26:54e1afed58b2 132
Near32 26:54e1afed58b2 133 DigitalOut led(LED1);
Near32 26:54e1afed58b2 134 while(1)
Near32 26:54e1afed58b2 135 {
Near32 26:54e1afed58b2 136 //setPWM(&pw1,0.0);
Near32 26:54e1afed58b2 137 //setPWM(&pw2,0.0);
Near32 26:54e1afed58b2 138 pw1.write(0.0);
Near32 26:54e1afed58b2 139 pw2.write(0.0);
Near32 26:54e1afed58b2 140 led = !led;
Near32 26:54e1afed58b2 141 wait(1);
Jagang 0:41149573d577 142 }
Near32 3:573a0dc8383f 143 }