ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@92:b403f2724252, 2015-04-29 (annotated)

Committer:
Jagang
Date:
Wed Apr 29 16:13:53 2015 +0000
Revision:
92:b403f2724252
Parent:
91:c0b436c52ede
Parent:
85:8e95432d99d3
Child:
93:4d5664e9188a
Child:
95:33520ead3b94
Merge

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