ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@69:6354497f9f59, 2015-04-10 (annotated)

Committer:
sype
Date:
Fri Apr 10 05:13:51 2015 +0000
Revision:
69:6354497f9f59
Parent:
68:1ef8b1a37252
Child:
70:56086a37f31f
Parametrage coef

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 0:41149573d577 1 #include "mbed.h"
Jagang 39:09c04fd42c94 2
Jagang 39:09c04fd42c94 3 #include "defines.h"
Jagang 39:09c04fd42c94 4
Jagang 0:41149573d577 5 #include "QEI.h"
Jagang 23:228317fe0457 6 #include "Map.h"
Jagang 39:09c04fd42c94 7 #include "AX12.h"
Jagang 0:41149573d577 8
Near32 42:fcb48e2fc426 9
Near32 42:fcb48e2fc426 10 #include "Odometry.h"
Jagang 56:eb0600022463 11 //#include "Asserv.h"
Jagang 62:454cd844fe1e 12 #include "planB.h"
Near32 36:54f86bc6fd80 13 #include "Motor.h"
Near32 17:f360e21d3307 14 /*----------------------------------------------------------------------------------------------*/
Jagang 39:09c04fd42c94 15 /*Serial*/
Near32 48:cb3ebbc27db3 16 Serial logger(OUT_TX, OUT_RX); // tx, rx
Near32 48:cb3ebbc27db3 17 //Serial logger(USBTX,USBRX);
Near32 48:cb3ebbc27db3 18 //logger.baud((int)115200);
Near32 17:f360e21d3307 19 /*----------------------------------------------------------------------------------------------*/
Jagang 21:5443f93819db 20
Jagang 23:228317fe0457 21 /* --- Initialisation de la liste des obstable --- */
Jagang 23:228317fe0457 22 int Obstacle::lastId = 0;
Jagang 23:228317fe0457 23
Jagang 0:41149573d577 24 int main()
Jagang 0:41149573d577 25 {
Jagang 39:09c04fd42c94 26 logger.printf("Initialisation...\r\n");
Near32 36:54f86bc6fd80 27
Jagang 40:83ce8d1072ef 28 PwmOut pw1(PWM_MOT1);
Jagang 40:83ce8d1072ef 29 DigitalOut dir1(DIR_MOT1);
Jagang 40:83ce8d1072ef 30 PwmOut pw2(PWM_MOT2);
Jagang 40:83ce8d1072ef 31 DigitalOut dir2(DIR_MOT2);
Near32 17:f360e21d3307 32
Near32 36:54f86bc6fd80 33
Near32 48:cb3ebbc27db3 34 Motor motorL(PWM_MOT1,DIR_MOT1);
Near32 48:cb3ebbc27db3 35 Motor motorR(PWM_MOT2,DIR_MOT2);
Jagang 57:ab13f4e7a2b2 36
Jagang 39:09c04fd42c94 37 Timer t;
Jagang 39:09c04fd42c94 38
Jagang 63:fd9af0693e50 39 //AX12 test(PA_9,NC,0x03,250000);
Jagang 54:e0e58c36658a 40
Jagang 39:09c04fd42c94 41 AnalogIn ain0(PA_0);
Jagang 39:09c04fd42c94 42 AnalogIn ain1(PA_1);
Jagang 39:09c04fd42c94 43 AnalogIn ain2(PA_4);
Jagang 39:09c04fd42c94 44 AnalogIn ain3(PB_0);
Jagang 39:09c04fd42c94 45 AnalogIn ain4(PC_1);
Near32 24:3c0422e1ebd6 46
Near32 3:573a0dc8383f 47 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 48 /*Odometry*/
Near32 48:cb3ebbc27db3 49 QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
Near32 48:cb3ebbc27db3 50 QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
Jagang 32:148147c0755e 51 Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,280);
Jagang 60:546d7b43333b 52
Jagang 63:fd9af0693e50 53 DigitalOut led1(LED1);
Jagang 63:fd9af0693e50 54 DigitalOut led2(LED2);
Jagang 63:fd9af0693e50 55 DigitalOut led3(LED3);
Jagang 39:09c04fd42c94 56
Jagang 67:0bdf181586b5 57 /*t.start();
Jagang 63:fd9af0693e50 58 t.reset();
Jagang 65:b5d7f13870be 59 while(0)
Jagang 60:546d7b43333b 60 {
Jagang 63:fd9af0693e50 61 odometry.update(t.read());
Jagang 63:fd9af0693e50 62 t.reset();
Jagang 63:fd9af0693e50 63
Jagang 63:fd9af0693e50 64 logger.printf("---------\r\n");
Jagang 63:fd9af0693e50 65 logger.printf("%f %f %f\r\n",odometry.getX(),odometry.getY(),odometry.getTheta());
Jagang 63:fd9af0693e50 66 logger.printf("%f %f %f\r\n",odometry.getVitLeft(),odometry.getVitRight(),t.read());
Jagang 63:fd9af0693e50 67 t.reset();
Jagang 63:fd9af0693e50 68 wait(0.5);
Jagang 63:fd9af0693e50 69 motorL.setSpeed(1);
Jagang 65:b5d7f13870be 70 motorR.setSpeed(1);
Jagang 67:0bdf181586b5 71 }*/
Jagang 60:546d7b43333b 72
Near32 48:cb3ebbc27db3 73 bool testOdo = false;
Near32 36:54f86bc6fd80 74
Near32 36:54f86bc6fd80 75
Jagang 56:eb0600022463 76 //Asserv<float> instanceAsserv(&odometry);
Near32 3:573a0dc8383f 77
Near32 3:573a0dc8383f 78 /*----------------------------------------------------------------------------------------------*/
Jagang 62:454cd844fe1e 79 //instanceAsserv.setGoal( (float)0,(float)0, (float)PI/2);
Jagang 62:454cd844fe1e 80
Jagang 62:454cd844fe1e 81 aserv_planB asserv(odometry,motorL,motorR);
Jagang 62:454cd844fe1e 82
Jagang 62:454cd844fe1e 83 t.start();
Jagang 63:fd9af0693e50 84 t.reset();
Jagang 40:83ce8d1072ef 85
Near32 47:4909e97570f6 86 while(!testOdo)
Jagang 60:546d7b43333b 87 {
sype 68:1ef8b1a37252 88 //Parametrage des coef par bluetooth
sype 68:1ef8b1a37252 89 /*while(logger.readable()) {
sype 68:1ef8b1a37252 90 char c = logger.getc();
sype 68:1ef8b1a37252 91 if(c=='a') Kp += 0.001;
sype 68:1ef8b1a37252 92 else if(c=='z') Kp -= 0.001;
sype 69:6354497f9f59 93 logger.printf("%f",Kp);
sype 68:1ef8b1a37252 94 }*/
sype 68:1ef8b1a37252 95
Near32 36:54f86bc6fd80 96 //Asservissement :
Near32 41:c04c2ec37aad 97
Jagang 67:0bdf181586b5 98 odometry.update(t.read());
Jagang 62:454cd844fe1e 99 asserv.update(t.read());
Jagang 62:454cd844fe1e 100 t.reset();
Jagang 67:0bdf181586b5 101
Jagang 67:0bdf181586b5 102 //logger.printf("%f %f %f\r\n",odometry.getVitLeft(),odometry.getVitRight(),t.read());
Jagang 62:454cd844fe1e 103
Jagang 56:eb0600022463 104 //instanceAsserv.update((float)t.read());
Jagang 56:eb0600022463 105 //Mat<float> X( instanceAsserv.getX() );
Jagang 56:eb0600022463 106 //float phi_r = (float)instanceAsserv.getPhiR();
Jagang 56:eb0600022463 107 //float phi_l = (float)instanceAsserv.getPhiL();
Jagang 56:eb0600022463 108 //float phi_max = (float)instanceAsserv.getPhiMax();
Near32 44:d5f95af61243 109 //logger.printf(" x : %f ;\t y : %f ;\t theta : %f ;\t VD = %f ;\t W = %f \n\r",X.get(1,1),X.get(2,1),X.get(3,1), X.get(4,1), X.get(5,1) );
Near32 44:d5f95af61243 110 //logger.printf(" x : %f ;\t y : %f ;\t theta : %f ",X.get(1,1),X.get(2,1),X.get(3,1) );
Near32 45:b7617d7cedce 111
Near32 45:b7617d7cedce 112 //logger.printf("PhiR = %f ; \t PhiL = %f ; \n\r",phi_r,phi_l);//X.get(4,1),X.get(5,1));
Near32 45:b7617d7cedce 113
Near32 44:d5f95af61243 114 //transpose(X).afficherMblue();
Near32 44:d5f95af61243 115
Near32 42:fcb48e2fc426 116
Near32 36:54f86bc6fd80 117 //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);
Near32 36:54f86bc6fd80 118 //blue.printf("State : \n\r");
Near32 36:54f86bc6fd80 119 //(instanceAsserv.getX()).afficherMblue();
Near32 36:54f86bc6fd80 120 //blue.printf("Odometry : \n\r");
Near32 41:c04c2ec37aad 121 //z.afficherMblue();
Near32 42:fcb48e2fc426 122
Jagang 56:eb0600022463 123 //motorR.setSpeed(0.08+(phi_r/phi_max) );
Jagang 62:454cd844fe1e 124 //motorL.setSpeed(0.06+(phi_l/phi_max) );
Near32 26:54e1afed58b2 125 }
Jagang 60:546d7b43333b 126
Near32 26:54e1afed58b2 127
Jagang 56:eb0600022463 128 /*int i=0;
Near32 48:cb3ebbc27db3 129 int nbrech = 100;
Near32 48:cb3ebbc27db3 130 float tableR[nbrech], tableL[nbrech];
Near32 48:cb3ebbc27db3 131
Near32 48:cb3ebbc27db3 132 motorR.setSpeed(0.0);
Near32 48:cb3ebbc27db3 133 motorL.setSpeed(0.0);
Near32 48:cb3ebbc27db3 134 while(i<nbrech)
Near32 48:cb3ebbc27db3 135 {
Near32 48:cb3ebbc27db3 136 tableR[i] = odometry.getPhiright();
Near32 48:cb3ebbc27db3 137 tableL[i] = odometry.getPhileft();
Near32 48:cb3ebbc27db3 138 motorR.setSpeed( ((float)i)/100);
Near32 48:cb3ebbc27db3 139 motorL.setSpeed( ((float)i)/100);
Near32 48:cb3ebbc27db3 140 logger.printf("vitesse = %f \r\n", (float)i/100);
Near32 48:cb3ebbc27db3 141 wait(0.5);
Near32 48:cb3ebbc27db3 142 i++;
Near32 48:cb3ebbc27db3 143 }
Near32 48:cb3ebbc27db3 144
Near32 48:cb3ebbc27db3 145 i=0;
Near32 26:54e1afed58b2 146 while(1)
Near32 48:cb3ebbc27db3 147 {
Near32 48:cb3ebbc27db3 148 motorR.setSpeed(0.0);
Near32 48:cb3ebbc27db3 149 motorL.setSpeed(0.0);
Near32 48:cb3ebbc27db3 150
Near32 48:cb3ebbc27db3 151 if(i<nbrech)
Near32 48:cb3ebbc27db3 152 {
Near32 48:cb3ebbc27db3 153 logger.printf("%f , %f \r\n", tableL[i],tableR[i]);
Near32 48:cb3ebbc27db3 154 wait(1);
Near32 48:cb3ebbc27db3 155 i++;
Near32 48:cb3ebbc27db3 156 }
Jagang 56:eb0600022463 157 }*/
Near32 43:87bdce65341f 158 }