ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@89:d05001d85a02, 2015-04-23 (annotated)

Committer:
Near32
Date:
Thu Apr 23 16:29:31 2015 +0000
Revision:
89:d05001d85a02
Parent:
88:77d6ced9977e
Child:
90:294b16267109
simplifying; testing pid pi pd ....

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 0:41149573d577 1 #include "mbed.h"
Jagang 39:09c04fd42c94 2
Near32 87:e8b64b4174b8 3 #define PLAN_A
Jagang 74:88be86f83d17 4
Jagang 39:09c04fd42c94 5 #include "defines.h"
Jagang 39:09c04fd42c94 6
Jagang 0:41149573d577 7 #include "QEI.h"
Jagang 23:228317fe0457 8 #include "Map.h"
Jagang 39:09c04fd42c94 9 #include "AX12.h"
Jagang 0:41149573d577 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*/
sype 80:5399183aa39b 22
Near32 48:cb3ebbc27db3 23 Serial logger(OUT_TX, OUT_RX); // tx, rx
sype 80:5399183aa39b 24 //Serial logger(USBTX,USBRX);
Near32 48:cb3ebbc27db3 25 //logger.baud((int)115200);
Near32 17:f360e21d3307 26 /*----------------------------------------------------------------------------------------------*/
Jagang 21:5443f93819db 27
Jagang 23:228317fe0457 28 /* --- Initialisation de la liste des obstable --- */
Jagang 23:228317fe0457 29 int Obstacle::lastId = 0;
Jagang 23:228317fe0457 30
Jagang 0:41149573d577 31 int main()
Jagang 0:41149573d577 32 {
Jagang 39:09c04fd42c94 33 logger.printf("Initialisation...\r\n");
Near32 36:54f86bc6fd80 34
Jagang 78:7c7cefbe1772 35 AX12 test(AX12_TX,AX12_RX,5,250000);
Jagang 74:88be86f83d17 36 test.setMode(0);
Jagang 74:88be86f83d17 37
Near32 89:d05001d85a02 38 #ifdef PLAN_B
Jagang 74:88be86f83d17 39 while(1)
Jagang 74:88be86f83d17 40 {
Jagang 74:88be86f83d17 41 test.setMaxTorque(0x1FF);
Jagang 74:88be86f83d17 42 test.setGoal(0);
Jagang 74:88be86f83d17 43 logger.printf("0\r\n");
Jagang 74:88be86f83d17 44 wait(2);
Jagang 74:88be86f83d17 45 test.setMaxTorque(0x1FF);
Jagang 74:88be86f83d17 46 test.setGoal(90);
Jagang 74:88be86f83d17 47 logger.printf("180\r\n");
Jagang 74:88be86f83d17 48 wait(4);
Jagang 78:7c7cefbe1772 49 }
Near32 89:d05001d85a02 50 #endif
Jagang 74:88be86f83d17 51
Jagang 40:83ce8d1072ef 52 PwmOut pw1(PWM_MOT1);
Jagang 40:83ce8d1072ef 53 DigitalOut dir1(DIR_MOT1);
Jagang 40:83ce8d1072ef 54 PwmOut pw2(PWM_MOT2);
Jagang 40:83ce8d1072ef 55 DigitalOut dir2(DIR_MOT2);
Near32 17:f360e21d3307 56
Near32 36:54f86bc6fd80 57
Near32 48:cb3ebbc27db3 58 Motor motorL(PWM_MOT1,DIR_MOT1);
Near32 48:cb3ebbc27db3 59 Motor motorR(PWM_MOT2,DIR_MOT2);
Jagang 57:ab13f4e7a2b2 60
Jagang 39:09c04fd42c94 61 Timer t;
Jagang 39:09c04fd42c94 62
Jagang 63:fd9af0693e50 63 //AX12 test(PA_9,NC,0x03,250000);
Jagang 54:e0e58c36658a 64
Jagang 39:09c04fd42c94 65 AnalogIn ain0(PA_0);
Jagang 39:09c04fd42c94 66 AnalogIn ain1(PA_1);
Jagang 39:09c04fd42c94 67 AnalogIn ain2(PA_4);
Jagang 39:09c04fd42c94 68 AnalogIn ain3(PB_0);
Jagang 39:09c04fd42c94 69 AnalogIn ain4(PC_1);
Near32 24:3c0422e1ebd6 70
Near32 3:573a0dc8383f 71 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 72 /*Odometry*/
Jagang 71:95d76c181b22 73 #ifdef PLAN_A
Jagang 71:95d76c181b22 74 QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
Jagang 71:95d76c181b22 75 QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
Jagang 71:95d76c181b22 76
sype 79:d97090bb6470 77 Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,255);
Jagang 78:7c7cefbe1772 78 odometry.setTheta(0);
Jagang 78:7c7cefbe1772 79 odometry.setX(0);
Jagang 78:7c7cefbe1772 80 odometry.setY(0);
Jagang 71:95d76c181b22 81 #else
Jagang 71:95d76c181b22 82 QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
Jagang 71:95d76c181b22 83 QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
Jagang 71:95d76c181b22 84
sype 79:d97090bb6470 85 Odometry2 odometry(&qei_left,&qei_right,63/2.,63/2.,255);
Jagang 71:95d76c181b22 86
Jagang 71:95d76c181b22 87 odometry.setTheta(PI/2);
Jagang 71:95d76c181b22 88 odometry.setX(0);
Jagang 71:95d76c181b22 89 odometry.setY(0);
Jagang 71:95d76c181b22 90 #endif
Jagang 60:546d7b43333b 91
Jagang 63:fd9af0693e50 92 DigitalOut led1(LED1);
Jagang 63:fd9af0693e50 93 DigitalOut led2(LED2);
Jagang 63:fd9af0693e50 94 DigitalOut led3(LED3);
Jagang 39:09c04fd42c94 95
Jagang 71:95d76c181b22 96 bool testOdo = false;
Jagang 71:95d76c181b22 97
Jagang 71:95d76c181b22 98 #ifdef PLAN_A
Jagang 71:95d76c181b22 99 Asserv<float> instanceAsserv(&odometry);
Near32 89:d05001d85a02 100 instanceAsserv.setGoal((float)100.0,(float)-150.0,(float)PI/4);
Near32 89:d05001d85a02 101 logger.printf("GOAL SET... RUNNING!\r\n");
Jagang 71:95d76c181b22 102
Jagang 74:88be86f83d17 103 char state = 0;
Jagang 74:88be86f83d17 104
Jagang 71:95d76c181b22 105 t.start();
Jagang 63:fd9af0693e50 106 t.reset();
Jagang 63:fd9af0693e50 107
Jagang 71:95d76c181b22 108 while(!testOdo)
Jagang 71:95d76c181b22 109 {
Jagang 71:95d76c181b22 110 float dt = t.read();
Jagang 71:95d76c181b22 111 t.reset();
Jagang 74:88be86f83d17 112
Jagang 71:95d76c181b22 113 odometry.update(dt);
Jagang 74:88be86f83d17 114 instanceAsserv.update(dt);
Near32 73:d8e1b543fbe3 115 float phi_r = (float)instanceAsserv.getPhiR();
Near32 73:d8e1b543fbe3 116 float phi_l = (float)instanceAsserv.getPhiL();
Jagang 74:88be86f83d17 117 float phi_max = (float)instanceAsserv.getPhiMax();
Jagang 71:95d76c181b22 118
Near32 89:d05001d85a02 119 //logger.printf("phi r = %3.f ; phi_l = %3.f \r\n", phi_r,phi_l);
Near32 89:d05001d85a02 120
Near32 89:d05001d85a02 121 motorR.setSpeed(/*0.08*/+(phi_r/phi_max));
Near32 89:d05001d85a02 122 motorL.setSpeed(/*0.08*/+(phi_l/phi_max));
Near32 87:e8b64b4174b8 123 //#define test
Near32 81:08eacb6179d8 124 #ifdef test
Jagang 76:a0c09fd62be1 125 if(state == 0 && instanceAsserv.isArrivedRho())
Jagang 76:a0c09fd62be1 126 {
Jagang 76:a0c09fd62be1 127 state = 1;
Jagang 76:a0c09fd62be1 128 logger.printf("Arrive en 0,0 !!!!!");
Jagang 76:a0c09fd62be1 129 wait(5);
Jagang 76:a0c09fd62be1 130 instanceAsserv.setGoal(200.0,200.0,0);
Jagang 76:a0c09fd62be1 131 }
Jagang 76:a0c09fd62be1 132 else if(state == 1 && instanceAsserv.isArrivedRho())
Jagang 76:a0c09fd62be1 133 {
Jagang 76:a0c09fd62be1 134 state = 2;
Jagang 76:a0c09fd62be1 135 logger.printf("Arrive en 200,200 !!!!!");
Jagang 76:a0c09fd62be1 136 wait(5);
Jagang 76:a0c09fd62be1 137 instanceAsserv.setGoal(0.0,200.0,0);
Jagang 76:a0c09fd62be1 138 }
Jagang 76:a0c09fd62be1 139 else if(state == 2 && instanceAsserv.isArrivedRho())
Jagang 76:a0c09fd62be1 140 {
Jagang 76:a0c09fd62be1 141 state = 0;
Jagang 76:a0c09fd62be1 142 logger.printf("Arrive en 0,200 !!!!!");
Jagang 76:a0c09fd62be1 143 wait(5);
Jagang 76:a0c09fd62be1 144 instanceAsserv.setGoal(0.0,0.0,0);
Jagang 76:a0c09fd62be1 145 }
Near32 81:08eacb6179d8 146 #endif
Jagang 74:88be86f83d17 147
Jagang 71:95d76c181b22 148 }
Jagang 71:95d76c181b22 149 #else
Jagang 71:95d76c181b22 150 aserv_planB asserv(odometry,motorL,motorR);
sype 79:d97090bb6470 151 asserv.setGoal(1,-5000,0);
Jagang 71:95d76c181b22 152
Jagang 71:95d76c181b22 153 t.start();
Jagang 63:fd9af0693e50 154 t.reset();
Jagang 71:95d76c181b22 155
Jagang 71:95d76c181b22 156 while(!testOdo)
Jagang 71:95d76c181b22 157 {
Jagang 71:95d76c181b22 158 //Parametrage des coef par bluetooth
Jagang 71:95d76c181b22 159 while(logger.readable()) {
Jagang 71:95d76c181b22 160 char c = logger.getc();
sype 79:d97090bb6470 161 if(c=='a') asserv.Kd += 0.001;
sype 79:d97090bb6470 162 else if(c=='z') asserv.Kd -= 0.001;
sype 79:d97090bb6470 163 logger.printf("%f\n\r",asserv.Kd);
Jagang 71:95d76c181b22 164 }
Jagang 71:95d76c181b22 165
Jagang 71:95d76c181b22 166 //Asservissement :
Jagang 71:95d76c181b22 167 float dt = t.read();
Jagang 71:95d76c181b22 168 t.reset();
Jagang 71:95d76c181b22 169 odometry.update(dt);
Jagang 71:95d76c181b22 170 asserv.update(dt);
Jagang 71:95d76c181b22 171
sype 79:d97090bb6470 172 wait_ms(100);
Jagang 71:95d76c181b22 173 }
Jagang 71:95d76c181b22 174 #endif
Near32 43:87bdce65341f 175 }