public void HandleInput() { if (ControlStrategy != null) { ControlStrategy.HandleInput(entity); } }
// include strategy specific settings public void updateControlStrategySettings() { ControlStrategy currentStrategy = Program.strategyList.ElementAt(Program.activeStrategyId); controlStrategySettingsPanel.Controls.Clear(); controlStrategySettingsPanel.Controls.Add(currentStrategy); }
/* * 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); }
// ----------------- 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 }
/// <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; }
/// <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; }
/// <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; }
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; } }
/// <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"); } }
//$$$$$$$$$$// //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); }
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; }