//on met a jour
private void updatePosition(mode m)
{
switch (m)
{
case mode.turn:
int angleDeplacement = 0;
double newTheta = position.theta;
int errorCodeAngle = 0;
errorCodeAngle = getDataSinceLastReset(mode.turn, ref angleDeplacement);
if (errorCodeAngle == 0)
{
newTheta += angleDeplacement;
position = new PointOriente(position.x, position.y, newTheta);
}
break;
case mode.drive:
int deplacement = 0;
double theta = position.theta;
double X = position.x;
double Y = position.y;
int errorCode = getDataSinceLastReset(mode.drive, ref deplacement);
if (errorCode == 0)
{
double angle = System.Math.PI * theta / 180.0;
X += deplacement * System.Math.Cos(angle);
Y += deplacement * System.Math.Sin(angle);
position = new PointOriente(X, Y, theta);
}
break;
}
Informations.printInformations(Priority.LOW, "l'information sur la position a été mise à jour");
}
public void TestRectangle1()
{
PointOriente[] pointsIn = { new PointOriente(10, 5, -5),
new PointOriente(-10, 15),
new PointOriente(10, 15, 10),
new PointOriente(-10, 5, 10),
new PointOriente(0, 10, 10) };
PointOriente[] pointsOut =
{
new PointOriente(-10, 0, -5),
new PointOriente(-10.01, 10, -5),
new PointOriente(0, 15.001, -5)
};
PointOriente centre = new PointOriente(0, 10, 0);
double LX = 10;
double LY = 5;
Rectangle rect = new Rectangle(centre, LX, LY);
foreach (PointOriente pt in pointsIn)
{
Assert.IsTrue(rect.Appartient(pt));
}
foreach (PointOriente pt in pointsOut)
{
Assert.IsFalse(rect.Appartient(pt));
}
}
public Mouvement(PointOriente destination, bool adjustToAngle, int speedDrive, int speedTurn)
{
this.adjustToAngle = adjustToAngle;
this.destination = destination;
this.speedDrive = speedDrive;
this.speedTurn = speedTurn;
}
public void TestRectangleOrienteRetourne()
{
PointOriente[] pointsIn = { new PointOriente(10, 5, -5),
new PointOriente(-10, 15),
//new PointOriente(10, 15, 10),
new PointOriente(9.9999, 14.999999, 10),
new PointOriente(-9.9999999, 5.0001, 10),
//new PointOriente(-10, 5, 10), // Attention à l'arrondi
new PointOriente(0, 10, 10) };
PointOriente[] pointsOut =
{
new PointOriente(-10, 0, -5),
new PointOriente(-10.01, 10, -5),
new PointOriente(0, 15.001, -5)
};
PointOriente centre = new PointOriente(0, 10, Math.PI);
double LX = 10;
double LY = 5;
Rectangle rect = new Rectangle(centre, LX, LY);
foreach (PointOriente pt in pointsIn)
{
Assert.IsTrue(rect.Appartient(pt));
}
foreach (PointOriente pt in pointsOut)
{
Assert.IsFalse(rect.Appartient(pt));
}
}
public Arc(PointOriente p, double l, double d, double a)
{
origine = p;
largeur = l;
distance = d;
angle = a;
}
public Boolean AdjustAngleToLieuCle(LieuCle lieu)
{
AdjustAngleToPoint(lieu.pointDeReference);
PointOriente positionDuRobotApresDeplacement = null;
return(lieu.IsAtTheRightAngle(positionDuRobotApresDeplacement));
}
public Boolean GoToLieuCle(LieuCle lieu)
{
GoToOrientedPoint(lieu.pointDeReference);
PointOriente positionDuRobotApresDeplacement = null;
return(lieu.IsAtTheRightPlace(positionDuRobotApresDeplacement));
}
public Mouvement(PointOriente destination, bool adjustToAngle, int speedDrive)
{
this.adjustToAngle = adjustToAngle;
this.destination = destination;
this.speedDrive = speedDrive;
this.speedTurn = Robot.robot.BASE_ROULANTE.speedTurn;
}
public Boolean GoAndAdjustAngleToLieuCle(LieuCle lieu)
{
GoToLieuCle(lieu);
AdjustAngleToLieuCle(lieu);
PointOriente positionDuRobotApresDeplacement = null;
return(lieu.IsAtTheRightPlaceAndAngle(positionDuRobotApresDeplacement));
}
public Mouvement(PointOriente destination, OBSTACLE_DIRECTION forcedDirection, int speedDrive, int speedTurn)
{
this.destination = destination;
this.isDirectionForced = true;
this.forcedDirection = forcedDirection;
this.speedDrive = speedDrive;
this.speedTurn = speedTurn;
}
public Mouvement(PointOriente destination, OBSTACLE_DIRECTION forcedDirection, int speedDrive)
{
this.destination = destination;
this.isDirectionForced = true;
this.forcedDirection = forcedDirection;
this.speedDrive = speedDrive;
this.speedTurn = Robot.robot.BASE_ROULANTE.speedTurn;
}
public Mouvement(PointOriente destination, bool adjustToAngle, OBSTACLE_DIRECTION forcedDirection)
{
this.adjustToAngle = adjustToAngle;
this.destination = destination;
this.isDirectionForced = true;
this.forcedDirection = forcedDirection;
this.speedDrive = Robot.robot.BASE_ROULANTE.speedDrive;
this.speedTurn = Robot.robot.BASE_ROULANTE.speedTurn;
}
public Boolean GoToOrientedPoint(PointOriente pt)
{
//Calculer l'angle a tourner
double deltaX = pt.x - position.x;
double deltaY = pt.y - position.y;
double result, radians, angle;
int deltaAngle;
if (deltaX != 0)
{
result = deltaY / deltaX;
radians = System.Math.Atan(result);
angle = radians * (180 / System.Math.PI);
}
else
{
if (deltaY > 0)
{
angle = 90;
}
else
{
angle = -90;
}
}
deltaAngle = doubleToIntForTheta(angle - position.theta);
int sens = 1; //avance=1,recule=-1
//Ameliorer l'angle a tourner
//[-360,-270],[270,360]->[0,90], [-90,0], sens=avance
if (System.Math.Abs(deltaAngle) >= 270)
{
deltaAngle -= System.Math.Sign(deltaAngle) * 360;
}
//[-270,-90],[90,270]->[-90,90], sens=recule
else if (System.Math.Abs(deltaAngle) >= 90)
{
deltaAngle -= System.Math.Sign(deltaAngle) * 180;
sens = -1;
angle -= System.Math.Sign(angle) * 180;
}
//Faire orienter a la destination
RotateAndSleep(deltaAngle);
//Aller tout droite
int distance = doubleToIntForXY(System.Math.Sqrt(pt.distanceSquared(position)));
AvanceAndSleep(distance * sens);
//Mise a jour la position
position = new PointOriente(pt.x, pt.y, angle);
return(false);
}
public void TestTranslation()
{
PointOriente point1 = new PointOriente(10, -10, -5);
PointOriente point2 = new PointOriente(0, 15, 10);
PointOriente resultat = new PointOriente(10, 5, 5);
Assert.AreEqual(point1.translater(point2), resultat);
}
public override bool Appartient(PointOriente p)
{
double rel_x = p.x - origine.x;
double rel_y = p.y - origine.y;
double proj_rel_1 = System.Math.Cos(origine.theta) * rel_x + System.Math.Sin(origine.theta) * rel_y;
double proj_rel_2 = -System.Math.Sin(origine.theta) * rel_x + System.Math.Cos(origine.theta) * rel_y;
return((-l_X <= proj_rel_1 && proj_rel_1 <= l_X) && (-l_Y <= proj_rel_2 && proj_rel_2 <= l_Y));
}
public override bool Appartient(PointOriente p)
{
double rel_x = p.x - origine.x;
double rel_y = p.y - origine.y;
if (rel_x == 0 && rel_y == 0)
{
return(distance == 0);
}
else
{
bool horsCercle1 = (rel_x * rel_x + rel_y * rel_y) >= distance;
bool dansCercle2 = (rel_x * rel_x + rel_y * rel_y) <= distance + largeur;
bool dansCadrant = (System.Math.Atan(rel_y / rel_x) >= p.theta && System.Math.Atan(rel_y / rel_x) <= p.theta + angle);
return(horsCercle1 && dansCercle2 && dansCadrant);
}
}
public Boolean Move(MOVETYPES type, PointOriente pt)
{
switch (type)
{
case MOVETYPES.GoTo:
GoToOrientedPoint(pt);
break;
case MOVETYPES.AdjustAngle:
AdjustAngleToPoint(pt);
break;
case MOVETYPES.GoToAndAdjustAngle:
GoToAndAdjustAngleToPoint(pt);
break;
}
return(false);
}
private static void loadActionTest1()
{
/*Action MOTHER_ACTION = new ActionBuilder("Action mère Test1").Add(
* new Actions.ActionsIHM.ActionBouton(Robot.robot.TR1_BOUTON_1)
* )
* .Add(
* new ActionBuilder("Allumer LED").BuildActionDelegate(Robot.robot.TR1_BOUTON_1.TurnLedOn)
* ).Add(
* new ActionBuilder("Wait a bit...").BuildActionWait(2000)
* )
* .Add(
* new ActionBuilder("Eteindre LED").BuildActionDelegate(Robot.robot.TR1_BOUTON_1.TurnLedOff)
* ).BuildActionEnSerieRepeated(-1); // Envois infinis
*
* setMotherAction(ModeOperatoire.TEST1, MOTHER_ACTION);*/
PointOriente pt1 = new PointOriente(100, 100, 50);
PointOriente pt2 = new PointOriente(100, 200, 50);
PointOriente pt3 = new PointOriente(100, 0, 50);
Action MOTHER_ACTION = new ActionBuilder("Action mère Test1").Add(
new Actions.ActionsIHM.ActionBouton(Robot.robot.TR1_BOUTON_1)
)
.Add(
new Actions.ActionBaseRoulante("Point1 ", pt1)
).Add(
new ActionBuilder("Wait a bit...").BuildActionWait(2000)
)
.Add(
new Actions.ActionBaseRoulante("Point2 ", pt2)
).Add(
new ActionBuilder("Wait a bit...").BuildActionWait(2000)
)
.Add(
new Actions.ActionBaseRoulante("Point3 ", pt3)
)
.BuildActionEnSerieRepeated(-1); // Envois infinis
setMotherAction(ModeOperatoire.TEST1, MOTHER_ACTION);
}
public void TestRectangleOrienteAngleDroit() // Attention, Math.Cos(PI/2) = 10e-17
{
PointOriente[] pointsIn = { new PointOriente(10, 5, -5),
new PointOriente(-10, 15),
//new PointOriente(10, 15, 10),
new PointOriente(9.9999, 14.999999, 10),
new PointOriente(-9.9999999, 5.0001, 10),
//new PointOriente(-10, 5, 10), // Attention à l'arrondi
new PointOriente(0, 10, 10) };
PointOriente[] pointsOut =
{
new PointOriente(-10, 0, -5),
new PointOriente(-10.01, 10, -5),
new PointOriente(0, 15.001, -5)
};
PointOriente centre = new PointOriente(0, 10, Math.PI / 2);
double LX = 5;
double LY = 10;
Rectangle rect = new Rectangle(centre, LX, LY);
//Assert.AreEqual(System.Math.Sin(Math.PI / 2), 1d);
//Assert.AreEqual(System.Math.Cos(Math.PI / 2), 0d);
foreach (PointOriente pt in pointsIn)
{
Assert.IsTrue(rect.Appartient(pt));
}
foreach (PointOriente pt in pointsOut)
{
Assert.IsFalse(rect.Appartient(pt));
}
}
public Boolean GoToOrientedPoint(PointOriente pt) // Automatically find direction
{
double angle;
double deltaX, deltaY, deltaTheta;
deltaX = pt.x - this.GetPosition().x;
deltaY = pt.y - this.GetPosition().y;
Informations.printInformations(Priority.HIGH, "Going to " + pt.x.ToString() + "," + pt.y.ToString() + "\r\n");
if (deltaX == 0)
{
if (deltaY > 0)
{
deltaTheta = 90;
}
else
{
deltaTheta = 270;
}
}
else
{
deltaTheta = convertTo360(180 / System.Math.PI * System.Math.Atan2(deltaY, deltaX));
}
angle = (convertTo360(this.GetPosition().theta) - convertTo360(deltaTheta) + 90);
Informations.printInformations(Priority.HIGH, "Angle before modulo : " + angle.ToString());
angle = convertTo360(angle);
Informations.printInformations(Priority.LOW, "J'ai deltaTheta = " + deltaTheta.ToString());
Informations.printInformations(Priority.LOW, "Donc angle = " + angle.ToString());
if (angle > 180)
{
return(this.GoToOrientedPoint(pt, OBSTACLE_DIRECTION.ARRIERE));
}
else
{
return(this.GoToOrientedPoint(pt, OBSTACLE_DIRECTION.AVANT));
}
}
public Boolean AdjustAngleToPoint(PointOriente pt) // ajuste theta, mais pas X,Y => mode turn
{
//Calculer l'angle a tourner
Double currentAngle = position.theta; //[-180,180]
Double Angle = pt.theta; //[-180,180]
int deltaAngle = doubleToIntForTheta((Angle - currentAngle)); //[-360,360]
//Amerliorer l'angle a tourner
if (deltaAngle > 180)
{
deltaAngle -= 360; //[180,360]->[-180,0]
}
else if (deltaAngle < -180)
{
deltaAngle += 360; //[-360,-180]->[-180,0]
}
//Faire la rotation
RotateAndSleep(deltaAngle);
//Mise a jour la position
position = new PointOriente(position.x, position.y, pt.theta);
return(false);
}
int PARAMETER_FOR_THETA = 100; //l'unite de l'angle = millidegree, on accepte l'entree de la forme X.XX degrees
public BaseRoulante(int socket)
: base(socket)
{
this.kangaroo = new Kangaroo(socket);
this.position = new PointOriente(0, 0, 0);
}
public void setPosition(PointOriente pt)
{
position = pt;
Informations.printInformations(Priority.LOW, "la position a été redéfinie");
}
public void setPosition(PointOriente pt)
{
position = pt;
}
public Action GetMotherActionPRCompetitionForCouleurOrange()
{
int dis = 93; //distance entre le milieu du robot (la ou on veut les cubes, et lecentre de rotation)
//signe y a changer selon le cote
PointOriente ptInit = new PointOriente(3000 - (125 - dis), -108, 180);
PointOriente pt0 = new PointOriente(3000 - 125, -108, 0);
PointOriente pt1 = new PointOriente(3000 - (1130 - dis + 90), -350, 50);
PointOriente pt2 = new PointOriente(3000 - (1130 - dis + 90), -5 /*-(-15 + dis)*/, 90);
// only 0-2 used for homol
PointOriente pt3 = new PointOriente(3000 - (1230 + dis + 90), -530, -180); //+610
//PointOriente ptIntermediaire = new PointOriente(1100+dis, 620, -180);
PointOriente pt4 = new PointOriente(3000 - (890 + dis), -530, -180);
PointOriente zone = new PointOriente(3000 - 600, -100, 0);
PointOriente pt5 = new PointOriente(3000 - 500, -200, 0);
/*
* PointOriente pt3 = new PointOriente(1230 + dis + 90, -530, -180); //+610
* //PointOriente ptIntermediaire = new PointOriente(1100+dis, 620, -180);
* PointOriente pt4 = new PointOriente(890 + dis, -530, -180);
* PointOriente zone = new PointOriente(500, -100, 0);
* PointOriente pt5 = new PointOriente(500, -200, 0); */
PointOriente pt_homol_1 = new PointOriente(3000 - (1230 + dis + 90), -1500, -180); //+610
PointOriente pt_homol_2 = new PointOriente(3000 - (1230 + dis + 90), -600, -180); //+610
Action MOTHER_ACTION = new ActionBuilder("test")
//.Add(new ActionBuilder("Afficher score homol").BuildActionDelegate(() => Robot.robot.IHM.AfficherInformation("Score : 0", false)))
.Add(new ActionJack())
.Add(new ActionBuilder("set init").BuildActionSetPositionInitiale(ptInit.x, ptInit.y, ptInit.theta))
.Add(new ActionBuilder("Regler vitese drive").BuildActionDelegate(() => Robot.robot.BASE_ROULANTE.speedDrive = 200))
.Add(new ActionBuilder("pt0").BuildActionBaseRoulante_GOTO_ONLY(pt0))
.Add(new ActionBuilder("Activer IR").BuildActionSetDetecteurObstacle(true))
.Add(new ActionBuilder("pt1").BuildActionBaseRoulante_GOTO_ONLY(pt1))
.Add(new ActionBuilder("Activer IR").BuildActionSetDetecteurObstacle(false))
.Add(new ActionBuilder("pt2").BuildActionBaseRoulante_GOTO_ONLY(pt2, OBSTACLE_DIRECTION.ARRIERE)).Add(new ActionBuilder("Ajouter score").BuildActionAddScore(25))
.Add(new ActionBuilder("Recallage").BuildActionRecallageAxeX(-30, 10, 1, 1, -90))
/* POINTS BIDON */ /*
* .Add(new ActionBuilder("Activer IR").BuildActionSetDetecteurObstacle(true))
*
* .Add(new ActionBuilder("pt2").BuildActionBaseRoulante_GOTO_ONLY(pt_homol_1, OBSTACLE_DIRECTION.AVANT))
* .Add(new ActionBuilder("pt2").BuildActionBaseRoulante_GOTO_ONLY(pt_homol_2, OBSTACLE_DIRECTION.ARRIERE))
*
*
* .Add(new ActionBuilder("pt2").BuildActionBaseRoulante_GOTO_ONLY(pt_homol_1, OBSTACLE_DIRECTION.AVANT))
* .Add(new ActionBuilder("pt2").BuildActionBaseRoulante_GOTO_ONLY(pt_homol_2, OBSTACLE_DIRECTION.ARRIERE))
*/
.Add(new ActionBuilder("Recule un peu").BuildActionBaseRoulante_DRIVE(-100, 100))
.Add(new ActionBuilder("pt3").BuildActionBaseRoulante_GOTO_ONLY(pt3, OBSTACLE_DIRECTION.AVANT))
//).Add(new ActionBuilder("ptInter").BuildActionBaseRoulante_GOTO_ONLY(ptIntermediaire, OBSTACLE_DIRECTION.AVANT)
.Add(new ActionBuilder("pt4").BuildActionBaseRoulante_GOTO_ONLY(pt4, OBSTACLE_DIRECTION.AVANT))
.Add(new ActionRamasseCube())
.Add(new ActionBuilder("Zone").BuildActionBaseRoulante_GOTO_ONLY(zone, OBSTACLE_DIRECTION.AVANT))
//.Add(new ActionReleaseCube())
.Add(new ActionBuilder("Recule un peu").BuildActionBaseRoulante_DRIVE(100, 100))
.BuildActionEnSerie();
return(MOTHER_ACTION);
}
public Boolean GoToOrientedPoint(PointOriente pt, OBSTACLE_DIRECTION forceDir) // forceDir = AVANT or ARRIERE
{
isPaused = Map.MapInformation.isDetecteurOn && Robot.robot.OBSTACLE_MANAGER.getObstacleStatusForDirection(forceDir);
if (isPaused)
{
return(false);
}
Informations.printInformations(Priority.HIGH, "Going (dir forced) to " + pt.x.ToString() + "," + pt.y.ToString() + "\r\n");
Robot.robot.BASE_ROULANTE.direction = forceDir;
// AdjustToAngle == True must be handled here
double angle = 0;
double deltaX, deltaY, deltaTheta, alpha;
deltaX = pt.x - this.GetPosition().x;
deltaY = pt.y - this.GetPosition().y;
deltaTheta = 180 / System.Math.PI * System.Math.Atan2(deltaY, deltaX);
if (deltaY == 0)
{
if (deltaX > 0)
{
deltaTheta = 0;
}
else
{
deltaTheta = 180;
}
}
if (deltaTheta < 0)
{
deltaTheta = 360 + deltaTheta;
}
// We now have the good theta (between 0 and 360)
if (forceDir == OBSTACLE_DIRECTION.AVANT)
{
Informations.printInformations(Priority.HIGH, "Going en avant");
if (this.GetPosition().theta % 360 - deltaTheta % 360 > 180) // That means we have to turn atrigo
{
Rotate(convertTo180(360 - this.GetPosition().theta % 360 - deltaTheta % 360));
// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
else
{
Rotate(convertTo180(-this.GetPosition().theta % 360 + deltaTheta % 360));
// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
deltaX = pt.x - this.GetPosition().x;
deltaY = pt.y - this.GetPosition().y;
Avance((int)System.Math.Sqrt(deltaX * deltaX + deltaY * deltaY));
//Robot.robot.BASE_ROULANTE.MoveCompleted.WaitOne();// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
//position = new PointOriente(pt.x, pt.y, deltaTheta);
angle = deltaTheta;
}
if (forceDir == OBSTACLE_DIRECTION.ARRIERE)
{
Informations.printInformations(Priority.HIGH, "Going en arriere");
if (this.GetPosition().theta % 360 - deltaTheta > 180)
{//turn antitrigo
Rotate(convertTo180(-convertTo360(this.GetPosition().theta) + 180 - convertTo360(deltaTheta)));
//Robot.robot.BASE_ROULANTE.MoveCompleted.WaitOne();// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
else
{
Rotate(convertTo180(180 - convertTo360(this.GetPosition().theta) + convertTo360(deltaTheta)));
//Robot.robot.BASE_ROULANTE.MoveCompleted.WaitOne();// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
deltaX = pt.x - this.GetPosition().x;
deltaY = pt.y - this.GetPosition().y;
Avance(-(int)System.Math.Sqrt(deltaX * deltaX + deltaY * deltaY));
//position = new PointOriente(pt.x, pt.y, 180+deltaTheta%360); // check the angle
//Robot.robot.BASE_ROULANTE.MoveCompleted.WaitOne();// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
if (adjustToAngle)
{
Informations.printInformations(Priority.LOW, "Starting to adjust angle");
Rotate((convertTo180(-GetPosition().theta + pt.theta)));
//Robot.robot.BASE_ROULANTE.MoveCompleted.WaitOne();// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
Informations.printInformations(Priority.HIGH, "DOne with gotoForced dir.");
return(true);
}
public Mouvement(PointOriente destination, int speedDrive)
{
this.destination = destination;
this.speedDrive = speedDrive;
this.speedTurn = Robot.robot.BASE_ROULANTE.speedTurn;
}
/// <summary>
/// Constructeur d'une surface rectangulaire orientée par l'angle du point central.
/// </summary>
/// <param name="centre">Centre du rectangle</param>
/// <param name="deltaX">Taille par rapport à l'axe X : [centre.x +- deltaX]</param>
/// <param name="deltaY">Taille par rapport à l'axe Y : [centre.y +- deltaY]</param>
public Rectangle(PointOriente centre, double deltaX, double deltaY)
{
origine = centre.Clone();
l_Y = System.Math.Abs(deltaY);
l_X = System.Math.Abs(deltaX);
}
public Action GetMotherActionForCouleurVerte()
{
ActionBuilder MotherActionBuilder = new ActionBuilder("Gros robot - mother action couleur verte");
//double positionInitialeGR_X = dimensionGR_X / 2;
double positionInitialeGR_X = 390 - dimensionGR_Y / 2;
double positionInitialeGR_Y = -(630 - dimensionGR_X / 2);
double positionInitialieTheta = -90;
PointOriente point_tube_uni_intermediaire_1 = new PointOriente(positionInitialeGR_X + 200, positionInitialeGR_Y, 0);
PointOriente point_tube_uni_intermediaire_2 = new PointOriente(200 + dimensionGR_X / 2, -845, 0);
PointOriente point_tube_uni_final = new PointOriente(100, -845, 0); // direction arriere, adjust to angle
PointOriente point_tube_uni_lancement = new PointOriente(710 - 168, -845, 110 - 90);
PointOriente point_abeille_intermediaire_1 = new PointOriente(1060, -845, -90);
PointOriente point_abeille_intermediaire_2 = new PointOriente(400, -2000 + 400, -130);
PointOriente point_abeille_final = new PointOriente(dimensionGR_Y / 2 + 40, 10 - 2000 + dimensionGR_X / 2, -90);
PointOriente point_tube_mixte_intermediaire_1 = new PointOriente(400, -2000 + 820, 0); // Parcourir la map
PointOriente point_tube_mixte_intermediaire_2 = new PointOriente(2400, -2000 + 820, 180); // Parcourir la map => IR = on
PointOriente point_tube_mixte_intermediaire_3 = new PointOriente(2400, -1600, 90); // Parcourir la map => IR = on
PointOriente point_tube_mixte_final = new PointOriente(2400, -2000 + dimensionGR_X / 2 - 10 - 100, 90); // direction arriere, adjust to angle
//PointOriente point_tube_mixte_final = new PointOriente(3000 - 2400 /*- 80*/ /* 7 cm de trop */, -2000 + dimensionGR_X / 2 - 10, 90+180); // direction avant, adjust to angle
PointOriente point_tube_mixte_envoi_balles_vertes = new PointOriente(2390, -2000 + 400, 50); // adjust to angle, (direction avant ?) 45 avannt
GestionnaireServosGR gestio = new GestionnaireServosGR();
ActionEnSerie ViderTubeUni = new ActionBuilder("Vider le tube uni")
.Add(new ActionBuilder("Ajouter score").BuildActionAddScore(10))
.Add(new ActionBuilder("Regler vitese drive").BuildActionDelegate(() => Robot.robot.BASE_ROULANTE.speedDrive = 300))
.Add(new ActionBuilder("Desactiver IR").BuildActionSetDetecteurObstacle(false))
.Add(new ActionBuilder("Deplacement").BuildActionBaseRoulante_GOTO_ONLY(point_tube_uni_intermediaire_2))
.Add(gestio.GR_PLATEAU_AVANT_VERT)
.Add(new ActionBuilder("Regler vitese drive").BuildActionDelegate(() => Robot.robot.BASE_ROULANTE.speedDrive = 300))
.Add(new ActionBuilder("Deplacement 2").BuildActionBaseRoulante_GOTO_ANGLE(point_tube_uni_final, OBSTACLE_DIRECTION.ARRIERE))
//.Add(new ActionBuilder("Recule un peu").BuildActionBaseRoulante_DRIVE(3, 100))
.Add(gestio.GR_TRAPPE_FERMER)
.Add(gestio.GR_PLATEAU_RECOLTE)
.Add(gestio.GR_PLATEAU_SLOT0)
//.Add(new ActionWait("Wait a bit...", 500))
.Add(new ActionBuilder("piche").BuildActionBaseRoulante_GOTO_ANGLE(point_tube_uni_lancement, OBSTACLE_DIRECTION.AVANT))
.Add(gestio.GR_PLATEAU_LIBERATION_TUBE_UNICOULEUR())
.Add(new ActionBuilder("Ajouter score").BuildActionAddScore(45))
.BuildActionEnSerie();
ActionEnSerie ActiverAbeille = new ActionBuilder("Activer l'abeille")
.Add(new ActionBuilder("Regler vitese drive").BuildActionDelegate(() => Robot.robot.BASE_ROULANTE.speedDrive = 800))
//.Add(new ActionBuilder("Recule un peu").BuildActionBaseRoulante_DRIVE(200, 100 * 200))
.Add(new ActionBuilder("Activer IR").BuildActionSetDetecteurObstacle(false))
.Add(new ActionBuilder("Deplacement").BuildActionBaseRoulante_GOTO_ONLY(point_abeille_intermediaire_1))
.Add(new ActionBuilder("Activer IR").BuildActionSetDetecteurObstacle(true))
.Add(new ActionBuilder("Deplacement").BuildActionBaseRoulante_GOTO_ONLY(point_abeille_intermediaire_2, OBSTACLE_DIRECTION.AVANT)) // change orange
.Add(new ActionBuilder("Activer IR").BuildActionSetDetecteurObstacle(false))
.Add(new ActionBuilder("Regler vitese drive").BuildActionDelegate(() => Robot.robot.BASE_ROULANTE.speedDrive = 350))
.Add(new ActionBuilder("Deplacement").BuildActionBaseRoulante_GOTO_ANGLE(point_abeille_final, OBSTACLE_DIRECTION.AVANT))
//.Add(new ActionDelegate("Reinit kangaroo", () => { Robot.robot.SWITCH_GLOBAL.Activate(); Thread.Sleep(300); Robot.robot.SWITCH_GLOBAL.Desactivate(); Thread.Sleep(300); }))
//.Add(new ActionBuilder("Recule un peu").BuildActionBaseRoulante_DRIVE(-200, 100 * 100))
.Add(new ActionBuilder("Deplacement").BuildActionRecallageAxeX(-2000 + dimensionGR_X / 2, 400, 300 * 100, 600 * 100, -90))
//.Add(new ActionBuilder("Deplacement").BuildActionBaseRoulante_GOTO_ONLY(point_abeille_final, OBSTACLE_DIRECTION.AVANT))
//.Add(new ActionDelegate("Reinit kangaroo", () => { Thread.Sleep(50); Robot.robot.SWITCH_GLOBAL.Activate(); Thread.Sleep(50); Robot.robot.SWITCH_GLOBAL.Desactivate(); Thread.Sleep(1500); }))
.Add(gestio.GR_ABEILLE_DECLENCHER)
.Add(new ActionBuilder("Ajouter score").BuildActionAddScore(50))
.Add(gestio.GR_PLATEAU_AVANT_VERT)
.Add(gestio.GR_ABEILLE_REPLIER)
.Add(new ActionBuilder("Recule un peu").BuildActionBaseRoulante_DRIVE(-200, 300))
/* Ajouter servoMoteur */
.BuildActionEnSerie();
ActionEnSerie ViderTubeMixte = new ActionBuilder("Vider le tube mixte")
.Add(new ActionBuilder("Activer IR").BuildActionSetDetecteurObstacle(false))
.Add(new ActionBuilder("Regler vitese drive").BuildActionDelegate(() => Robot.robot.BASE_ROULANTE.speedDrive = 800)) // 700-800
.Add(new ActionBuilder("Deplacement").BuildActionBaseRoulante_GOTO_ONLY(point_tube_mixte_intermediaire_1))
.Add(new ActionBuilder("Activer IR").BuildActionSetDetecteurObstacle(true))
.Add(new ActionBuilder("Deplacement").BuildActionBaseRoulante_GOTO_ONLY(point_tube_mixte_intermediaire_2, OBSTACLE_DIRECTION.ARRIERE))
.Add(new ActionBuilder("Regler vitese drive").BuildActionDelegate(() => Robot.robot.BASE_ROULANTE.speedDrive = 600))
.Add(new ActionBuilder("Deplacement").BuildActionBaseRoulante_GOTO_ONLY(point_tube_mixte_intermediaire_3, OBSTACLE_DIRECTION.ARRIERE))
.Add(new ActionBuilder("Desactiver IR").BuildActionSetDetecteurObstacle(false))
.Add(new ActionBuilder("Regler vitese drive").BuildActionDelegate(() => Robot.robot.BASE_ROULANTE.speedDrive = 200))
.Add(gestio.GR_TRAPPE_FERMER)
//.Add(gestio.GR_PLATEAU_AVANT_VERT)
.Add(new ActionBuilder("Deplacement").BuildActionBaseRoulante_GOTO_ONLY(point_tube_mixte_final, OBSTACLE_DIRECTION.ARRIERE))
//.Add(new ActionBuilder("Recule un peu").BuildActionBaseRoulante_DRIVE(3, 100))
.Add(gestio.GR_PLATEAU_RECOLTE)
.Add(new ActionBuilder("ouverture recolte mixte").BuildActionAddScore(10))
.Add(gestio.GR_PLATEAU_LIBERATION_BALLES_COULEUR_OPPOSEE())
.Add(new ActionBuilder("Deplacement").BuildActionBaseRoulante_GOTO_ANGLE(point_tube_mixte_envoi_balles_vertes, OBSTACLE_DIRECTION.AVANT))
.Add(gestio.GR_PLATEAU_LIBERATION_BALLES_TUBE_MIXTE_NOTRE_COULEUR())
.BuildActionEnSerie();
Action MOTHER_ACTION = new ActionBuilder("Mother action - competition Gros Robot")
.Add(new ActionJack())
.Add(new ActionBuilder("Position initiale GR Vert").BuildActionSetPositionInitiale(positionInitialeGR_X, positionInitialeGR_Y, positionInitialieTheta))
//.Add(new ActionBuilder("Position initiale GR Vert").BuildActionSetPositionInitiale(point_tube_uni_final.x, point_tube_uni_final.y, point_tube_uni_final.theta))
//.Add(new ActionBuilder("Recule un peu").BuildActionBaseRoulante_DRIVE(50, 100 * 200))
.Add(ViderTubeUni)
.Add(ActiverAbeille)
.Add(ViderTubeMixte)
.BuildActionEnSerie();
return(MOTHER_ACTION);
}
public Mouvement(PointOriente destination, int speedDrive, int speedTurn)
{
this.destination = destination;
this.speedDrive = speedDrive;
this.speedTurn = speedTurn;
}
