ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@57:ab13f4e7a2b2, 2015-04-09 (annotated)

Committer:
Jagang
Date:
Thu Apr 09 16:46:48 2015 +0000
Revision:
57:ab13f4e7a2b2
Parent:
56:eb0600022463
Child:
60:546d7b43333b
Vitesse odo

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