ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@74:88be86f83d17, 2015-04-13 (annotated)

Committer:
Jagang
Date:
Mon Apr 13 16:53:19 2015 +0000
Revision:
74:88be86f83d17
Parent:
73:d8e1b543fbe3
Child:
76:a0c09fd62be1
bla

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 0:41149573d577 1 #include "mbed.h"
Jagang 39:09c04fd42c94 2
Jagang 74:88be86f83d17 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
Near32 42:fcb48e2fc426 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*/
Near32 48:cb3ebbc27db3 23 Serial logger(OUT_TX, OUT_RX); // tx, rx
Near32 17:f360e21d3307 24 /*----------------------------------------------------------------------------------------------*/
Jagang 21:5443f93819db 25
Jagang 23:228317fe0457 26 /* --- Initialisation de la liste des obstable --- */
Jagang 23:228317fe0457 27 int Obstacle::lastId = 0;
Jagang 23:228317fe0457 28
Jagang 0:41149573d577 29 int main()
Jagang 0:41149573d577 30 {
Jagang 39:09c04fd42c94 31 logger.printf("Initialisation...\r\n");
Near32 36:54f86bc6fd80 32
Jagang 74:88be86f83d17 33 /*AX12 test(PA_9,NC,5,250000);
Jagang 74:88be86f83d17 34 test.setMode(0);
Jagang 74:88be86f83d17 35
Jagang 74:88be86f83d17 36 while(1)
Jagang 74:88be86f83d17 37 {
Jagang 74:88be86f83d17 38 test.setMaxTorque(0x1FF);
Jagang 74:88be86f83d17 39 test.setGoal(0);
Jagang 74:88be86f83d17 40 logger.printf("0\r\n");
Jagang 74:88be86f83d17 41 wait(2);
Jagang 74:88be86f83d17 42 test.setMaxTorque(0x1FF);
Jagang 74:88be86f83d17 43 test.setGoal(90);
Jagang 74:88be86f83d17 44 logger.printf("180\r\n");
Jagang 74:88be86f83d17 45 wait(4);
Jagang 74:88be86f83d17 46 }*/
Jagang 74:88be86f83d17 47
Jagang 40:83ce8d1072ef 48 PwmOut pw1(PWM_MOT1);
Jagang 40:83ce8d1072ef 49 DigitalOut dir1(DIR_MOT1);
Jagang 40:83ce8d1072ef 50 PwmOut pw2(PWM_MOT2);
Jagang 40:83ce8d1072ef 51 DigitalOut dir2(DIR_MOT2);
Near32 17:f360e21d3307 52
Near32 36:54f86bc6fd80 53
Near32 48:cb3ebbc27db3 54 Motor motorL(PWM_MOT1,DIR_MOT1);
Near32 48:cb3ebbc27db3 55 Motor motorR(PWM_MOT2,DIR_MOT2);
Jagang 57:ab13f4e7a2b2 56
Jagang 39:09c04fd42c94 57 Timer t;
Jagang 39:09c04fd42c94 58
Jagang 63:fd9af0693e50 59 //AX12 test(PA_9,NC,0x03,250000);
Jagang 54:e0e58c36658a 60
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 71:95d76c181b22 69 #ifdef PLAN_A
Jagang 71:95d76c181b22 70 QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
Jagang 71:95d76c181b22 71 QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
Jagang 71:95d76c181b22 72
Jagang 71:95d76c181b22 73 Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,280);
Jagang 71:95d76c181b22 74 #else
Jagang 71:95d76c181b22 75 QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
Jagang 71:95d76c181b22 76 QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
Jagang 71:95d76c181b22 77
Jagang 71:95d76c181b22 78 Odometry2 odometry(&qei_left,&qei_right,63/2.,63/2.,280);
Jagang 71:95d76c181b22 79
Jagang 71:95d76c181b22 80 odometry.setTheta(PI/2);
Jagang 71:95d76c181b22 81 odometry.setX(0);
Jagang 71:95d76c181b22 82 odometry.setY(0);
Jagang 71:95d76c181b22 83 #endif
Jagang 60:546d7b43333b 84
Jagang 63:fd9af0693e50 85 DigitalOut led1(LED1);
Jagang 63:fd9af0693e50 86 DigitalOut led2(LED2);
Jagang 63:fd9af0693e50 87 DigitalOut led3(LED3);
Jagang 39:09c04fd42c94 88
Jagang 71:95d76c181b22 89 bool testOdo = false;
Jagang 71:95d76c181b22 90
Jagang 71:95d76c181b22 91 #ifdef PLAN_A
Jagang 71:95d76c181b22 92 Asserv<float> instanceAsserv(&odometry);
Near32 73:d8e1b543fbe3 93 instanceAsserv.setGoal( (float)200,(float)0, (float)0);
Jagang 71:95d76c181b22 94
Jagang 74:88be86f83d17 95 char state = 0;
Jagang 74:88be86f83d17 96
Jagang 71:95d76c181b22 97 t.start();
Jagang 63:fd9af0693e50 98 t.reset();
Jagang 63:fd9af0693e50 99
Jagang 71:95d76c181b22 100 while(!testOdo)
Jagang 71:95d76c181b22 101 {
Jagang 71:95d76c181b22 102 float dt = t.read();
Jagang 71:95d76c181b22 103 t.reset();
Jagang 74:88be86f83d17 104
Jagang 71:95d76c181b22 105 odometry.update(dt);
Jagang 74:88be86f83d17 106 instanceAsserv.update(dt);
Near32 73:d8e1b543fbe3 107 float phi_r = (float)instanceAsserv.getPhiR();
Near32 73:d8e1b543fbe3 108 float phi_l = (float)instanceAsserv.getPhiL();
Jagang 74:88be86f83d17 109 float phi_max = (float)instanceAsserv.getPhiMax();
Jagang 71:95d76c181b22 110
Jagang 74:88be86f83d17 111 motorR.setSpeed(0.08+(phi_r/phi_max));
Jagang 74:88be86f83d17 112 motorL.setSpeed(0.06+(phi_l/phi_max));
Jagang 71:95d76c181b22 113
Jagang 74:88be86f83d17 114 if(state == 0 && instanceAsserv.
Jagang 74:88be86f83d17 115
Jagang 71:95d76c181b22 116 }
Jagang 71:95d76c181b22 117 #else
Jagang 71:95d76c181b22 118 aserv_planB asserv(odometry,motorL,motorR);
Jagang 72:b2a128486332 119 asserv.setGoal(-5000,-5000,0);
Jagang 71:95d76c181b22 120
Jagang 71:95d76c181b22 121 t.start();
Jagang 63:fd9af0693e50 122 t.reset();
Jagang 71:95d76c181b22 123
Jagang 71:95d76c181b22 124 while(!testOdo)
Jagang 71:95d76c181b22 125 {
Jagang 71:95d76c181b22 126 //Parametrage des coef par bluetooth
Jagang 71:95d76c181b22 127 while(logger.readable()) {
Jagang 71:95d76c181b22 128 char c = logger.getc();
Jagang 71:95d76c181b22 129 if(c=='a') asserv.Kp += 0.001;
Jagang 71:95d76c181b22 130 else if(c=='z') asserv.Kp -= 0.001;
Jagang 71:95d76c181b22 131 //logger.printf("%f\n\r",asserv.Kp);
Jagang 71:95d76c181b22 132 }
Jagang 71:95d76c181b22 133
Jagang 71:95d76c181b22 134 //Asservissement :
Jagang 71:95d76c181b22 135 float dt = t.read();
Jagang 71:95d76c181b22 136 t.reset();
Jagang 71:95d76c181b22 137 odometry.update(dt);
Jagang 71:95d76c181b22 138 asserv.update(dt);
Jagang 71:95d76c181b22 139
Jagang 71:95d76c181b22 140 wait(0.1);
Jagang 71:95d76c181b22 141 }
Jagang 71:95d76c181b22 142 #endif
Near32 43:87bdce65341f 143 }