ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@39:09c04fd42c94, 2015-03-16 (annotated)

Committer:
Jagang
Date:
Mon Mar 16 21:46:12 2015 +0000
Revision:
39:09c04fd42c94
Parent:
38:28f476eacde4
Child:
40:83ce8d1072ef
Update to AX12 from ProtocarteV1

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