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