ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Map/Objectifs/Obj_clap.cpp@123:55e5e9acc541, 2015-05-11 (annotated)

Committer:
Jagang
Date:
Mon May 11 20:32:11 2015 +0000
Revision:
123:55e5e9acc541
Parent:
117:f8c147141a0c 
Maj AI; Repositionnement du d?part (1000,177); Ajout de l'action sur les claps

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 109:53918ba98306 1 #include "Obj_clap.h"
Jagang 109:53918ba98306 2
Jagang 123:55e5e9acc541 3 #ifdef PLAN_A
Jagang 123:55e5e9acc541 4 extern Asserv<float> asserv;
Jagang 123:55e5e9acc541 5 #else
Jagang 123:55e5e9acc541 6 extern aserv_planB asserv;
Jagang 123:55e5e9acc541 7 #endif
Jagang 117:f8c147141a0c 8
Jagang 123:55e5e9acc541 9 Obj_clap::Obj_clap(float x, float y, float theta, float x2, float y2, float theta2, AX12 *ax12_brasG, AX12 *ax12_brasD)
Jagang 117:f8c147141a0c 10 :Objectif(x,y,theta)
Jagang 109:53918ba98306 11 {
Jagang 109:53918ba98306 12 this->ax12_brasG = ax12_brasG;
Jagang 109:53918ba98306 13 this->ax12_brasD = ax12_brasD;
Jagang 123:55e5e9acc541 14 this->x2 = x2;
Jagang 123:55e5e9acc541 15 this->y2 = y2;
Jagang 123:55e5e9acc541 16 this->theta2 = theta2;
Jagang 109:53918ba98306 17 }
Jagang 109:53918ba98306 18
Jagang 109:53918ba98306 19 void Obj_clap::run()
Jagang 109:53918ba98306 20 {
Jagang 123:55e5e9acc541 21 if(theta2 == PI/2)
Jagang 123:55e5e9acc541 22 {
Jagang 123:55e5e9acc541 23 ax12_brasD->setMaxTorque(MAX_TORQUE);
Jagang 123:55e5e9acc541 24 ax12_brasD->setGoal(BRASD_OUVERT);
Jagang 123:55e5e9acc541 25 wait(1);
Jagang 123:55e5e9acc541 26 asserv.setGoal(x2,y2,theta2);
Jagang 123:55e5e9acc541 27 while(!asserv.isArrived())wait(0.1);
Jagang 123:55e5e9acc541 28 ax12_brasD->setGoal(BRASD_FERME);
Jagang 123:55e5e9acc541 29 wait(1);
Jagang 123:55e5e9acc541 30 ax12_brasD->setMaxTorque(0);
Jagang 123:55e5e9acc541 31 }
Jagang 123:55e5e9acc541 32 else
Jagang 123:55e5e9acc541 33 {
Jagang 123:55e5e9acc541 34 ax12_brasG->setMaxTorque(MAX_TORQUE);
Jagang 123:55e5e9acc541 35 ax12_brasG->setGoal(BRASG_OUVERT);
Jagang 123:55e5e9acc541 36 wait(1);
Jagang 123:55e5e9acc541 37 asserv.setGoal(x2,y2,theta2);
Jagang 123:55e5e9acc541 38 while(!asserv.isArrived())wait(0.1);
Jagang 123:55e5e9acc541 39 ax12_brasG->setGoal(BRASG_FERME);
Jagang 123:55e5e9acc541 40 wait(1);
Jagang 123:55e5e9acc541 41 ax12_brasG->setMaxTorque(0);
Jagang 123:55e5e9acc541 42 }
Jagang 109:53918ba98306 43
Jagang 123:55e5e9acc541 44 done = true;
Jagang 109:53918ba98306 45 }
Jagang 109:53918ba98306 46
Jagang 109:53918ba98306 47 int Obj_clap::isActive()
Jagang 109:53918ba98306 48 {
Jagang 109:53918ba98306 49 if(!active)
Jagang 109:53918ba98306 50 return false;
Jagang 109:53918ba98306 51
Jagang 109:53918ba98306 52 if(ax12_brasG->getGoal() == BRASG_OUVERT || ax12_brasD->getGoal() == BRASD_OUVERT)
Jagang 109:53918ba98306 53 return false;
Jagang 109:53918ba98306 54
Jagang 109:53918ba98306 55 return true;
Jagang 109:53918ba98306 56 }