ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@44:d5f95af61243, 2015-03-20 (annotated)

Committer:
Near32
Date:
Fri Mar 20 12:12:07 2015 +0000
Revision:
44:d5f95af61243
Parent:
43:87bdce65341f
Child:
45:b7617d7cedce
pid acc pid vit

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