/// <summary>
/// set all signal lights to reflect current Collision State
/// </summary>
private void setCollisionLights()
{
CollisionState cs = _state.collisionState;
SetLightsWhiskers();
SetLightsMustStop(cs.mustStop);
SetLightsCanMoveForward(cs.canMoveForward);
SetLightsCanMoveBackwards(cs.canMoveBackwards);
SetLightsCanTurnLeft(cs.canTurnLeft);
SetLightsCanTurnRight(cs.canTurnRight);
}
/// <summary>
/// get a subset of Kata's that can be executed given a CollisionState, optionally further filtered by names matching regular expression
/// </summary>
/// <param name="collisionState"></param>
/// <param name="kataGroupName"></param>
/// <returns></returns>
public static IEnumerable <Kata> KataByCollisionState(CollisionState collisionState, string kataGroupName)
{
List <Kata> ret = new List <Kata>();
if (collisionState == null)
{
return(ret);
}
if (collisionState.canMoveBackwards && collisionState.canMoveBackwardsDistanceMm > 500)
{
// have some room in the back, try some diagonal moves:
if (collisionState.canTurnRight)
{
ret.AddRange(from k in katas
where Regex.Match(k.name, kataGroupName + " .*LeftBack.*").Success
select k);
}
if (collisionState.canTurnLeft)
{
ret.AddRange(from k in katas
where Regex.Match(k.name, kataGroupName + " .*RightBack.*").Success
select k);
}
}
else
{
// can't move back; try the sides:
if (collisionState.canTurnRight)
{
ret.AddRange(from k in katas
where Regex.Match(k.name, kataGroupName + " .*LeftSide.*").Success
select k);
}
if (collisionState.canTurnLeft)
{
ret.AddRange(from k in katas
where Regex.Match(k.name, kataGroupName + " .*RightSide.*").Success
select k);
}
}
return(ret);
}
/// <summary>
/// Based on distanceToObstacle calculates canMoveForward, canMoveBackwards and related distances and speeds
/// </summary>
/// <param name="distanceToObstacleMeters"></param>
/// <param name="isForward"></param>
public void adjustNearCollisionSpeed(double distanceToObstacleMeters, bool isForward)
{
int canMoveDistanceMm = (int)(distanceToObstacleMeters * 1000.0d);
double canMoveSpeedMms = canMoveDistanceMm * speedFactor;
bool canMove = canMoveSpeedMms > speedTresholdMms;
CollisionState cs = _state.collisionState;
if (isForward)
{
cs.canMoveForwardDistanceMm = canMoveDistanceMm;
cs.canMoveForwardSpeedMms = Math.Min(canMoveSpeedMms, MaximumForwardVelocityMmSec);
cs.canMoveForward = canMove;
}
else
{
cs.canMoveBackwardsDistanceMm = canMoveDistanceMm;
cs.canMoveBackwardsSpeedMms = Math.Min(canMoveSpeedMms, MaximumBackwardVelocityMmSec);
cs.canMoveBackwards = canMove;
}
}
protected IEnumerator <ITask> KataRunner(Kata kata, Handler onComplete)
{
LogInfo("DriveBehaviorServiceBase: KataRunner(" + kata.name + ") Started" + currentCompass);
kata.success = false;
kata.successfulStepsCount = 0;
bool lastOpSuccess = false;
_state.MovingState = MovingState.InTransition;
// onComplete handler may set MovingState to whatever appropriate. We can set MovingState.Unknown on any error or interruption, and at the end to tamp.desiredMovingState
lastInTransitionStarted = DateTime.Now;
foreach (KataStep kataStep in kata)
{
LogInfo("IP: KataRunner(" + kata.name + ") started step " + (kata.successfulStepsCount + 1) + " " + kataStep.name + currentCompass);
kataStep.success = false;
Fault fault = null;
int rotateAngle = kataStep.rotateAngle;
int distance = kataStep.distance;
if (Math.Abs(rotateAngle) > 1) // "rotate" step
{
CollisionState collisionState = _state.collisionState;
if (collisionState == null || !kataStep.CanPerform(collisionState))
{
LogInfo("Error: KataRunner cannot perform due to CollisionState - on turn");
break; // kata interrupted
}
LogInfo("IP: KataRunner Turn " + rotateAngle);
yield return(Arbiter.Choice(
TurnByAngle(rotateAngle, kataStep.rotatePower * PowerScale),
delegate(DefaultUpdateResponseType response)
{
LogInfo("IP: KataRunner TurnByAngle accepted" + currentCompass);
lastOpSuccess = true;
},
delegate(Fault f)
{
LogInfo("Error: KataRunner TurnByAngle rejected: " + fault);
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
fault = f;
}
));
// If the RotateDegrees was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
TrackRoamerBehaviorsState state = _state;
state.IsTurning = true; // can't trust laser while turning
state.LastTurnStarted = state.LastTurnCompleted = DateTime.Now; // reset watchdog
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("OK: WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
if (lastOpSuccess)
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
}
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogInfo("op failure");
state.MovingState = MovingState.Unknown;
}
state.IsTurning = false;
state.LastTurnCompleted = DateTime.Now;
// make sure we display zero power:
_mapperVicinity.robotState.leftPower = 0.0d;
_mapperVicinity.robotState.rightPower = 0.0d;
}
else
{
break;
}
}
if (Math.Abs(distance) > 1) // "translate" step
{
// while we were rotating, collisionState might have changed
CollisionState collisionState = _state.collisionState;
if (collisionState == null || !kataStep.CanPerform(collisionState))
{
LogInfo("Error: KataRunner cannot perform due to CollisionState - on translate");
break; // kata interrupted
}
LogInfo("IP: KataRunner Translate " + distance);
yield return(Arbiter.Choice(
Translate(distance, kataStep.speed * PowerScale),
delegate(DefaultUpdateResponseType response) { lastOpSuccess = true; },
delegate(Fault f)
{
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
fault = f;
}
));
// If the Translate was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
// make sure we display zero power:
_mapperVicinity.robotState.leftPower = 0.0d;
_mapperVicinity.robotState.rightPower = 0.0d;
}
if (!lastOpSuccess)
{
break;
}
}
LogInfo("KataRunner(" + kata.name + ") finished step=" + kataStep.name + currentCompass);
kata.successfulStepsCount++;
}
kata.success = kata.Count == kata.successfulStepsCount;
_state.MovingState = MovingState.Unknown; // that's for now, onComplete may set it to whatever appropriate
LogInfo("KataRunner - calling onComplete()");
onComplete(); // check kata.success, will be false if DriveStage.Cancel or other interruption occured.
// done
LogInfo("DriveBehaviorServiceBase: KataRunner(" + kata.name + ") finished" + currentCompass);
yield break;
}
public void Init()
{
Dropping = false;
IsTurning = false;
LastTurnStarted = DateTime.MinValue;
LastTurnCompleted = DateTime.MinValue;
if (WheelsEncoderState == null)
{
WheelsEncoderState = new WheelsEncoderState();
}
if (collisionState == null)
{
collisionState = new CollisionState();
}
if (gpsState == null)
{
gpsState = new GpsState();
}
if (MostRecentAnalogValues == null)
{
MostRecentAnalogValues = new proxibrick.AnalogDataDssSerializable()
{
TimeStamp = DateTime.MinValue
};
}
MostRecentLaserTimeStamp = DateTime.Now;
if (VoiceCommandState == null)
{
VoiceCommandState = new VoiceCommandState();
}
int MAX_HUMANS_TO_TRACK = 7; // FrameProcessor preallocates 7
HumanInteractionStates = new HumanInteractionState[MAX_HUMANS_TO_TRACK];
for (int i = 0; i < HumanInteractionStates.Length; i++)
{
HumanInteractionStates[i] = new HumanInteractionState();
}
if (followDirectionPidControllerAngularSpeed == null)
{
followDirectionPidControllerAngularSpeed = new PIDController()
{
Name = "AngularSpeed",
MaxIntegralError = 180.0d, // degrees; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 100.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (followDirectionPidControllerLinearSpeed == null)
{
followDirectionPidControllerLinearSpeed = new PIDController()
{
Name = "LinearSpeed",
MaxIntegralError = 2000.0d, // mm/sec; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 1000.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (PowerScale == 0.0d)
{
PowerScale = 0.5d;
}
}
/// <summary>
/// evaluates proximity and parking sensor data and translates all of it into
/// actionable restrictions in CollisionState object
/// </summary>
public void computeCollisionState(RoutePlan plan, double?intendedVelocity)
{
CollisionState cs = _state.collisionState;
//cs.initRestrictive();
cs.initPermissive(FreeDistanceMm, MaximumForwardVelocityMmSec, MaximumBackwardVelocityMmSec);
// initialize to the maximums:
canMoveForwardDistanceM = FreeDistanceMm * 2.0d / 1000.0d;
canMoveBackwardsDistanceM = FreeDistanceMm * 2.0d / 1000.0d;
//Tracer.Trace("IR ffl=" + _state.MostRecentProximity.mfl + "m PS MetersLF=" + _state.MostRecentParkingSensor.parkingSensorMetersLF + "m");
if (intendedVelocity == null)
{
intendedVelocity = _state.Velocity;
}
bool isMovingForward = intendedVelocity > 0.01d; // m/s
bool isMovingBackwards = intendedVelocity < -0.01d;
int?distance = null; // mm
if (plan != null && plan.isGoodPlan)
{
double?bestHeadingRelative = plan.bestHeadingRelative(_mapperVicinity);
if (bestHeadingRelative != null && bestHeadingRelative.Value > -45.0d && bestHeadingRelative.Value < 45.0d)
{
distance = (int)(plan.legMeters.Value * 1000.0d);
if (distance < ObstacleDistanceMm)
{
cs.canMoveByPlan = false;
}
else
{
cs.canMoveByPlan = true;
}
}
}
DateTime sensorInvalidateTime = DateTime.Now.AddSeconds(-ignoreOldSensorReadingsIntervalSec); // ignore sensor readings that are older than this time
StringBuilder msb = new StringBuilder(); // build the message here
// we compute full state every time, but format the message so it is relevant to the direction we move to.
// forward sensors: -------------------------------------------------------------------------------------------
proxibrick.ProximityDataDssSerializable MostRecentProximity = _state.MostRecentProximity;
proxibrick.ParkingSensorDataDssSerializable MostRecentParkingSensor = _state.MostRecentParkingSensor;
if (MostRecentProximity != null && MostRecentProximity.TimeStamp > sensorInvalidateTime)
{
double mfl = MostRecentProximity.mfl;
if (mfl < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Left Side proximity detected obstacle at {0}m; ", mfl);
}
cs.canTurnLeft &= mfl > proxiTresholdTurn1;
if (mfl < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mfl;
}
}
double mffl = MostRecentProximity.mffl;
if (mffl < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("FF_Left proximity detected obstacle at {0}m; ", mffl);
}
//cs.canTurnLeft &= mffl > proxiTresholdTurn2;
if (mffl < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mffl;
}
}
double mffr = MostRecentProximity.mffr;
if (mffr < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("FF_Right proximity detected obstacle at {0}m; ", mffr);
}
//cs.canTurnRight &= mffr > proxiTresholdTurn2;
if (mffr < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mffr;
}
}
double mfr = MostRecentProximity.mfr;
if (mfr < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Right Side proximity detected obstacle at {0}m; ", mfr);
}
cs.canTurnRight &= mfr > proxiTresholdTurn1;
if (mfr < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mfr;
}
}
}
if (MostRecentParkingSensor != null && MostRecentParkingSensor.TimeStamp > sensorInvalidateTime)
{
double parkingSensorMetersLF = MostRecentParkingSensor.parkingSensorMetersLF;
if (parkingSensorMetersLF < psiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Left parking sensor detected obstacle at {0}m; ", parkingSensorMetersLF);
}
if (parkingSensorMetersLF < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = parkingSensorMetersLF;
}
}
double parkingSensorMetersRF = MostRecentParkingSensor.parkingSensorMetersRF;
if (parkingSensorMetersRF < psiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Right parking sensor detected obstacle at {0}m; ", parkingSensorMetersRF);
}
if (parkingSensorMetersRF < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = parkingSensorMetersRF;
}
}
}
// backward sensors: -------------------------------------------------------------------------------------------
if (MostRecentProximity != null && MostRecentProximity.TimeStamp > sensorInvalidateTime)
{
double mbl = MostRecentProximity.mbl;
if (mbl < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Left Side proximity detected obstacle at {0}m; ", mbl);
}
cs.canTurnRight &= mbl > proxiTresholdTurn1;
if (mbl < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbl;
}
}
double mbbl = MostRecentProximity.mbbl;
if (mbbl < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("BB_Left proximity detected obstacle at {0}m; ", mbbl);
}
//cs.canTurnRight &= mbbl > proxiTresholdTurn2;
if (mbbl < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbbl;
}
}
double mbbr = MostRecentProximity.mbbr;
if (mbbr < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("BB_Right proximity detected obstacle at {0}m; ", mbbr);
}
//cs.canTurnLeft &= mbbr > proxiTresholdTurn2;
if (mbbr < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbbr;
}
}
double mbr = MostRecentProximity.mbr;
if (mbr < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Right Side proximity detected obstacle at {0}m; ", mbr);
}
cs.canTurnLeft &= mbr > proxiTresholdTurn1;
if (mbr < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbr;
}
}
}
if (MostRecentParkingSensor != null && MostRecentParkingSensor.TimeStamp > sensorInvalidateTime)
{
double parkingSensorMetersLB = MostRecentParkingSensor.parkingSensorMetersLB;
if (parkingSensorMetersLB < psiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Left parking sensor detected obstacle at {0}m; ", parkingSensorMetersLB);
}
if (parkingSensorMetersLB < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = parkingSensorMetersLB;
}
}
double parkingSensorMetersRB = MostRecentParkingSensor.parkingSensorMetersRB;
if (parkingSensorMetersRB < psiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Right parking sensor detected obstacle at {0}m; ", parkingSensorMetersRB);
}
if (parkingSensorMetersRB < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = parkingSensorMetersRB;
}
}
}
if (_state.MostRecentWhiskerLeft)
{
msb.Append("Left Whisker; ");
canMoveForwardDistanceM = 0.0d;
cs.canMoveForward = false;
cs.canTurnRight = false;
cs.canTurnLeft = false;
}
if (_state.MostRecentWhiskerRight)
{
msb.Append("Right Whisker; ");
canMoveForwardDistanceM = 0.0d;
cs.canMoveForward = false;
cs.canTurnRight = false;
cs.canTurnLeft = false;
}
adjustNearCollisionSpeed(canMoveForwardDistanceM, true);
adjustNearCollisionSpeed(canMoveBackwardsDistanceM, false);
// compute the "mustStop" and "message":
if (isMovingForward)
{
cs.mustStop = !cs.canMoveForward;
}
else if (isMovingBackwards)
{
cs.mustStop = !cs.canMoveBackwards;
}
if (msb.Length > 0 || cs.mustStop)
{
if (isMovingForward)
{
msb.Insert(0, "While moving forward: ");
}
else if (isMovingBackwards)
{
msb.Insert(0, "While moving backwards: ");
}
if (cs.mustStop)
{
msb.Insert(0, "!! MUST STOP !! ");
}
}
else
{
if (isMovingForward)
{
msb.Append("Can continue forward: ");
}
else if (isMovingBackwards)
{
msb.Append("Can continue backwards: ");
}
else
{
msb.Append("Can move: ");
}
}
msb.AppendFormat(" FWD: {0} to {1} mm at {2:F0} mm/s BKWD: {3} to {4} mm at {5:F0} mm/s LEFT:{6} RIGHT:{7}",
cs.canMoveForward, cs.canMoveForwardDistanceMm, cs.canMoveForwardSpeedMms,
cs.canMoveBackwards, cs.canMoveBackwardsDistanceMm, cs.canMoveBackwardsSpeedMms,
cs.canTurnLeft, cs.canTurnRight);
if (cs.canMoveByPlan.HasValue && !cs.canMoveByPlan.Value)
{
msb.AppendFormat("; best plan obstacle too close at {0} mm; ", distance);
}
string message = msb.ToString();
//if (!string.Equals(message, _lastMessage))
//{
// Tracer.Trace(message);
// _lastMessage = message;
//}
cs.message = message;
}
/// <summary>
/// compute and set all FollowDirection parameters based on polar coordinates of target
/// </summary>
/// <param name="distanceToHumanMeters"></param>
/// <param name="targetPanRelativeToRobot"></param>
/// <param name="toleranceMeters">positive</param>
/// <param name="toleranceDegrees">positive</param>
private void ComputeMovingVelocity(double distanceToHumanMeters, double targetPanRelativeToRobot, double toleranceMeters, double toleranceDegrees)
{
//Tracer.Trace("++++++ ComputeMovingVelocity() distanceToHumanMeters=" + distanceToHumanMeters + " targetPanRelativeToRobot=" + targetPanRelativeToRobot);
setCurrentGoalBearingRelativeToRobot(targetPanRelativeToRobot);
double distanceToCoverMeters = distanceToHumanMeters - TargetDistanceToGoalMeters;
// see if we are OK with just keeping the current position or heading:
bool positionOk = Math.Abs(distanceToCoverMeters) < toleranceMeters;
bool headingOk = Math.Abs(targetPanRelativeToRobot) < toleranceDegrees;
bool intendToStay = positionOk && headingOk; // do not move
if (intendToStay)
{
// within the margin from target and almost pointed to it.
// this is dead zone - we don't want to jerk around the desired distance and small angle:
SayState("keeping - target at " + Math.Round(targetPanRelativeToRobot));
//setCurrentGoalDistance(TargetDistanceToGoalMeters); // let PID think we've reached the target
_mapperVicinity.robotState.robotTacticsType = RobotTacticsType.None;
return;
}
// we need to move - maybe turn in place. See if we can move at all.
bool canMove = PerformAvoidCollision(null, positionOk ? null : (double?)Math.Sign(distanceToCoverMeters)); // make sure CollisionState is computed if we want to move. Use velocity +-1.0 to communicate direction.
CollisionState collisionState = _state.collisionState;
if (positionOk && !headingOk) // just turn
{
FollowDirectionMaxVelocityMmSec = MinimumForwardVelocityMmSec;
if (targetPanRelativeToRobot > 0.0d && collisionState.canTurnRight)
{
SayState("Turn right in place " + Math.Round(targetPan));
FollowDirectionMaxVelocityMmSec = MaximumForwardVelocityMmSec;
_mapperVicinity.robotState.robotTacticsType = RobotTacticsType.FollowDirection;
setCurrentGoalDistance(distanceToHumanMeters);
return;
}
else if (targetPanRelativeToRobot < 0.0d && collisionState.canTurnLeft)
{
SayState("Turn left in place " + Math.Round(-targetPan));
FollowDirectionMaxVelocityMmSec = MaximumForwardVelocityMmSec;
_mapperVicinity.robotState.robotTacticsType = RobotTacticsType.FollowDirection;
setCurrentGoalDistance(distanceToHumanMeters);
return;
}
else
{
SayState("Turn blocked");
_mapperVicinity.robotState.robotTacticsType = RobotTacticsType.None;
return;
}
}
if (!canMove)
{
SayState("movement blocked");
StopMoving();
_state.MovingState = MovingState.Unable;
_state.Countdown = 0; // 0 = immediate response
_mapperVicinity.robotState.robotTacticsType = RobotTacticsType.None;
return;
}
if (distanceToCoverMeters >= 0.0d)
{
SayState(collisionState.canMoveForward ? ("Forward " + Math.Round(distanceToCoverMeters, 1)) : "Forward Blocked");
FollowDirectionMaxVelocityMmSec = Math.Min(MaximumForwardVelocityMmSec, collisionState.canMoveForwardSpeedMms);
//Tracer.Trace("++ fwd: FollowDirectionMaxVelocityMmSec = " + FollowDirectionMaxVelocityMmSec);
_mapperVicinity.robotState.robotTacticsType = collisionState.canMoveForward ? RobotTacticsType.FollowDirection : RobotTacticsType.None;
setCurrentGoalDistance(distanceToHumanMeters);
}
else if (distanceToCoverMeters <= 0.0d)
{
FollowDirectionMaxVelocityMmSec = Math.Min(MaximumBackwardVelocityMmSec, collisionState.canMoveBackwardsSpeedMms);
SayState(collisionState.canMoveBackwards ? ("Backwards " + Math.Round(distanceToCoverMeters, 1)) : "Backwards Blocked");
//SayState(collisionState.canMoveBackwards ? ("Backwards " + Math.Round(distanceToCoverMeters, 1) + " velocity " + FollowDirectionMaxVelocityMmSec + " human at " + Math.Round(distanceToHumanMeters, 1)) : "Backwards Blocked");
_mapperVicinity.robotState.robotTacticsType = collisionState.canMoveBackwards ? RobotTacticsType.FollowDirection : RobotTacticsType.None;
setCurrentGoalDistance(distanceToHumanMeters);
}
//Tracer.Trace("FollowDirectionMaxVelocityMmSec = " + FollowDirectionMaxVelocityMmSec);
}
public bool CanPerform(CollisionState collisionState)
{
return(true);
}
public void Init()
{
Dropping = false;
IsTurning = false;
LastTurnStarted = DateTime.MinValue;
LastTurnCompleted = DateTime.MinValue;
if (WheelsEncoderState == null)
{
WheelsEncoderState = new WheelsEncoderState();
}
if (collisionState == null)
{
collisionState = new CollisionState();
}
if (gpsState == null)
{
gpsState = new GpsState();
}
if (MostRecentAnalogValues == null)
{
MostRecentAnalogValues = new proxibrick.AnalogDataDssSerializable() { TimeStamp = DateTime.MinValue };
}
MostRecentLaserTimeStamp = DateTime.Now;
if (VoiceCommandState == null)
{
VoiceCommandState = new VoiceCommandState();
}
int MAX_HUMANS_TO_TRACK = 7; // FrameProcessor preallocates 7
HumanInteractionStates = new HumanInteractionState[MAX_HUMANS_TO_TRACK];
for (int i = 0; i < HumanInteractionStates.Length; i++)
{
HumanInteractionStates[i] = new HumanInteractionState();
}
if (followDirectionPidControllerAngularSpeed == null)
{
followDirectionPidControllerAngularSpeed = new PIDController()
{
Name = "AngularSpeed",
MaxIntegralError = 180.0d, // degrees; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 100.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (followDirectionPidControllerLinearSpeed == null)
{
followDirectionPidControllerLinearSpeed = new PIDController()
{
Name = "LinearSpeed",
MaxIntegralError = 2000.0d, // mm/sec; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 1000.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (PowerScale == 0.0d)
{
PowerScale = 0.5d;
}
}
