public override async void ControlDeviceCommand(string command)
{
//Debug.WriteLine("PluckyTheRobot: ControlDeviceCommand: " + command);
bool isDispatcherCommand = true; // if true, dispatcher will be forwarding command to behaviors
if (command.StartsWith("button") && previousButtonCommand != command) // avoid button commands repetitions
{
Debug.WriteLine("PluckyTheRobot: ControlDeviceCommand: " + command);
isDispatcherCommand = false; // no need for further command processing
switch (command)
{
case "button1": // "A" button - caution: screen side effects
terminatePreviousBehavior(command);
currentBehavior = BehaviorCompositionType.Escape;
behaviorFactory.produce(currentBehavior);
speaker.Speak("Escape");
break;
case "button2": // "B" button terminates current behavior
terminatePreviousBehavior(command);
behaviorFactory.produce(currentBehavior); // None - closes all active tasks in Dispatcher
break;
//case "button3": // "X" button
// break;
case "button4": // "Y" button
robotPose.resetXY();
//robotPose.resetRotation();
robotPose.direction = new Direction() { heading = currentSensorsData.CompassHeadingDegrees };
driveController.OdometryReset();
speaker.Speak("Reset X Y");
break;
case "button5": // "Left Bumper"
terminatePreviousBehavior(command);
currentTrack = new Track() { trackFileName = waypointsFileName };
waypointsIndex = 0;
currentBehavior = BehaviorCompositionType.JoystickAndStop;
behaviorFactory.produce(currentBehavior);
speaker.Speak("Joystick control");
break;
case "button6": // "Right Bumper"
terminatePreviousBehavior(command);
ComputeGoal(); // for CruiseAndStop - corner-to-corner
//currentBehavior = BehaviorCompositionType.CruiseAndStop;
currentBehavior = BehaviorCompositionType.ChaseColorBlob;
//currentBehavior = BehaviorCompositionType.AroundTheBlock;
behaviorFactory.produce(currentBehavior);
//speaker.Speak("Around The Block");
//speaker.Speak("Cruise control");
speaker.Speak("Chase color");
break;
// buttons 7 and 8 on RamblePad2 reserved for throttle up/down control. Not on Xbox360 controller.
case "button7": // "Back" to the left of the sphere
terminatePreviousBehavior(command);
// Load stored waypoints:
// from C:\Users\sergei\AppData\Local\Packages\RobotPluckyPackage_sjh4qacv6p1wm\LocalState\MyTrack.xml
currentBehavior = BehaviorCompositionType.RouteFollowing;
behaviorFactory.TrackFileName = null; // will be replaced with "MyTrack.xml" and serializer will be used.
behaviorFactory.produce(currentBehavior);
speaker.Speak("Saved trackpoints following");
break;
case "button8": // "Start" to the right of the sphere
terminatePreviousBehavior(command);
currentBehavior = BehaviorCompositionType.RouteFollowing;
behaviorFactory.TrackFileName = "ParkingLot1.waypoints";
behaviorFactory.produce(currentBehavior);
speaker.Speak("Planned trackpoints following");
break;
case "button9": // Left Joystick push
{
bool allowStaleGps = true;
bool allowOnlyGpsFix3D = false;
if (!allowStaleGps && (DateTime.Now - new DateTime(currentSensorsData.GpsTimestamp)).TotalSeconds > 3)
{
speaker.Speak("Cannot add waypoint, GPS data stale");
}
else
{
if (!allowOnlyGpsFix3D || allowOnlyGpsFix3D && currentSensorsData.GpsFixType == GpsFixTypes.Fix3D)
{
currentTrack.trackpoints.Add(new Trackpoint(
waypointsIndex,
false, //waypointsIndex == 0, // home
currentSensorsData.GpsLatitude,
currentSensorsData.GpsLongitude,
currentSensorsData.GpsAltitude
));
waypointsIndex++;
speaker.Speak("Added waypoint " + currentTrack.Count);
}
else
{
speaker.Speak("Cannot add waypoint, low GPS fix: " + currentSensorsData.GpsFixType);
}
}
}
break;
//case "button10": // right stick push
// break;
default:
speaker.Speak(command + " not supported");
break;
}
previousButtonCommand = command; // avoid repetitions
}
if (isDispatcherCommand)
{
// dispatcher will be forwarding command like "speed" to behaviors
subsumptionTaskDispatcher.ControlDeviceCommand(command);
}
}
/// <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");
}
}
}
