//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; }