ARES
Time is good
Fork of
Robot2016_2-0
by 
ARES
Revision 59:94b44c4a8a5a, committed 2016-05-04
- Comitter:
- Jagang
- Date:
- Wed May 04 19:58:56 2016 +0000
- Parent:
- 58:02dc8328975a
- Parent:
- 57:86f491f5b25d
- Child:
- 61:4a414820870f
- Commit message:
- Merge
Changed in this revision
| Functions/defines.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/Functions/defines.h Wed May 04 19:55:03 2016 +0000 +++ b/Functions/defines.h Wed May 04 19:58:56 2016 +0000 @@ -1,6 +1,9 @@ #ifndef DEFINES_H #define DEFINES_H +/* Dernier Changement : Romain 19h23 +Inclu dans main.cpp Map/map.h func.h */ + #include "mbed.h" #include "../RoboClaw/RoboClaw.h" #include "../Odometry/Odometry.h"
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectif/objectif.cpp Wed May 04 19:58:56 2016 +0000
@@ -0,0 +1,14 @@
+#include "objectif.h"
+
+/* Dernier Changement : Romain 20h20 */
+
+Objectif::Objectif (int type, float x_obj, float y_obj, float thet_obj, AX12 * nA1, AX12 * nA2, AX12 * nA3, Stepper * nS1, Stepper * nS2);
+bool Objectif::Action () {
+ switch(type) {
+ case OBJ_BLOC:
+ break;
+ default:
+ break;
+ }
+ return true;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectif/objectif.h Wed May 04 19:58:56 2016 +0000
@@ -0,0 +1,27 @@
+#ifndef OBJECTIF_H
+#define OBJECTIF_H
+
+/* Dernier Changement : Romain 20h20
+Inclu dans : map.h */
+
+#include "../../AX12/AX12.h"
+#include "../../StepperMotor/Stepper.h"
+
+class objectif {
+public:
+ objectif (int type, float x_obj, float y_obj, float thet_obj, AX12 * nA1, AX12 * nA2, AX12 * nA3, Stepper * nS1, Stepper * nS2);
+ bool Action ();
+ float getX() { return x_objectif; }
+ float getY() { return y_objectif; }
+ float getThet() { return thet_object; }
+
+private:
+ bool obj_bloc_action();
+
+ int type;
+ float x_objectif, y_objectif, thet_object;
+ AX12 * A1, * A2, * A3;
+ Stepper * S1, * S2;
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Map/Objectif/objectif_type.h Wed May 04 19:58:56 2016 +0000 @@ -0,0 +1,6 @@ +#ifndef OBJECTIF_TYPE_H +#define OBJECTIF_TYPE_H + +#define OBJ_BLOC 1 + +#endif \ No newline at end of file
--- a/Map/map.cpp Wed May 04 19:55:03 2016 +0000
+++ b/Map/map.cpp Wed May 04 19:58:56 2016 +0000
@@ -1,16 +1,25 @@
#include "map.h"
+/* Dernier Changement : Romain 20h30 */
+
map::map (Odometry* nodo) : Codo(nodo) {
}
void map::Build (int couleur, int formation) {
+ if (couleur == VERT) {
+ max_x_table = 2800;
+ max_y_table = 1800;
+ min_x_table = 0;
+ min_y_table = 0;
+ }
+
addObs(obsCarr (800, 100, 100, 15));
addObs(obsCarr (2200, 100, 100, 15));
addObs(obsCarr (1500, 750, 1100, 15));
addObs(obsCarr (1500, 1050, 20, 300));
- if (formation == MAP_RIGHTSIDE) {
+ if (formation == 1) {
addObs(obsCarr (0, 2000, 250, 150)); // Coté haut droite
addObs(obsCarr (200, 2000, 200, 50));
@@ -83,17 +92,20 @@
ended = false;
tmp = obs[i].getPoints ();
+ // On vérifie si le point est sur la table
+ if (tmp.p0.getX() < min_x_table || tmp.p0.getY() < min_y_table || tmp.p0.getX() > max_x_table || tmp.p0.getY() > max_y_table)
+ val[0] = false;
// On vérifie si le point croise un obstacle
- for (j = 0; j < os; ++j)
+ for (j = 0; j < os && val[0]; ++j)
if (obs[j].getCroisement (tmp.p0, close[indTMP1]))
val[0] = false;
// On vérifie si le point existe déjà dans la liste ouverte
- for (j = 0; j < open.size (); ++j) {
+ for (j = 0; j < open.size () && val[0]; ++j) {
if (open[j] == tmp.p0)
val[0] = false;
}
// On vérifie si le point existe déjà dans la liste fermée
- for (j = 0; j < close.size (); ++j) {
+ for (j = 0; j < close.size () && val[0]; ++j) {
if (close[j] == tmp.p0)
val[0] = false;
}
@@ -102,14 +114,16 @@
}
// On repete l'operation pour le second point
- for (j = 0; j < os; ++j)
+ if (tmp.p1.getX() < min_x_table || tmp.p1.getY() < min_y_table || tmp.p1.getX() > max_x_table || tmp.p1.getY() > max_y_table)
+ val[1] = false;
+ for (j = 0; j < os && val[1]; ++j)
if (obs[j].getCroisement (tmp.p1, close[indTMP1]))
val[1] = false;
- for (j = 0; j < open.size (); ++j) {
+ for (j = 0; j < open.size () && val[1]; ++j) {
if (open[j] == tmp.p1)
val[1] = false;
}
- for (j = 0; j < close.size (); ++j) {
+ for (j = 0; j < close.size () && val[1]; ++j) {
if (close[j] == tmp.p1)
val[1] = false;
}
@@ -118,14 +132,16 @@
}
// On répète l'opération pour le troisième point
- for (j = 0; j < os; ++j)
+ if (tmp.p2.getX() < min_x_table || tmp.p2.getY() < min_y_table || tmp.p2.getX() > max_x_table || tmp.p2.getY() > max_y_table)
+ val[2] = false;
+ for (j = 0; j < os && val[2]; ++j)
if (obs[j].getCroisement (tmp.p2, close[indTMP1]))
val[2] = false;
- for (j = 0; j < open.size (); ++j) {
+ for (j = 0; j < open.size () && val[2]; ++j) {
if (open[j] == tmp.p2)
val[2] = false;
}
- for (j = 0; j < close.size (); ++j) {
+ for (j = 0; j < close.size () && val[2]; ++j) {
if (close[j] == tmp.p2)
val[2] = false;
}
@@ -134,14 +150,16 @@
}
// On répète l'opération pour le quatrieme point
- for (j = 0; j < os; ++j)
+ if (tmp.p3.getX() < min_x_table || tmp.p3.getY() < min_y_table || tmp.p3.getX() > max_x_table || tmp.p3.getY() > max_y_table)
+ val[3] = false;
+ for (j = 0; j < os && val[3]; ++j)
if (obs[j].getCroisement (tmp.p3, close[indTMP1]))
val[3] = false;
- for (j = 0; j < open.size (); ++j) {
+ for (j = 0; j < open.size () && val[3]; ++j) {
if (open[j] == tmp.p3)
val[3] = false;
}
- for (j = 0; j < close.size (); ++j) {
+ for (j = 0; j < close.size () && val[3]; ++j) {
if (close[j] == tmp.p3)
val[3] = false;
}
@@ -192,4 +210,26 @@
logger.printf("Chemin pas trouve ...\n\r");
}
endedParc = false;
+}
+
+void map::Execute(float XObjectif, float YObjectif) {
+ // logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
+ Objectif o;
+ FindWay (Codo->getX(), Codo->getY(), o.getX(), o.getY());
+
+ if (endedParc) {
+ //logger.printf("\n\r");
+ for (int i = 0; i < path.size (); i++) {
+ // logger.printf("Goto %d/%d [%f, %f]... \n\r", i, path.size()-1, path[i].getX(), path[i].getY());
+ //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX()));
+ Codo->GotoXY((double)path[i].getX(), (double)path[i].getY());
+ // logger.printf("Goto Fini\n\r");
+ }
+
+ //logger.printf("Chemin fini !\n\r");
+ }
+ else {
+ logger.printf("Chemin pas trouve ...\n\r");
+ }
+ endedParc = false;
}
\ No newline at end of file
--- a/Map/map.h Wed May 04 19:55:03 2016 +0000
+++ b/Map/map.h Wed May 04 19:58:56 2016 +0000
@@ -1,21 +1,29 @@
#ifndef MAP_H
#define MAP_H
+/* Dernier Changement : Romain 20h30
+Inclu dans : main.cpp
+Il faut encore gerer les formations de coquillage */
+
#include "obsCarr.h"
#include "pointParcours.h"
#include "nVector.h"
#include "controle.h"
-#include "Odometry.h"
+#include "Objectif/objectif.h"
+#include "../Odometry/Odometry.h"
+#include "../AX12/AX12.h"
+#include "../../StepperMotor/Stepper.h"
#define MAP_RIGHTSIDE 1
#define MAP_LEFTSIDE 2
class map {
public:
- map (Odometry* nodo);
+ map (Odometry* nodo, AX12 * nA1, AX12 * nA2, AX12 * nA3, Stepper * nS1, Stepper * nS2);
void addObs (obsCarr nobs);
void FindWay (point dep, point arr);
void FindWay (float depX, float depY, float arrX, float arrY);
+ void Execute (int obj);
void Execute (float XObjectif, float YObjectif);
void Build(int couleur, int formation);
nVector<pointParcours>& getParc () { return path; }
@@ -24,8 +32,14 @@
protected:
nVector<obsCarr> obs;
nVector<pointParcours> path;
+ nVector<Objectif> objectifs;
+
bool endedParc; // Definit s'il existe un chemin parcourable dans le dernier FindWay
Odometry* Codo;
+ AX12 * A1, * A2, * A3;
+ Stepper * S1, * S2;
+
+ float min_x_table, min_y_table, max_x_table, max_y_table;
};
#endif
\ No newline at end of file