public void HandleInput()
 {
     if (ControlStrategy != null)
     {
         ControlStrategy.HandleInput(entity);
     }
 }
Beispiel #2
0
        // include strategy specific settings
        public void updateControlStrategySettings()
        {
            ControlStrategy currentStrategy = Program.strategyList.ElementAt(Program.activeStrategyId);

            controlStrategySettingsPanel.Controls.Clear();
            controlStrategySettingsPanel.Controls.Add(currentStrategy);
        }
Beispiel #3
0
 /*
  * This function doesn't take a random number each time, instead it adds a smaller random number to the existing speed and heading.
  */
 public override void calculateNextMove(DoublePoint robotPosition, double speed, DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out DoublePoint referenceHeading)
 {
     this.heading     = heading;
     this.position    = robotPosition;
     referenceSpeed   = speed + (2 * random.NextDouble() - 1) * this.maxSpeed / 10;
     referenceHeading = ControlStrategy.ang2Point(ControlStrategy.point2Ang(heading) + this.maxAngSpeed * (2 * random.NextDouble() - 1));
     borderAvoidance.calculateNextMove(robotPosition, referenceSpeed, referenceHeading, neighbors, out referenceSpeed, out referenceHeading);
 }
Beispiel #4
0
        // ----------------- Tick method -----------------
        /// <summary> Make a tick with robot brain.</summary>
        public void updateRobot()
        {
            if (currentStrategy != null)
            {
                currentStrategy.calculateNextMove(position, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
            }
            else
            {
                setStrategy(new StandStill());
            }

            // Run collision avoidance code to check if robot is blocked.
            blocked = false;
            blocked = isBlocked();

            if (blocked)
            {
                //setMotorSignals(new int[2] { 0, 0 }); //uncomment if using m3pi robots

                //$$$$$Changes/Additions for RC car$$$$$//
                setMotorSignals(new int[2] {
                    Program.neutralSpeed, motorSignals[1]
                });
                if (neighbors[0].getID() != 0) //The updateRobot() function is called separately for each robot; therefore a new obstacle avoidance strategy needs to be set only for glyph 0.
                {
                    if (!blocktime)            //The first time obstacle is detected, calculate the final point until which obstacle avoidance strategy needs to be valid.
                    {
                        finalPositionX = 2 * neighbors[0].getPosition().X - position.X;
                        finalPositionY = 2 * neighbors[0].getPosition().Y - position.Y;
                        blocktime      = true;
                    }
                    int         nrPointsObsAvoid = 15;
                    DoublePoint positionObst     = neighbors[0].getPosition();

                    if (Math.Abs(finalPositionX - position.X) > 300 || Math.Abs(finalPositionY - position.Y) > 300)
                    {
                        setStrategy(new FollowPath("Avoid Obstacle", FollowPath.createEllipsePoints(200, 100, positionObst, nrPointsObsAvoid)));   //obstacle avoidance path.
                    }
                    else
                    {
                        currentStrategy = Program.strategyList.ElementAt(Program.activeStrategyId);     //After completing half-circle, the robot should continue on it's previous path as set in GUI.
                    }
                }
                else
                {
                    setStrategy(new StandStill());    //For the obstacle, keep the strategy as StandStill.
                }
                //$$$$$$$$$$//
            }
            //else  //uncomment if using m3pi robots
            //{ //uncomment if using m3pi robots
            //  setMotorSignals(controller(speed, heading, referenceSpeed, referenceHeading));    //uncomment if using m3pi robots
            setMotorSignals(controller(speed, heading, referenceHeading));

            //} //uncomment if using m3pi robots
        }
Beispiel #5
0
 /// <summary>
 ///     创建序号件维修控制组
 /// </summary>
 /// <param name="ctrlStrategy">控制策略</param>
 /// <param name="snScope">序号范围</param>
 /// <param name="description">维修控制描述</param>
 /// <param name="ctrlDetail">维修控制明细</param>
 /// <param name="maintainWork">维修工作</param>
 /// <returns>序号件维修控制组</returns>
 public static SnMaintainCtrl CreateSnMaintainCtrl(string snScope, ControlStrategy ctrlStrategy,
     string description, string ctrlDetail, MaintainWork maintainWork)
 {
     var snMaintainCtrl = new SnMaintainCtrl();
     snMaintainCtrl.GenerateNewIdentity();
     snMaintainCtrl.SetCtrlStrategy(ctrlStrategy);
     snMaintainCtrl.SetSnScope(snScope);
     snMaintainCtrl.SetCtrlDetail(ctrlDetail);
     snMaintainCtrl.SetDescription(description);
     snMaintainCtrl.SetMaintainWork(maintainWork);
     return snMaintainCtrl;
 }
Beispiel #6
0
 /// <summary>
 ///     创建附件维修控制组
 /// </summary>
 /// <param name="pnReg">附件</param>
 /// <param name="ctrlStrategy">控制策略</param>
 /// <param name="description">维修控制描述</param>
 /// <param name="ctrlDetail">维修控制明细</param>
 /// <param name="maintainWork">维修工作</param>
 /// <returns>附件维修控制组</returns>
 public static PnMaintainCtrl CreatePnMaintainCtrl(PnReg pnReg, ControlStrategy ctrlStrategy,
     string description, string ctrlDetail, MaintainWork maintainWork)
 {
     var pnMaintainCtrl = new PnMaintainCtrl();
     pnMaintainCtrl.GenerateNewIdentity();
     pnMaintainCtrl.SetCtrlStrategy(ctrlStrategy);
     pnMaintainCtrl.SetPnReg(pnReg);
     pnMaintainCtrl.SetCtrlDetail(ctrlDetail);
     pnMaintainCtrl.SetDescription(description);
     pnMaintainCtrl.SetMaintainWork(maintainWork);
     return pnMaintainCtrl;
 }
Beispiel #7
0
 /// <summary>
 ///     创建项维修控制组
 /// </summary>
 /// <param name="item">附件项</param>
 /// <param name="ctrlStrategy">控制策略</param>
 /// <param name="description">维修控制描述</param>
 /// <param name="ctrlDetail">维修控制明细</param>
 /// <param name="maintainWork">维修工作</param>
 /// <returns>项维修控制组</returns>
 public static ItemMaintainCtrl CreateItemMaintainCtrl(Item item, ControlStrategy ctrlStrategy,
     string description, string ctrlDetail, MaintainWork maintainWork)
 {
     var itemMaintainCtrl = new ItemMaintainCtrl();
     itemMaintainCtrl.GenerateNewIdentity();
     itemMaintainCtrl.SetItem(item);
     itemMaintainCtrl.SetCtrlStrategy(ctrlStrategy);
     itemMaintainCtrl.SetDescription(description);
     itemMaintainCtrl.SetMaintainWork(maintainWork);
     itemMaintainCtrl.SetCtrlDetail(ctrlDetail);
     return itemMaintainCtrl;
 }
Beispiel #8
0
        public void calculateNextMove(DoublePoint referencePoint, int offset, DoublePoint robotPosition, double speed, DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out DoublePoint referenceHeading)
        {
            this.referencePoint = referencePoint;
            if (robotPosition != referencePoint)
            {
                referenceHeading = ControlStrategy.vector2Point(robotPosition, referencePoint);

                double distance = robotPosition.DistanceTo(referencePoint);
                referenceSpeed = speedRegulator(distance - offset);
            }
            else
            {
                referenceSpeed   = Program.neutralSpeed;
                referenceHeading = heading;
            }
        }
Beispiel #9
0
 /// <summary>
 ///     设置控制策略
 /// </summary>
 /// <param name="ctrlStrategy">控制策略</param>
 public void SetCtrlStrategy(ControlStrategy ctrlStrategy)
 {
     switch (ctrlStrategy)
     {
         case ControlStrategy.先到为准:
             CtrlStrategy = ControlStrategy.先到为准;
             break;
         case ControlStrategy.后到为准:
             CtrlStrategy = ControlStrategy.后到为准;
             break;
         default:
             throw new ArgumentOutOfRangeException("ctrlStrategy");
     }
 }
Beispiel #10
0
        //$$$$$$$$$$//


        //Uncomment the below section if using m3pi robots//
        /// <summary>Robot speed and angle control.</summary>

        /*private int[] controller(double speed, DoublePoint heading, double referenceSpeed, DoublePoint referenceHeading)
         * {
         *  int[] tempMotorSignals = new int[2];
         *  const int zeroControl = 2;
         *  const int minControl = 25;
         *  const int maxControl = 127;
         *  const int maxSpeedControl = 70;
         *
         *  // Regulator P constants.
         *  double pAngle = 20;
         *  double pSpeed = 1.2;
         *
         *  // Calculate angels from vectors.
         *  double angle = Math.Atan2(heading.Y, heading.X);
         *  double referenceAngle = Math.Atan2(referenceHeading.Y, referenceHeading.X);
         *
         *  // Calculate errors and correct for if angle changes sign.
         *  double angleError = referenceAngle - angle;
         *  if (Math.Abs(angleError) > Math.PI)
         *  {
         *      angleError = angleError - Math.Sign(angleError) * 2 * Math.PI;
         *  }
         *  //double speedError = referenceSpeed - speed // NO REAL SPEED CONTROLLER!!!
         *  double speedError = referenceSpeed;
         *
         *  double angleControl = pAngle * angleError;
         *  double speedControl = pSpeed * speedError;
         *
         *  // Saturate speed contol if larger than max allowed value
         *  if (Math.Abs(speedControl) > maxSpeedControl)
         *  {
         *      speedControl = Math.Sign(speedControl) * maxSpeedControl;
         *  }
         *
         *  // Merge angle and speed part of control signal.
         *  tempMotorSignals[0] = (int)(speedControl + angleControl / 2);
         *  tempMotorSignals[1] = (int)(speedControl - angleControl / 2);
         *
         *  // Fix contol signals if smaller/larger than min/max allowed value
         *  if (Math.Abs(tempMotorSignals[0]) < minControl && Math.Abs(tempMotorSignals[0]) > zeroControl || Math.Abs(tempMotorSignals[1]) < minControl && Math.Abs(tempMotorSignals[1]) > zeroControl)
         *  {
         *      if (Math.Abs(tempMotorSignals[0]) < Math.Abs(tempMotorSignals[1]))
         *      {
         *          tempMotorSignals[1] = tempMotorSignals[1] + Math.Sign(tempMotorSignals[1]) * (minControl - Math.Abs(tempMotorSignals[0]));
         *          tempMotorSignals[0] = Math.Sign(tempMotorSignals[0]) * minControl;
         *      }
         *      else
         *      {
         *          tempMotorSignals[0] = tempMotorSignals[0] + Math.Sign(tempMotorSignals[0]) * (minControl - Math.Abs(tempMotorSignals[1]));
         *          tempMotorSignals[1] = Math.Sign(tempMotorSignals[1]) * minControl;
         *      }
         *  }
         *
         *  // Saturate control signal or set to zero if its smaller than the min allowed value.
         *  if (Math.Abs(tempMotorSignals[0]) > maxControl)
         *  {
         *      tempMotorSignals[0] = Math.Sign(tempMotorSignals[0]) * maxControl;
         *  }
         *  else if (Math.Abs(tempMotorSignals[0]) <= zeroControl)
         *  {
         *      tempMotorSignals[0] = 0;
         *  }
         *
         *  if (Math.Abs(tempMotorSignals[1]) > maxControl)
         *  {
         *      tempMotorSignals[1] = Math.Sign(tempMotorSignals[1]) * maxControl;
         *  }
         *  else if (Math.Abs(tempMotorSignals[1]) <= zeroControl)
         *  {
         *      tempMotorSignals[1] = 0;
         *  }
         *
         *  return tempMotorSignals;
         *
         * }*/


        // ----------------- Update GUI method -----------------
        public void setStrategy(ControlStrategy strategy)
        {
            currentStrategy = strategy.cloneStrategy();
        }
 public DoublePoint[] getHeadingEdges(AForge.DoublePoint robotPosition, AForge.DoublePoint robotHeading)
 {
     ControlStrategy.isAtHeading(this, robotPosition, robotHeading);
     return(headingEdges);
 }
Beispiel #12
0
        public void calculateNextMove(CollisionArea collisionArea, AForge.DoublePoint robotPosition, double speed, AForge.DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out AForge.DoublePoint referenceHeading)
        {
            bool go = true;

            if (ControlStrategy.isAtHeading(collisionArea, robotPosition, heading))
            {
                int distance = collisionArea.distance2Middle(robotPosition);

                if (queue.Count == 0)
                {
                    foreach (Robot neighbor in neighbors)
                    {
                        if (ControlStrategy.isAtHeading(collisionArea, neighbor.getPosition(), neighbor.getHeading()))
                        {
                            if (distance > collisionArea.distance2Robot(neighbor))
                            {
                                queue.Add(neighbor.getID());
                                go = false;
                            }
                        }
                    }
                }
                else
                {
                    foreach (Robot neighbor in neighbors)
                    {
                        if (ControlStrategy.isAtHeading(collisionArea, neighbor.getPosition(), neighbor.getHeading()))
                        {
                            foreach (int ID in queue)
                            {
                                if (ID == neighbor.getID())
                                {
                                    go = false;
                                    break;
                                }
                            }
                        }
                    }
                }
                foreach (Robot neighbor in neighbors)
                {
                    if (collisionArea.intersects(neighbor.getPosition()))
                    {
                        go = false;
                        break;
                    }
                }
            }
            else
            {
                queue.Clear();
            }

            if (go)
            {
                referenceSpeed   = speed;
                referenceHeading = heading;
            }
            else
            {
                stopStrategy.calculateNextMove(collisionArea.getNearestPoint(robotPosition), Program.robotRadius, robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
            }

            corner1            = collisionArea.getHeadingEdges(robotPosition, heading)[0];
            corner2            = collisionArea.getHeadingEdges(robotPosition, heading)[1];
            this.robotPosition = robotPosition;
        }