ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@107:a6e498b5706c, 2015-05-05 (annotated)

Committer:
Near32
Date:
Tue May 05 12:08:20 2015 +0000
Revision:
107:a6e498b5706c
Parent:
106:05096985d1b2
Child:
111:c8a1129691da
coefficient pid position : coefficient pid vit --> attention, il y a un gros probl?me sur le gain du pid en vitesse s'il est trop grand....

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 0:41149573d577 1 #include "mbed.h"
Jagang 39:09c04fd42c94 2
Near32 107:a6e498b5706c 3 #define PLAN_A
Jagang 103:6a38cc0765f5 4 #define OUT_USB
Jagang 74:88be86f83d17 5
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"
Jagang 93:4d5664e9188a 21
Jagang 93:4d5664e9188a 22 void update();
Jagang 93:4d5664e9188a 23
Jagang 93:4d5664e9188a 24 // *--------------------------* //
Jagang 93:4d5664e9188a 25 // Déclarations //
Jagang 93:4d5664e9188a 26
Jagang 93:4d5664e9188a 27 // Decl. logger //
Jagang 93:4d5664e9188a 28
Jagang 93:4d5664e9188a 29 Serial logger(OUT_TX, OUT_RX); // tx, rx
Jagang 93:4d5664e9188a 30
Jagang 93:4d5664e9188a 31 // Decl. timer //
Jagang 93:4d5664e9188a 32
Jagang 93:4d5664e9188a 33 Timer t;
Jagang 93:4d5664e9188a 34 Ticker ticker;
Jagang 93:4d5664e9188a 35
Jagang 93:4d5664e9188a 36 // Decl. AX12 //
Jagang 93:4d5664e9188a 37
Jagang 93:4d5664e9188a 38 AX12 ax12_pince(AX12_TX,AX12_RX,5,250000);
Jagang 93:4d5664e9188a 39 AX12 ax12_brasG(AX12_TX,AX12_RX,2,250000);
Jagang 93:4d5664e9188a 40 AX12 ax12_brasD(AX12_TX,AX12_RX,3,250000);
Jagang 93:4d5664e9188a 41
Jagang 93:4d5664e9188a 42 // Decl. Moteurs //
Jagang 93:4d5664e9188a 43
Jagang 93:4d5664e9188a 44 PwmOut pw1(PWM_MOT1);
Jagang 93:4d5664e9188a 45 DigitalOut dir1(DIR_MOT1);
Jagang 93:4d5664e9188a 46 PwmOut pw2(PWM_MOT2);
Jagang 93:4d5664e9188a 47 DigitalOut dir2(DIR_MOT2);
Jagang 93:4d5664e9188a 48
Jagang 93:4d5664e9188a 49 Motor motorL(PWM_MOT1,DIR_MOT1);
Jagang 93:4d5664e9188a 50 Motor motorR(PWM_MOT2,DIR_MOT2);
Jagang 93:4d5664e9188a 51
Jagang 93:4d5664e9188a 52 // Decl. Sharps //
sype 80:5399183aa39b 53
Jagang 93:4d5664e9188a 54 AnalogIn sharp_devant(SHARP_D);
Jagang 93:4d5664e9188a 55 AnalogIn sharp_devant_droite(SHARP_DD);
Jagang 93:4d5664e9188a 56 AnalogIn sharp_devant_gauche(SHARP_DG);
Jagang 93:4d5664e9188a 57 AnalogIn sharp_arriere_gauche(SHARP_AG);
Jagang 93:4d5664e9188a 58 AnalogIn sharp_arriere_droite(SHARP_AD);
Jagang 93:4d5664e9188a 59
Jagang 93:4d5664e9188a 60 // Decl. Boutons //
Jagang 93:4d5664e9188a 61
Jagang 93:4d5664e9188a 62 DigitalIn bp(BP_DESSUS);
Jagang 93:4d5664e9188a 63 DigitalIn tirette(TIRETTE_DESSUS);
Jagang 93:4d5664e9188a 64 DigitalIn couleur(COULEUR_DESSUS);
Jagang 93:4d5664e9188a 65
Jagang 93:4d5664e9188a 66 // Decl. Odometrie //
Jagang 21:5443f93819db 67
Jagang 93:4d5664e9188a 68 #ifdef PLAN_A
Jagang 93:4d5664e9188a 69 QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 70 QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 71
Near32 107:a6e498b5706c 72 Odometry2 odometry(&qei_left,&qei_right,63.84/2.,63.65/2.,252);
Jagang 93:4d5664e9188a 73 #else
Jagang 93:4d5664e9188a 74 QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 75 QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 76
Jagang 103:6a38cc0765f5 77 Odometry2 odometry(&qei_left,&qei_right,63.84/2.,63.65/2.,252);
Jagang 93:4d5664e9188a 78 #endif
Jagang 93:4d5664e9188a 79
Jagang 93:4d5664e9188a 80 // Decl. Asserv //
Jagang 93:4d5664e9188a 81
Jagang 93:4d5664e9188a 82 #ifdef PLAN_A
Jagang 93:4d5664e9188a 83 Asserv<float> instanceAsserv(&odometry);
Jagang 93:4d5664e9188a 84 #else
Jagang 93:4d5664e9188a 85 aserv_planB asserv(odometry,motorL,motorR);
Jagang 93:4d5664e9188a 86 #endif
Jagang 93:4d5664e9188a 87
Jagang 93:4d5664e9188a 88 // Fin Decl. //
Jagang 93:4d5664e9188a 89 // *--------------------------* //
Jagang 93:4d5664e9188a 90
Jagang 23:228317fe0457 91
Jagang 0:41149573d577 92 int main()
Jagang 0:41149573d577 93 {
Jagang 93:4d5664e9188a 94 #ifdef OUT_USB
Jagang 93:4d5664e9188a 95 logger.baud((int)921600);
Jagang 93:4d5664e9188a 96 #endif
Jagang 74:88be86f83d17 97
Jagang 93:4d5664e9188a 98 // *--------------------------* //
Jagang 93:4d5664e9188a 99 // Init //
Near32 17:f360e21d3307 100
Jagang 93:4d5664e9188a 101 logger.printf("Initialisation.............");
Jagang 57:ab13f4e7a2b2 102
Jagang 93:4d5664e9188a 103 // Init. AX12 //
Jagang 39:09c04fd42c94 104
Jagang 93:4d5664e9188a 105 ax12_pince.setMode(0);
Jagang 93:4d5664e9188a 106 ax12_brasG.setMode(0);
Jagang 93:4d5664e9188a 107 ax12_brasD.setMode(0);
Jagang 54:e0e58c36658a 108
Jagang 93:4d5664e9188a 109 // Init. Moteurs //
Near32 24:3c0422e1ebd6 110
Jagang 93:4d5664e9188a 111 motorL.setSpeed(0);
Jagang 93:4d5664e9188a 112 motorR.setSpeed(0);
Jagang 93:4d5664e9188a 113
Jagang 93:4d5664e9188a 114 // Init. Sharps //
Jagang 93:4d5664e9188a 115
Jagang 93:4d5664e9188a 116 // Init. Boutons //
Jagang 93:4d5664e9188a 117
Jagang 93:4d5664e9188a 118 // Init. Odometrie //
Jagang 93:4d5664e9188a 119
Jagang 71:95d76c181b22 120 #ifdef PLAN_A
Jagang 78:7c7cefbe1772 121 odometry.setTheta(0);
Jagang 78:7c7cefbe1772 122 odometry.setX(0);
Jagang 78:7c7cefbe1772 123 odometry.setY(0);
Jagang 71:95d76c181b22 124 #else
Jagang 71:95d76c181b22 125 odometry.setTheta(PI/2);
Jagang 71:95d76c181b22 126 odometry.setX(0);
Jagang 71:95d76c181b22 127 odometry.setY(0);
Jagang 71:95d76c181b22 128 #endif
Jagang 60:546d7b43333b 129
Jagang 93:4d5664e9188a 130 logger.printf("[done]\r\n");
Jagang 93:4d5664e9188a 131
Jagang 93:4d5664e9188a 132 // Fin Init. //
Jagang 93:4d5664e9188a 133 // *--------------------------* //
Jagang 93:4d5664e9188a 134
Jagang 93:4d5664e9188a 135 // *--------------------------* //
Jagang 93:4d5664e9188a 136 // MIP //
Jagang 93:4d5664e9188a 137
Jagang 93:4d5664e9188a 138 logger.printf("Appuyer sur le bouton pour mettre ne position\r\n");
Jagang 93:4d5664e9188a 139
Jagang 93:4d5664e9188a 140 while(!bp); // On attend le top de mise en position
Jagang 93:4d5664e9188a 141
Jagang 93:4d5664e9188a 142 logger.printf("MIP........................");
Jagang 93:4d5664e9188a 143
Jagang 93:4d5664e9188a 144 ax12_pince.setMaxTorque(MAX_TORQUE);
Jagang 93:4d5664e9188a 145 ax12_brasG.setMaxTorque(MAX_TORQUE);
Jagang 93:4d5664e9188a 146 ax12_brasD.setMaxTorque(MAX_TORQUE);
Jagang 39:09c04fd42c94 147
Jagang 93:4d5664e9188a 148 ax12_pince.setGoal(PINCE_FERMEE);
Jagang 93:4d5664e9188a 149 ax12_brasG.setGoal(BRASG_OUVERT);
Jagang 93:4d5664e9188a 150 ax12_brasD.setGoal(BRASD_OUVERT);
Jagang 103:6a38cc0765f5 151 wait(0.5);
Jagang 93:4d5664e9188a 152 ax12_pince.setGoal(PINCE_OUVERTE);
Jagang 93:4d5664e9188a 153 ax12_brasG.setGoal(BRASG_FERME);
Jagang 93:4d5664e9188a 154 ax12_brasD.setGoal(BRASD_FERME);
Jagang 93:4d5664e9188a 155 wait(1.5);
Jagang 93:4d5664e9188a 156 ax12_pince.setMaxTorque(0);
Jagang 93:4d5664e9188a 157 ax12_brasG.setMaxTorque(0);
Jagang 93:4d5664e9188a 158 ax12_brasD.setMaxTorque(0);
Jagang 93:4d5664e9188a 159
Jagang 93:4d5664e9188a 160 logger.printf("[done]\r\n");
Jagang 93:4d5664e9188a 161
Jagang 93:4d5664e9188a 162 // //
Jagang 93:4d5664e9188a 163 // *--------------------------* //
Jagang 93:4d5664e9188a 164
Jagang 93:4d5664e9188a 165 // *--------------------------* //
Jagang 93:4d5664e9188a 166 // Asserv //
Jagang 93:4d5664e9188a 167
Jagang 93:4d5664e9188a 168 logger.printf("Demarrage asserv...........");
Jagang 93:4d5664e9188a 169 t.start();
Jagang 93:4d5664e9188a 170 ticker.attach_us(&update,10000); //100Hz
Jagang 93:4d5664e9188a 171 logger.printf("[done]\r\n");
Jagang 93:4d5664e9188a 172
Jagang 93:4d5664e9188a 173 // //
Jagang 93:4d5664e9188a 174 // *--------------------------* //
Jagang 93:4d5664e9188a 175
Jagang 93:4d5664e9188a 176 bool continuer = true;
Jagang 71:95d76c181b22 177
Jagang 71:95d76c181b22 178 #ifdef PLAN_A
Near32 107:a6e498b5706c 179 instanceAsserv.setGoal(0.0f,0.0f,(float)PI/4);
Near32 89:d05001d85a02 180 logger.printf("GOAL SET... RUNNING!\r\n");
Jagang 71:95d76c181b22 181
Jagang 74:88be86f83d17 182 char state = 0;
Jagang 74:88be86f83d17 183
Jagang 93:4d5664e9188a 184 while(continuer)
Jagang 71:95d76c181b22 185 {
Near32 107:a6e498b5706c 186 //#define test
Near32 107:a6e498b5706c 187 #ifndef test
Near32 107:a6e498b5706c 188 if(state == 0 && instanceAsserv.isArrived())
Near32 107:a6e498b5706c 189 {
Near32 107:a6e498b5706c 190 state = 1;
Near32 107:a6e498b5706c 191 logger.printf("Arrive en 0,0 !!!!!\r\n");
Near32 107:a6e498b5706c 192 motorR.setSpeed(0.0f);
Near32 107:a6e498b5706c 193 motorL.setSpeed(0.0f);
Near32 107:a6e498b5706c 194 wait(5);
Near32 107:a6e498b5706c 195 instanceAsserv.setGoal(0.0f,0.0f,0.0f);
Near32 107:a6e498b5706c 196 }
Near32 107:a6e498b5706c 197 else if(state == 1 && instanceAsserv.isArrived())
Near32 107:a6e498b5706c 198 {
Near32 107:a6e498b5706c 199 state = 0;
Near32 107:a6e498b5706c 200 logger.printf("Arrive en 0,200 !!!!!\r\n");
Near32 107:a6e498b5706c 201 motorR.setSpeed(0.0f);
Near32 107:a6e498b5706c 202 motorL.setSpeed(0.0f);
Near32 107:a6e498b5706c 203 wait(5);
Near32 107:a6e498b5706c 204 instanceAsserv.setGoal(0.0f,0.0f,0.0f);
Near32 107:a6e498b5706c 205 }
Near32 107:a6e498b5706c 206 #endif
Jagang 92:b403f2724252 207 #ifdef test
Jagang 92:b403f2724252 208 if(state == 0 && instanceAsserv.isArrivedRho())
Jagang 92:b403f2724252 209 {
Jagang 92:b403f2724252 210 state = 1;
Jagang 92:b403f2724252 211 logger.printf("Arrive en 0,0 !!!!!\r\n");
Near32 98:2ec4e17dfc92 212 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 213 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 214 wait(5);
Jagang 92:b403f2724252 215 instanceAsserv.setGoal(300.0f,200.0f,0.0f);
Jagang 92:b403f2724252 216 }
Jagang 92:b403f2724252 217 else if(state == 1 && instanceAsserv.isArrivedRho())
Jagang 92:b403f2724252 218 {
Jagang 92:b403f2724252 219 state = 2;
Jagang 92:b403f2724252 220 logger.printf("Arrive en 200,200 !!!!!\r\n");
Near32 98:2ec4e17dfc92 221 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 222 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 223 wait(5);
Jagang 92:b403f2724252 224 instanceAsserv.setGoal(0.0f,300.0f,0.0f);
Jagang 92:b403f2724252 225 }
Jagang 92:b403f2724252 226 else if(state == 2 && instanceAsserv.isArrivedRho())
Jagang 92:b403f2724252 227 {
Jagang 92:b403f2724252 228 state = 0;
Jagang 92:b403f2724252 229 logger.printf("Arrive en 0,200 !!!!!\r\n");
Near32 98:2ec4e17dfc92 230 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 231 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 232 wait(5);
Jagang 92:b403f2724252 233 instanceAsserv.setGoal(0.0f,0.0f,0.0f);
Jagang 92:b403f2724252 234 }
Jagang 92:b403f2724252 235 #endif
Jagang 71:95d76c181b22 236 }
Jagang 71:95d76c181b22 237 #else
sype 106:05096985d1b2 238 asserv.setGoal(1,500,0);
sype 106:05096985d1b2 239 /*wait(2);
sype 106:05096985d1b2 240 asserv.setGoal(-450,1,0);
sype 106:05096985d1b2 241 wait(2);
sype 106:05096985d1b2 242 asserv.setGoal(-450,-450,0);
sype 106:05096985d1b2 243 wait(2);
sype 106:05096985d1b2 244 asserv.setGoal(1,-450,0);
sype 106:05096985d1b2 245 wait(2);
sype 106:05096985d1b2 246 asserv.setGoal(450,-450,0);
sype 106:05096985d1b2 247 wait(2);*/
Jagang 71:95d76c181b22 248 #endif
Jagang 103:6a38cc0765f5 249
Jagang 103:6a38cc0765f5 250 while(1);
Near32 43:87bdce65341f 251 }
Jagang 93:4d5664e9188a 252
Jagang 93:4d5664e9188a 253 void update()
Jagang 93:4d5664e9188a 254 {
Jagang 93:4d5664e9188a 255 float dt = t.read();
Jagang 93:4d5664e9188a 256 t.reset();
Jagang 93:4d5664e9188a 257
Jagang 93:4d5664e9188a 258 odometry.update(dt);
Jagang 93:4d5664e9188a 259
Jagang 93:4d5664e9188a 260 #ifdef PLAN_A
Jagang 93:4d5664e9188a 261 instanceAsserv.update(dt);
Jagang 93:4d5664e9188a 262 float phi_r = (float)instanceAsserv.getPhiR();
Jagang 93:4d5664e9188a 263 float phi_l = (float)instanceAsserv.getPhiL();
Jagang 93:4d5664e9188a 264 float phi_max = (float)instanceAsserv.getPhiMax();
Jagang 93:4d5664e9188a 265
Jagang 93:4d5664e9188a 266 motorR.setSpeed(0.08+(phi_r/phi_max));
Jagang 93:4d5664e9188a 267 motorL.setSpeed(0.08+(phi_l/phi_max));
Jagang 93:4d5664e9188a 268 #else
Jagang 93:4d5664e9188a 269 asserv.update(dt);
Jagang 93:4d5664e9188a 270 #endif
Jagang 93:4d5664e9188a 271 }
Jagang 93:4d5664e9188a 272