ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@72:b2a128486332, 2015-04-10 (annotated)

Committer:
Jagang
Date:
Fri Apr 10 18:40:58 2015 +0000
Revision:
72:b2a128486332
Parent:
71:95d76c181b22
Child:
73:d8e1b543fbe3
calcul d'erreur de theta asserv; Gros probl?me d'instabilit? en vitesse (consignes nulles)

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