/*
* Coursera formulas for reference:
*
* function [vel_r,vel_l] = uni_to_diff(obj,v,w)
* R = obj.wheel_radius; % 31.9mm
* L = obj.wheel_base_length; % 99.25mm
*
* vel_r = (2 * v + w * L) / 2 * R;
* vel_l = (2 * v - w * L) / 2 * R;
* end
*
* function [v,w] = diff_to_uni(obj,r,l)
* R = obj.wheel_radius;
* L = obj.wheel_base_length;
*
* v = R/2*(r+l);
* w = R/L*(r-l);
* end
*
*
* %% Output shaping - adjusting linear velocity to preserve turn velocity (omega):
*
* function [vel_r, vel_l] = ensure_w(obj, robot, v, w)
*
* % This function ensures that w is respected as best as possible
* % by scaling v.
*
* R = robot.wheel_radius;
* L = robot.wheel_base_length;
*
* vel_max = robot.max_vel;
* vel_min = robot.min_vel;
*
* % fprintf('IN (v,w) = (%0.3f,%0.3f)\n', v, w);
*
* if (abs(v) > 0)
* % 1. Limit v,w to be possible in the range [vel_min, vel_max]
* % (avoid stalling or exceeding motor limits)
* v_lim = max(min(abs(v), (R/2)*(2*vel_max)), (R/2)*(2*vel_min));
* w_lim = max(min(abs(w), (R/L)*(vel_max-vel_min)), 0);
*
* % 2. Compute the desired curvature of the robot's motion
*
* [vel_r_d, vel_l_d] = robot.dynamics.uni_to_diff(v_lim, w_lim);
*
* % 3. Find the max and min vel_r/vel_l
* vel_rl_max = max(vel_r_d, vel_l_d);
* vel_rl_min = min(vel_r_d, vel_l_d);
*
* % 4. Shift vel_r and vel_l if they exceed max/min vel
* if (vel_rl_max > vel_max)
* vel_r = vel_r_d - (vel_rl_max-vel_max);
* vel_l = vel_l_d - (vel_rl_max-vel_max);
* elseif (vel_rl_min < vel_min)
* vel_r = vel_r_d + (vel_min-vel_rl_min);
* vel_l = vel_l_d + (vel_min-vel_rl_min);
* else
* vel_r = vel_r_d;
* vel_l = vel_l_d;
* end
*
* % 5. Fix signs (Always either both positive or negative)
* [v_shift, w_shift] = robot.dynamics.diff_to_uni(vel_r, vel_l);
*
* v = sign(v)*v_shift;
* w = sign(w)*w_shift;
*
* else
* % Robot is stationary, so we can either not rotate, or
* % rotate with some minimum/maximum angular velocity
* w_min = R/L*(2*vel_min);
* w_max = R/L*(2*vel_max);
*
* if abs(w) > w_min
* w = sign(w)*max(min(abs(w), w_max), w_min);
* else
* w = 0;
* end
*
*
* end
*
* % fprintf('OUT (v,w) = (%0.3f,%0.3f)\n', v, w);
* [vel_r, vel_l] = robot.dynamics.uni_to_diff(v, w);
* end
*
*/
/// <summary>
/// Computes speedLeft and speedRight based on velocity and omega,
/// using Unicycle to Differential Drive formula and adjusting for maximum wheel speed
/// </summary>
/// <param name="geometry">contains wheel base and radius</param>
public override void Compute(IDriveGeometry g)
{
IDifferentialDriveGeometry geometry = g as IDifferentialDriveGeometry;
Debug.Assert(g != null);
double velocityMax = geometry.speedToVelocityFactor * 100.0d;
double omegaMax = geometry.turnToOmegaFactor * 100.0d;
double velocity2 = velocity * 2.0d;
double omegaL = omega * geometry.wheelBaseMeters; // positive - left
double R2 = 2.0 * geometry.wheelRadiusMeters;
double PIR2 = Math.PI * R2;
// the uni_to_diff formula gives rotational speed of the wheel:
int count = 0;
do
{
this.rotationLeftWheel = (velocity2 - omegaL) / R2;
this.rotationRightWheel = (velocity2 + omegaL) / R2;
this.velocityLeftWheel = this.rotationLeftWheel * R2 / 2.0d; // meters per second at the wheel edge (rim)
this.velocityRightWheel = this.rotationRightWheel * R2 / 2.0d;
// keep adjusting linear velocity to preserve turn velocity (omega):
velocity2 *= 0.8d;
}while ((this.velocityLeftWheel > velocityMax || this.velocityRightWheel > velocityMax) && count++ < 10);
}
public BehaviorGoToGoal(IDriveGeometry ddg)
: base(ddg)
{
BehaviorActivateCondition = bd =>
{
return distanceToGoal != null && directionToGoal != null;
};
BehaviorDeactivateCondition = bd =>
{
return distanceToGoal == null || directionToGoal == null || distanceToGoal.Meters < 0.2d;
};
}
public BehaviorGoToGoal(IDriveGeometry ddg)
: base(ddg)
{
BehaviorActivateCondition = bd =>
{
return(distanceToGoal != null && directionToGoal != null);
};
BehaviorDeactivateCondition = bd =>
{
return(distanceToGoal == null || directionToGoal == null || distanceToGoal.Meters < 0.2d);
};
}
//public IDirection directionToGoal { get { return goalGeoPosition == null ? null : new Direction() { heading = behaviorData.robotPose.direction.heading, bearing = behaviorData.robotPose.geoPosition.bearingTo(goalGeoPosition) }; } }
public BehaviorGoToPixy(IDriveGeometry ddg)
: base(ddg)
{
BehaviorActivateCondition = bd =>
{
pbd = pixyBearingDegrees; // save it for the loop
return pbd != null; // have colored object in Pixy Camera view
};
BehaviorDeactivateCondition = bd =>
{
return !behaviorData.sensorsData.IsTargetingCameraDataValid(); // no colored objects in Pixy Camera view
};
}
//public IDirection directionToGoal { get { return goalGeoPosition == null ? null : new Direction() { heading = behaviorData.robotPose.direction.heading, bearing = behaviorData.robotPose.geoPosition.bearingTo(goalGeoPosition) }; } }
public BehaviorGoToPixy(IDriveGeometry ddg)
: base(ddg)
{
BehaviorActivateCondition = bd =>
{
pbd = pixyBearingDegrees; // save it for the loop
return(pbd != null); // have colored object in Pixy Camera view
};
BehaviorDeactivateCondition = bd =>
{
return(!behaviorData.sensorsData.IsTargetingCameraDataValid()); // no colored objects in Pixy Camera view
};
}
private void InitDrive()
{
IDifferentialMotorController dmc = hardwareBrick.produceDifferentialMotorController();
driveController = new DifferentialDrive(hardwareBrick)
{
wheelRadiusMeters = WHEEL_RADIUS_METERS,
wheelBaseMeters = WHEEL_BASE_METERS,
encoderTicksPerRevolution = ENCODER_TICKS_PER_REVOLUTION,
speedToVelocityFactor = SPEED_TO_VELOCITY_FACTOR,
turnToOmegaFactor = TURN_TO_OMEGA_FACTOR,
differentialMotorController = dmc
};
driveGeometry = (IDifferentialDrive)driveController;
driveController.Init();
driveController.Enabled = true;
}
private void InitDrive()
{
IDifferentialMotorController dmc = hardwareBrick.produceDifferentialMotorController();
driveController = new DifferentialDrive(hardwareBrick)
{
wheelRadiusMeters = WHEEL_RADIUS_METERS,
wheelBaseMeters = WHEEL_BASE_METERS,
encoderTicksPerRevolution = ENCODER_TICKS_PER_REVOLUTION,
speedToVelocityFactor = SPEED_TO_VELOCITY_FACTOR,
turnToOmegaFactor = TURN_TO_OMEGA_FACTOR,
differentialMotorController = dmc
};
this.odometry = hardwareBrick.produceOdometry(ODOMETRY_SAMPLING_INTERVAL_MS);
this.odometry.OdometryChanged += ((SensorsControllerPlucky)sensorsController).ArduinoBrickOdometryChanged;
driveController.hardwareBrickOdometry = odometry;
driveGeometry = (IDifferentialDrive)driveController;
driveController.Init();
driveController.Enabled = true;
}
public BehaviorFollowWall(IDriveGeometry ddg)
: base(ddg)
{
BehaviorActivateCondition = bd =>
{
if ((DateTime.Now - deactivatedLast).TotalSeconds < 2.0d)
{
return(false); // dead zone after deactivation for heading
}
//if (BehaviorBase.getCoordinatorData().EnablingRequest.StartsWith("FollowWall"))
{
double irLeftMeters = bd.sensorsData.IrLeftMeters;
double sonarLeftMeters = behaviorData.sensorsData.RangerFrontLeftMeters;
double irRightMeters = bd.sensorsData.IrRightMeters;
double sonarRightMeters = behaviorData.sensorsData.RangerFrontRightMeters;
double activateDistanceToWallMeters = distanceToWallMeters * 0.9d;
if (irLeftMeters < activateDistanceToWallMeters || (irLeftMeters < distanceToWallMeters && sonarLeftMeters < distanceToWallMeters))
{
fireOnLeft = true;
}
if (irRightMeters < activateDistanceToWallMeters || (irRightMeters < distanceToWallMeters && sonarRightMeters < distanceToWallMeters))
{
fireOnRight = true;
}
return(fireOnLeft || fireOnRight);
}
//return false;
};
BehaviorDeactivateCondition = bd =>
{
//return (DateTime.Now - started).TotalSeconds > 6.0d;
double?goalBearing = bd.robotState.goalBearingDegrees;
if (goalBearing.HasValue && bd.sensorsData.CompassHeadingDegrees.HasValue) // behaviorData.robotPose.direction.heading ?
{
double heading = bd.sensorsData.CompassHeadingDegrees.Value;
if (Math.Abs(heading - goalBearing.Value) < 10.0d)
{
deactivatedLast = DateTime.Now;
return(true);
}
}
deactivatedLast = DateTime.MinValue;
double irLeftMeters = bd.sensorsData.IrLeftMeters;
double irRightMeters = bd.sensorsData.IrRightMeters;
double deactivateDistanceToWallMeters = distanceToWallMeters * 2.0d;
bool noWall = lostTheWall(irLeftMeters, irRightMeters, deactivateDistanceToWallMeters);
// "lost the wall" condition:
if (noWall && lostTheWallLast == DateTime.MinValue)
{
// just lost it, set timer:
lostTheWallLast = DateTime.Now;
}
else if (!noWall)
{
// acquired the wall, reset timer interval:
lostTheWallLast = DateTime.MinValue;
}
return(noWall && (DateTime.Now - lostTheWallLast).TotalSeconds > 2.0d); // no wall for some time
};
}
/// <summary>
/// Computes physical drive parameters based on velocity and omega,
/// using Unicycle to specific Drive formula and adjusting for maximum speed
/// </summary>
/// <param name="geometry">contains wheel base and radius or similar drive characteristics</param>
public virtual void Compute(IDriveGeometry g)
{
throw new NotImplementedException("must implement DriveInputsBase:Compute() when calling it.");
}
/// <summary>
///
/// </summary>
/// <param name="ddg"></param>
/// <param name="speaker"></param>
/// <param name="trackFileName">can be null, for a saved track</param>
public BehaviorRouteFollowing(IDriveGeometry ddg, ISpeaker speaker, string trackFileName, double powerFactor = 1.0d)
: base(ddg)
{
this.speaker = speaker;
this.powerFactor = powerFactor;
BehaviorActivateCondition = bd =>
{
return(nextWp != null);
};
BehaviorDeactivateCondition = bd =>
{
return(nextWp == null);
};
if (String.IsNullOrWhiteSpace(trackFileName))
{
//speaker.Speak("Loading saved track");
try
{
missionTrack = null;
// Load stored waypoints:
// on the PC, from C:\Users\sergei\AppData\Local\Packages\RobotPluckyPackage_sjh4qacv6p1wm\LocalState\MyTrack.xml
// RPi: \\172.16.1.175\c$\Data\Users\DefaultAccount\AppData\Local\Packages\RobotPluckyPackage_sjh4qacv6p1wm\LocalState
Track track = SerializableStorage <Track> .Load(savedTrackFileName).Result;
if (track != null)
{
missionTrack = track;
//speaker.Speak("Loaded file " + missionTrack.Count + " trackpoints");
}
}
catch (Exception ex)
{
speaker.Speak("could not load saved track file");
}
if (missionTrack == null)
{
speaker.Speak("failed to load saved track file");
missionTrack = new Track();
}
nextWp = missionTrack.nextTargetWp;
stopStartedTime = null;
}
else
{
speaker.Speak("Loading file " + trackFileName);
missionTrack = new Track();
try
{
missionTrack.Init(trackFileName);
speaker.Speak("Loaded file " + missionTrack.Count + " trackpoints");
nextWp = missionTrack.nextTargetWp;
}
catch (Exception ex)
{
speaker.Speak("could not load planned track file");
}
}
}
/// <summary>
///
/// </summary>
/// <param name="ddg"></param>
/// <param name="speaker"></param>
/// <param name="trackFileName">can be null, for a saved track</param>
public BehaviorRouteFollowing(IDriveGeometry ddg, ISpeaker speaker, string trackFileName)
: base(ddg)
{
this.speaker = speaker;
BehaviorActivateCondition = bd =>
{
return nextWp != null;
};
BehaviorDeactivateCondition = bd =>
{
return nextWp == null;
};
if (String.IsNullOrWhiteSpace(trackFileName))
{
speaker.Speak("Loading saved track");
try
{
missionTrack = null;
// Load stored waypoints:
// on the PC, from C:\Users\sergei\AppData\Local\Packages\RobotPluckyPackage_sjh4qacv6p1wm\LocalState\MyTrack.xml
// RPi: \\172.16.1.175\c$\Data\Users\DefaultAccount\AppData\Local\Packages\RobotPluckyPackage_sjh4qacv6p1wm\LocalState
Track track = SerializableStorage<Track>.Load(savedTrackFileName).Result;
if (track != null)
{
missionTrack = track;
speaker.Speak("Loaded file " + missionTrack.Count + " trackpoints");
}
}
catch (Exception ex)
{
speaker.Speak("could not load saved track file");
}
if(missionTrack == null)
{
speaker.Speak("failed to load saved track file");
missionTrack = new Track();
}
nextWp = missionTrack.nextTargetWp;
}
else
{
speaker.Speak("Loading file " + trackFileName);
missionTrack = new Track();
try
{
missionTrack.Init(trackFileName);
speaker.Speak("Loaded file " + missionTrack.Count + " trackpoints");
nextWp = missionTrack.nextTargetWp;
}
catch (Exception ex)
{
speaker.Speak("could not load planned track file");
}
}
}
private void InitDrive()
{
IDifferentialMotorController dmc = produceDifferentialMotorController();
IDifferentialDrive ddc = new DifferentialDrive(hardwareBrick)
{
wheelRadiusMeters = WHEEL_RADIUS_METERS,
wheelBaseMeters = WHEEL_BASE_METERS,
encoderTicksPerRevolution = ENCODER_TICKS_PER_REVOLUTION,
speedToVelocityFactor = SPEED_TO_VELOCITY_FACTOR,
turnToOmegaFactor = TURN_TO_OMEGA_FACTOR,
differentialMotorController = dmc
};
driveController = ddc;
driveGeometry = ddc;
driveController.Init();
}
public BehaviorFactory(SubsumptionTaskDispatcher disp, IDriveGeometry driveGeom, ISpeaker speaker)
{
this.subsumptionDispatcher = disp;
this.driveGeometry = driveGeom;
this.speaker = speaker;
}
/*
* Coursera formulas for reference:
*
function [vel_r,vel_l] = uni_to_diff(obj,v,w)
R = obj.wheel_radius; % 31.9mm
L = obj.wheel_base_length; % 99.25mm
vel_r = (2 * v + w * L) / 2 * R;
vel_l = (2 * v - w * L) / 2 * R;
end
function [v,w] = diff_to_uni(obj,r,l)
R = obj.wheel_radius;
L = obj.wheel_base_length;
v = R/2*(r+l);
w = R/L*(r-l);
end
*
*
%% Output shaping - adjusting linear velocity to preserve turn velocity (omega):
function [vel_r, vel_l] = ensure_w(obj, robot, v, w)
% This function ensures that w is respected as best as possible
% by scaling v.
R = robot.wheel_radius;
L = robot.wheel_base_length;
vel_max = robot.max_vel;
vel_min = robot.min_vel;
% fprintf('IN (v,w) = (%0.3f,%0.3f)\n', v, w);
if (abs(v) > 0)
% 1. Limit v,w to be possible in the range [vel_min, vel_max]
% (avoid stalling or exceeding motor limits)
v_lim = max(min(abs(v), (R/2)*(2*vel_max)), (R/2)*(2*vel_min));
w_lim = max(min(abs(w), (R/L)*(vel_max-vel_min)), 0);
% 2. Compute the desired curvature of the robot's motion
[vel_r_d, vel_l_d] = robot.dynamics.uni_to_diff(v_lim, w_lim);
% 3. Find the max and min vel_r/vel_l
vel_rl_max = max(vel_r_d, vel_l_d);
vel_rl_min = min(vel_r_d, vel_l_d);
% 4. Shift vel_r and vel_l if they exceed max/min vel
if (vel_rl_max > vel_max)
vel_r = vel_r_d - (vel_rl_max-vel_max);
vel_l = vel_l_d - (vel_rl_max-vel_max);
elseif (vel_rl_min < vel_min)
vel_r = vel_r_d + (vel_min-vel_rl_min);
vel_l = vel_l_d + (vel_min-vel_rl_min);
else
vel_r = vel_r_d;
vel_l = vel_l_d;
end
% 5. Fix signs (Always either both positive or negative)
[v_shift, w_shift] = robot.dynamics.diff_to_uni(vel_r, vel_l);
v = sign(v)*v_shift;
w = sign(w)*w_shift;
else
% Robot is stationary, so we can either not rotate, or
% rotate with some minimum/maximum angular velocity
w_min = R/L*(2*vel_min);
w_max = R/L*(2*vel_max);
if abs(w) > w_min
w = sign(w)*max(min(abs(w), w_max), w_min);
else
w = 0;
end
end
% fprintf('OUT (v,w) = (%0.3f,%0.3f)\n', v, w);
[vel_r, vel_l] = robot.dynamics.uni_to_diff(v, w);
end
*/
/// <summary>
/// Computes speedLeft and speedRight based on velocity and omega,
/// using Unicycle to Differential Drive formula and adjusting for maximum wheel speed
/// </summary>
/// <param name="geometry">contains wheel base and radius</param>
public override void Compute(IDriveGeometry g)
{
IDifferentialDriveGeometry geometry = g as IDifferentialDriveGeometry;
Debug.Assert(g != null);
double velocityMax = geometry.speedToVelocityFactor * 100.0d;
double omegaMax = geometry.turnToOmegaFactor * 100.0d;
double velocity2 = velocity * 2.0d;
double omegaL = omega * geometry.wheelBaseMeters; // positive - left
double R2 = 2.0 * geometry.wheelRadiusMeters;
double PIR2 = Math.PI * R2;
// the uni_to_diff formula gives rotational speed of the wheel:
int count = 0;
do
{
this.rotationLeftWheel = (velocity2 - omegaL) / R2;
this.rotationRightWheel = (velocity2 + omegaL) / R2;
this.velocityLeftWheel = this.rotationLeftWheel * R2 / 2.0d; // meters per second at the wheel edge (rim)
this.velocityRightWheel = this.rotationRightWheel * R2 / 2.0d;
// keep adjusting linear velocity to preserve turn velocity (omega):
velocity2 *= 0.8d;
}
while ((this.velocityLeftWheel > velocityMax || this.velocityRightWheel > velocityMax) && count++ < 10);
}
/// <summary>
/// ballistic "Escape" behavior,
/// will set a grab and FiredOn when triggered.
/// </summary>
/// <param name="ddg"></param>
/// <param name="_doOnce"></param>
/// <param name="desiredEscapeSpeed">0...-100, negative</param>
/// <param name="desiredEscapeTurn">0...100, positive</param>
public BehaviorBackAndTurn(IDriveGeometry ddg, double desiredEscapeSpeed = -15.0d, double desiredEscapeTurn = 20.0d)
: base(ddg)
{
escapeSpeed = desiredEscapeSpeed;
escapeTurn = desiredEscapeTurn;
}
public BehaviorFactory(SubsumptionTaskDispatcher disp, IDriveGeometry driveGeom, ISpeaker speaker)
{
this.subsumptionDispatcher = disp;
this.driveGeometry = driveGeom;
this.speaker = speaker;
}
public BehaviorControlledByJoystick(IDriveGeometry ddg)
: base(ddg)
{
// behaviors can subscribe to control device events:
this.controlDeviceEvent += new EventHandler <ControlDeviceEventArgs>(Behavior_controlDeviceEvent);
}
public BehaviorAvoidObstacles(IDriveGeometry ddg)
: base(ddg)
{
}
public BehaviorBase(IDriveGeometry ddg)
: this()
{
driveGeometry = ddg;
}
private void InitDrive()
{
IDifferentialMotorController dmc = hardwareBrick.produceDifferentialMotorController();
driveController = new DifferentialDrive(hardwareBrick)
{
wheelRadiusMeters = WHEEL_RADIUS_METERS,
wheelBaseMeters = WHEEL_BASE_METERS,
encoderTicksPerRevolution = ENCODER_TICKS_PER_REVOLUTION,
speedToVelocityFactor = SPEED_TO_VELOCITY_FACTOR,
turnToOmegaFactor = TURN_TO_OMEGA_FACTOR,
differentialMotorController = dmc
};
this.odometry = hardwareBrick.produceOdometry(ODOMETRY_SAMPLING_INTERVAL_MS);
this.odometry.OdometryChanged += ((SensorsControllerPlucky)sensorsController).ArduinoBrickOdometryChanged;
this.gps = hardwareBrick.produceGps(GPS_SAMPLING_INTERVAL_MS);
this.gps.GpsPositionChanged += ((SensorsControllerPlucky)sensorsController).ArduinoBrickGpsChanged;
this.gps.Enabled = true;
driveController.hardwareBrickOdometry = odometry;
driveGeometry = (IDifferentialDrive)driveController;
driveController.Init();
driveController.Enabled = true;
}
/// <summary>
/// Computes physical drive parameters based on velocity and omega,
/// using Unicycle to specific Drive formula and adjusting for maximum speed
/// </summary>
/// <param name="geometry">contains wheel base and radius or similar drive characteristics</param>
public virtual void Compute(IDriveGeometry g)
{
throw new NotImplementedException("must implement DriveInputsBase:Compute() when calling it.");
}
public BehaviorBase(IDriveGeometry ddg)
: this()
{
driveGeometry = ddg;
}
private double cruiseTurn; // -100...100, positive - right
/// <summary>
/// just go forward - non-grabbing, always FiredOn behavior
/// </summary>
/// <param name="ddg">IDriveGeometry</param>
/// <param name="desiredCruiseSpeed">-100...100</param>
/// <param name="desiredCruiseTurn">-100...100, positive - right; we can cruise in a circle</param>
public BehaviorCruise(IDriveGeometry ddg, double desiredCruiseSpeed = 20.0d, double desiredCruiseTurn = 0.0d)
: base(ddg)
{
cruiseSpeed = desiredCruiseSpeed;
cruiseTurn = desiredCruiseTurn;
}
public BehaviorControlledByJoystick(IDriveGeometry ddg)
: base(ddg)
{
// behaviors can subscribe to control device events:
this.controlDeviceEvent += new EventHandler<ControlDeviceEventArgs>(Behavior_controlDeviceEvent);
}
/// <summary>
/// just go forward towards a compass bearing - non-grabbing, always FiredOn behavior
/// </summary>
/// <param name="ddg">IDriveGeometry</param>
/// <param name="desiredCruiseSpeed">-100...100</param>
/// <param name="desiredTurnFactor">how aggressively we turn towards goal</param>
public BehaviorGoToAngle(IDriveGeometry ddg, double desiredCruiseSpeed = 20.0d, double desiredTurnFactor = 20.0d)
: base(ddg)
{
cruiseSpeed = desiredCruiseSpeed;
turnFactor = desiredTurnFactor;
}
/// <summary>
/// just go forward towards a compass bearing - non-grabbing, always FiredOn behavior
/// </summary>
/// <param name="ddg">IDriveGeometry</param>
/// <param name="desiredCruiseSpeed">-100...100</param>
/// <param name="desiredTurnFactor">how aggressively we turn towards goal</param>
public BehaviorGoToAngle(IDriveGeometry ddg, double desiredCruiseSpeed = 20.0d, double desiredTurnFactor = 20.0d)
: base(ddg)
{
cruiseSpeed = desiredCruiseSpeed;
turnFactor = desiredTurnFactor;
}
private double cruiseTurn; // -100...100, positive - right
/// <summary>
/// just go forward - non-grabbing, always FiredOn behavior
/// </summary>
/// <param name="ddg">IDriveGeometry</param>
/// <param name="desiredCruiseSpeed">-100...100</param>
/// <param name="desiredCruiseTurn">-100...100, positive - right; we can cruise in a circle</param>
public BehaviorCruise(IDriveGeometry ddg, double desiredCruiseSpeed = 20.0d, double desiredCruiseTurn = 0.0d)
: base(ddg)
{
cruiseSpeed = desiredCruiseSpeed;
cruiseTurn = desiredCruiseTurn;
}
public BehaviorFollowWall(IDriveGeometry ddg)
: base(ddg)
{
BehaviorActivateCondition = bd =>
{
if ((DateTime.Now - deactivatedLast).TotalSeconds < 2.0d)
{
return false; // dead zone after deactivation for heading
}
//if (BehaviorBase.getCoordinatorData().EnablingRequest.StartsWith("FollowWall"))
{
double irLeftMeters = bd.sensorsData.IrLeftMeters;
double sonarLeftMeters = behaviorData.sensorsData.RangerFrontLeftMeters;
double irRightMeters = bd.sensorsData.IrRightMeters;
double sonarRightMeters = behaviorData.sensorsData.RangerFrontRightMeters;
double activateDistanceToWallMeters = distanceToWallMeters * 0.9d;
if (irLeftMeters < activateDistanceToWallMeters || (irLeftMeters < distanceToWallMeters && sonarLeftMeters < distanceToWallMeters))
{
fireOnLeft = true;
}
if (irRightMeters < activateDistanceToWallMeters || (irRightMeters < distanceToWallMeters && sonarRightMeters < distanceToWallMeters))
{
fireOnRight = true;
}
return fireOnLeft || fireOnRight;
}
//return false;
};
BehaviorDeactivateCondition = bd =>
{
//return (DateTime.Now - started).TotalSeconds > 6.0d;
double? goalBearing = bd.robotState.goalBearingDegrees;
if (goalBearing.HasValue && bd.sensorsData.CompassHeadingDegrees.HasValue) // behaviorData.robotPose.direction.heading ?
{
double heading = bd.sensorsData.CompassHeadingDegrees.Value;
if (Math.Abs(heading - goalBearing.Value) < 10.0d)
{
deactivatedLast = DateTime.Now;
return true;
}
}
deactivatedLast = DateTime.MinValue;
double irLeftMeters = bd.sensorsData.IrLeftMeters;
double irRightMeters = bd.sensorsData.IrRightMeters;
double deactivateDistanceToWallMeters = distanceToWallMeters * 2.0d;
bool noWall = lostTheWall(irLeftMeters, irRightMeters, deactivateDistanceToWallMeters);
// "lost the wall" condition:
if (noWall && lostTheWallLast == DateTime.MinValue)
{
// just lost it, set timer:
lostTheWallLast = DateTime.Now;
}
else if(!noWall)
{
// acquired the wall, reset timer interval:
lostTheWallLast = DateTime.MinValue;
}
return noWall && (DateTime.Now - lostTheWallLast).TotalSeconds > 2.0d; // no wall for some time
};
}
/// <summary>
/// ballistic "Escape" behavior,
/// will set a grab and FiredOn when triggered.
/// </summary>
/// <param name="ddg"></param>
/// <param name="_doOnce"></param>
/// <param name="desiredEscapeSpeed">0...-100, negative</param>
/// <param name="desiredEscapeTurn">0...100, positive</param>
public BehaviorBackAndTurn(IDriveGeometry ddg, double desiredEscapeSpeed = -15.0d, double desiredEscapeTurn = 20.0d)
: base(ddg)
{
escapeSpeed = desiredEscapeSpeed;
escapeTurn = desiredEscapeTurn;
}
public BehaviorAvoidObstacles(IDriveGeometry ddg)
: base(ddg)
{
}
