ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@37:41abbd7eaec1, 2015-03-10 (annotated)

Committer:
Jagang
Date:
Tue Mar 10 17:31:11 2015 +0000
Revision:
37:41abbd7eaec1
Parent:
36:54f86bc6fd80
Child:
38:28f476eacde4
Update

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 36:54f86bc6fd80 6 #include "Motor.h"
Near32 17:f360e21d3307 7 /*----------------------------------------------------------------------------------------------*/
Near32 17:f360e21d3307 8 /*Serial*/
Near32 17:f360e21d3307 9 Serial pcs(USBTX, USBRX); // tx, rx
Near32 17:f360e21d3307 10 /*----------------------------------------------------------------------------------------------*/
Jagang 21:5443f93819db 11
Jagang 23:228317fe0457 12 /* --- Initialisation de la liste des obstable --- */
Jagang 23:228317fe0457 13 int Obstacle::lastId = 0;
Jagang 23:228317fe0457 14
Jagang 0:41149573d577 15 int main()
Jagang 0:41149573d577 16 {
Near32 36:54f86bc6fd80 17 pcs.printf("Initialisation...");
Near32 24:3c0422e1ebd6 18 //mbed
Near32 24:3c0422e1ebd6 19 /*
Near32 17:f360e21d3307 20 PwmOut pw1(p22);
Near32 17:f360e21d3307 21 DigitalOut dir1(p21);
Near32 17:f360e21d3307 22 PwmOut pw2(p24);
Near32 17:f360e21d3307 23 DigitalOut dir2(p23);
Near32 24:3c0422e1ebd6 24 */
Near32 17:f360e21d3307 25
Near32 17:f360e21d3307 26 //mbuino
Near32 17:f360e21d3307 27 /*
Near32 17:f360e21d3307 28 PwmOut pw1(P0_17);
Near32 17:f360e21d3307 29 DigitalOut dir1(P0_18);
Near32 17:f360e21d3307 30 PwmOut pw2(P0_23);
Near32 17:f360e21d3307 31 DigitalOut dir2(P0_19);
Near32 17:f360e21d3307 32 */
Near32 17:f360e21d3307 33 /*
Near32 17:f360e21d3307 34 //nucleo
Near32 17:f360e21d3307 35 PwmOut pw1(PB_8);
Near32 17:f360e21d3307 36 DigitalOut dir1(D12);
Near32 17:f360e21d3307 37 PwmOut pw2(PB_9);
Near32 17:f360e21d3307 38 DigitalOut dir2(D13);
Near32 17:f360e21d3307 39 */
Near32 24:3c0422e1ebd6 40 //nucleo
Near32 36:54f86bc6fd80 41
Jagang 37:41abbd7eaec1 42 PwmOut pw1(PB_13);
Jagang 37:41abbd7eaec1 43 DigitalOut dir1(PC_9);
Jagang 37:41abbd7eaec1 44 PwmOut pw2(PB_14);
Jagang 37:41abbd7eaec1 45 DigitalOut dir2(PB_8);
Near32 17:f360e21d3307 46
Near32 36:54f86bc6fd80 47
Near32 36:54f86bc6fd80 48 Motor motorR(&pw1,&dir1);
Near32 36:54f86bc6fd80 49 Motor motorL(&pw2,&dir2);
Near32 24:3c0422e1ebd6 50 pcs.printf("mise a jour des pwm.\n");
Near32 26:54e1afed58b2 51 Timer t;
Near32 24:3c0422e1ebd6 52
Near32 3:573a0dc8383f 53 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 54 /*Odometry*/
Jagang 37:41abbd7eaec1 55 QEI qei_left(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
Jagang 37:41abbd7eaec1 56 QEI qei_right(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
Jagang 32:148147c0755e 57 Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,280);
Jagang 37:41abbd7eaec1 58 DigitalOut led(LED1);
Jagang 37:41abbd7eaec1 59 while(1)
Jagang 37:41abbd7eaec1 60 {
Jagang 37:41abbd7eaec1 61 pcs.printf("x: %f\ty: %f\ttheta: %f\r\n",odometry.getX(),odometry.getY(),odometry.getTheta());
Jagang 37:41abbd7eaec1 62 led = !led;
Jagang 37:41abbd7eaec1 63 wait(0.5);
Jagang 37:41abbd7eaec1 64 }
Near32 34:95b9e61c7dae 65 bool testOdo = true;
Near32 36:54f86bc6fd80 66
Near32 36:54f86bc6fd80 67
Near32 35:54b13e154801 68 Asserv<float> instanceAsserv(&odometry);
Near32 3:573a0dc8383f 69
Near32 3:573a0dc8383f 70 /*----------------------------------------------------------------------------------------------*/
Near32 36:54f86bc6fd80 71 instanceAsserv.setGoal( (float)10,(float)0, (float)PI/2);
Near32 3:573a0dc8383f 72
Near32 34:95b9e61c7dae 73 while(1)
Near32 26:54e1afed58b2 74 {
Near32 36:54f86bc6fd80 75 //Asservissement :
Near32 36:54f86bc6fd80 76 t.start();
Near32 36:54f86bc6fd80 77 instanceAsserv.update((float)t.read());
Near32 36:54f86bc6fd80 78
Near32 35:54b13e154801 79 Mat<float> X( instanceAsserv.getX() );
Near32 35:54b13e154801 80 float phi_r = instanceAsserv.getPhiR();
Near32 35:54b13e154801 81 float phi_l = instanceAsserv.getPhiL();
Near32 35:54b13e154801 82 float phi_max = instanceAsserv.getPhiMax();
Near32 34:95b9e61c7dae 83 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 36:54f86bc6fd80 84 //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);
Near32 36:54f86bc6fd80 85 //blue.printf("State : \n\r");
Near32 36:54f86bc6fd80 86 //(instanceAsserv.getX()).afficherMblue();
Near32 36:54f86bc6fd80 87 //blue.printf("Odometry : \n\r");
Near32 36:54f86bc6fd80 88 //z.afficherMblue();
Near32 36:54f86bc6fd80 89
Near32 36:54f86bc6fd80 90 //Mise a jour des moteurs :
Near32 31:e60cd1c984f4 91 if(!testOdo)
Near32 36:54f86bc6fd80 92 {
Near32 36:54f86bc6fd80 93 motorR.update((float)phi_r/phi_max);
Near32 36:54f86bc6fd80 94 motorL.update((float)phi_l/phi_max);
Near32 36:54f86bc6fd80 95 }
Near32 36:54f86bc6fd80 96 pcs.printf("\n\n----------------- Commandes moteurs : mises a jour. ------------------ \n");
Near32 31:e60cd1c984f4 97
Near32 36:54f86bc6fd80 98 //Timer Handling :
Near32 26:54e1afed58b2 99 t.stop();
Near32 36:54f86bc6fd80 100 pcs.printf("%f s ecoulee.\n\r", t.read());
Near32 26:54e1afed58b2 101 t.reset();
Near32 36:54f86bc6fd80 102 t.start();
Near32 26:54e1afed58b2 103 }
Near32 3:573a0dc8383f 104
Near32 26:54e1afed58b2 105
Near32 26:54e1afed58b2 106 while(1)
Near32 26:54e1afed58b2 107 {
Near32 26:54e1afed58b2 108 //setPWM(&pw1,0.0);
Near32 26:54e1afed58b2 109 //setPWM(&pw2,0.0);
Near32 26:54e1afed58b2 110 pw1.write(0.0);
Near32 26:54e1afed58b2 111 pw2.write(0.0);
Near32 26:54e1afed58b2 112 led = !led;
Near32 26:54e1afed58b2 113 wait(1);
Jagang 0:41149573d577 114 }
Near32 3:573a0dc8383f 115 }