public override void Fire()
        {
            Init();  //be sure to leave this here

            if (GetNeuronValue("Auto") == 0)
            {
                return;
            }
            goToGoal = GetNeuronValue("Goal") == 1;

            ModuleBehavior mBehavior = (ModuleBehavior)FindModleu(typeof(ModuleBehavior));

            if (mBehavior == null)
            {
                return;
            }
            Module2DModel mModel = (Module2DModel)FindModleu(typeof(Module2DModel));

            if (mModel is null)
            {
                return;
            }
            ModuleEvent mEvent = (ModuleEvent)FindModleu(typeof(ModuleEvent));

            if (mEvent == null)
            {
                return;
            }
            ModuleUKSN UKS = (ModuleUKSN)FindModleu(typeof(ModuleUKSN));

            if (UKS == null)
            {
                return;
            }

            if (GetNeuronValue("ModuleBehavior", "Done") == 0)
            {
                return;
            }


            mModel.GetSegmentsFromUKS();
            IList <Thing> segments = mModel.GetUKSSegments();

            if (segments.Count == 0)
            {
                return;
            }
            Thing t = segments[0]; //TODO: this only works with just a single item in the UKS

            //figure out if any motion occured
            Segment s = Module2DModel.SegmentFromUKSThing(t);

            Module2DModel.OrderSegment(s);

            if (GetNeuronValue("E0") == 1)
            {
                GoToLandmark(UKS.Labeled("E0").References[0].T, s);
                doPush    = 2;
                doBackOff = true;
                return;
            }
            if (GetNeuronValue("E1") == 1)
            {
                GoToLandmark(UKS.Labeled("E1").References[0].T, s);
                doPush    = 2;
                doBackOff = true;
                return;
            }
            if (GetNeuronValue("E2") == 1)
            {
                GoToLandmark(UKS.Labeled("E2").References[0].T, s);
                doPush    = 2;
                doBackOff = true;
                return;
            }


            if (doPush != 0)
            {
                if (doPush == 2)
                {
                    Push(s);
                }
                doPush--;
                return;
            }
            if (doFaceSegment)
            {
                DoFaceSegment(s);
                doFaceSegment = false;
                return;
            }

            Segment s1;

            if (lastPosition == null) //create objects to keep track of the target and last position
            {
                s1                 = s.Clone();
                lastPosition       = mModel.AddSegmentToUKS(s1);
                lastPosition.Label = "LastPosition";
                lastPosition.RemoveParent(UKS.Labeled("Segment"));
                lastPosition.AddParent(UKS.Labeled("SSegment"));
                Module2DSim mSim = (Module2DSim)FindModleu(typeof(Module2DSim));
                if (mSim is null)
                {
                    return;
                }
                Segment motionTarget = mSim.GetMotionTarget();
                if (motionTarget == null)
                {
                    motionTarget          = new Segment();
                    motionTarget.P1       = new PointPlus(4, 1.5f);
                    motionTarget.P2       = new PointPlus(4, -2.5f);
                    motionTarget.theColor = (ColorInt)0xff;
                }
                endTarget       = mModel.AddSegmentToUKS(motionTarget);
                endTarget.Label = "EndTarget";
                //endTarget.RemoveParent(UKS.Labeled("Segment"));
                //endTarget.AddParent(UKS.Labeled("SSegment"));
            }
            else
            {
                s1 = Module2DModel.SegmentFromUKSThing(lastPosition);
            }

            //get motion from subtracting and then updating last position
            Angle rotation = s.Angle - s1.Angle;

            if (rotation > PI / 2)
            {
                rotation = PI - rotation;
            }
            if (rotation < -PI / 2)
            {
                rotation = PI + rotation;
            }
            Motion motion = new Motion()
            {
                P        = (Point)s.MidPoint.V - s1.MidPoint.V,
                rotation = rotation,
            };

            lastPosition.References[0].T.V = s.P1.Clone();
            lastPosition.References[1].T.V = s.P2.Clone();

            if (Abs(motion.R) > .01 || Abs(motion.rotation) > .05 && !goToGoal && !doBackOff)
            {
                //check for existing Event
                Thing currentEvent = MostLikelyEvent(t);
                if (currentEvent == null)
                {                //add new Event
                    Thing lm1 = mEvent.CreateLandmark(new List <Thing>()
                    {
                        t
                    });
                    Thing t1 = mEvent.CreateEvent(lm1);
                    Thing t3 = UKS.AddThing("m" + motionCount++, UKS.Labeled("Motion"), motion);
                    mEvent.AddOutcomePair(t1, UKS.GetOrAddThing("Push", "Action"), t3);
                }
                return;
            }

            if (doBackOff)
            {
                DoBackOff(s);
                doBackOff     = false;
                doFaceSegment = true;
                return;
            }

            if (goToGoal)
            {
                if (endTarget != null)
                {
                    Segment s2 = Module2DModel.SegmentFromUKSThing(endTarget);
                    GoToGoal(s, s2);
                    return;
                }
            }

            Explore(s);
        }
Exemple #2
0
        public override void Fire()
        {
            Init();  //be sure to leave this here

            //get the external references
            ModuleUKSN UKS = (ModuleUKSN)FindModuleByType(typeof(ModuleUKSN));

            if (UKS == null)
            {
                return;
            }
            Module2DModel nmModel = (Module2DModel)FindModuleByType(typeof(Module2DModel));

            if (nmModel == null)
            {
                return;
            }
            ModuleBehavior nmBehavior = (ModuleBehavior)FindModuleByType(typeof(ModuleBehavior));

            if (nmBehavior == null)
            {
                return;
            }
            ModuleEvent mEvent = (ModuleEvent)FindModuleByType(typeof(ModuleEvent));

            if (mEvent == null)
            {
                return;
            }


            //check on the various input neurons...
            //check for a goal selection
            foreach (Neuron n in na.Neurons())
            {
                if (n.Fired() && n.Label.IndexOf("c") == 0 && n.Model == Neuron.modelType.Std)
                {
                    Thing newTarget = UKS.Labeled(n.Label);
                    if (newTarget == currentTarget)
                    {
                        currentTarget = null;
                    }
                    else
                    {
                        currentTarget = newTarget;
                    }
                    currentTargetReached = null;
                    break;
                }
            }
            //check for operating mode
            auto = (GetNeuronValue(null, "Auto") == 1) ? true : false;

            //don't do anything while a behavior is in progress
            if (GetNeuronValue("ModuleBehavior", "Done") == 0)
            {
                return;
            }

            //is there an existing landmark? Do I recognize I'm near a spot I was before?
            //this only calculates "R" so it is rotation-independent
            Thing best     = null;
            float bestDist = 1000;

            List <Thing> near = nmModel.NearbySegments(3);

            FindBestLandmarkMatch(UKS, ref best, ref bestDist, near);

            nmModel.FireVisibleObjects(); //not really needed

            if (bestDist < .2f)           //are we near a landmark we've been at before?
            {
                SetNeuronValue(null, "Found", 1);

                //yse, we have returned to a landmark we've been at before
                currentLandmark = best;
                currentEvent    = currentLandmark.ReferencedBy[0].T;

                //we need to reorient ourselves to face the same way as we did before (set a flag)
                if (lastLandmark != currentLandmark)
                {
                    lastLandmark = currentLandmark;
                    orienting    = true;
                }
            }
            else
            {
                //we're not near an existing landmark
                currentLandmark = null;
                currentEvent    = null;
                lastLandmark    = null;
            }

            //this is on arrival back at a landmark
            if (orienting)
            {
                Angle totalAngleError = GetTotalAngleError(near);
                if (Abs(totalAngleError) > PI / 2 - .1f)
                {
                    //turn in large increments until the angular error gets small
                    Angle angle = (float)PI / 2;
                    nmBehavior.TurnTo(angle);
                    return;
                }
                //this corrects for roundoff errors
                CorrectModelPosition(nmModel, nmBehavior, best, near);
                orienting = false;
                return;
            }

            //we are in going-to-goal mode
            if (currentTarget != null)    //goingToGoal)
            {
                if (currentEvent != null) //currentEvent means we're at a decision point...check for the decision and execute it
                {
                    Thing action = GoToGoal(currentTarget);
                    if (action != null)
                    {
                        float angle = GetAngleFromAction(action);
                        if (angle != 0)
                        {
                            nmBehavior.TurnTo(angle);
                        }
                        nmBehavior.MoveTo(1);
                        currentEvent = null;
                        return;
                    }
                }
            }

            //we are in exploration mode
            //We're not at a known landmark
            //decide which way to turn at an obstacle
            //If I am up to an obstacle...is there a decision or can I only go one way
            float distAhead  = nmModel.GetDistanceAtDirection(0);
            float distLeft   = nmModel.GetDistanceAtDirection((float)PI / 2);
            float distRight  = nmModel.GetDistanceAtDirection((float)-PI / 2);
            bool  canGoAhead = distAhead > 1;
            bool  canGoLeft  = distLeft > 1;
            bool  canGoRight = distRight > 1;
            int   options    = (canGoAhead ? 1 : 0) + (canGoRight ? 1 : 0) + (canGoLeft ? 1 : 0);

            //First determine if there is a decision to be made or if there is only one option
            if (options == 1 && auto)
            {
                //we have no choice but to follow the path
                if (canGoAhead)
                {
                    nmBehavior.MoveTo(1);
                    return;
                }
                if (canGoLeft)
                {
                    nmBehavior.TurnTo((float)-PI / 2);
                    nmBehavior.MoveTo(1);
                    return;
                }
                if (canGoRight)
                {
                    nmBehavior.TurnTo((float)PI / 2);
                    nmBehavior.MoveTo(1);
                    return;
                }
            }
            else if (options == 0 && auto)
            {
                //we're trapped...note the color ahead and turn around
                Thing thingAhead = nmModel.GetNearestThing();
                currentTargetReached = thingAhead.References[2].T;
                if (mostRecentDecisionPoint != null)
                {
                    mEvent.AddOutcomePair(mostRecentDecisionPoint, mostRecentAction, currentTargetReached);
                    mostRecentAction        = null;
                    mostRecentDecisionPoint = null;
                }

                Neuron n1 = null;
                if (currentTargetReached != null)
                {
                    n1 = na.GetNeuronAt(currentTargetReached.Label);
                }

                //color a new goal neuron
                if (n1 == null && currentTargetReached != null)
                {
                    n1       = AddLabel(currentTargetReached.Label + " ");
                    n1.Model = Neuron.modelType.Color;
                    n1.SetValueInt((int)currentTargetReached.V);
                    na.GetNeuronLocation(n1, out int X, out int Y);
                    Neuron n2 = na.GetNeuronAt(X + 1, Y);
                    if (n2 != null)
                    {
                        n2.Label = currentTargetReached.Label;
                    }
                }

                if (currentTargetReached == currentTarget)
                {
                    //if we're looking for a goal, we've reached it, so stop
                    //currentTarget = null;
                    currentEvent = null;
                }
                else
                {
                    //make a U-Turn and start backtracking
                    nmBehavior.TurnTo((float)-PI);
                }
            }
            else if (auto)
            {
                //we have a choice...
                //if the current landmark is null, create a new landmark & Event
                if (currentLandmark == null)
                {
                    //Create new Landmark...it clones the points so they are not modified by the model module
                    Thing newLandmark = mEvent.CreateLandmark(near);
                    currentEvent    = mEvent.CreateEvent(newLandmark);
                    currentLandmark = newLandmark;
                    if (mostRecentDecisionPoint != null)
                    {
                        mEvent.AddOutcomePair(mostRecentDecisionPoint, mostRecentAction, currentEvent);
                    }
                }
                else
                {
                    if (mostRecentDecisionPoint != null && mostRecentDecisionPoint.Children.Find(t1 => t1.References[0].T == mostRecentAction) == null)
                    {
                        mEvent.AddOutcomePair(mostRecentDecisionPoint, mostRecentAction, currentEvent);
                    }
                }

                //TODO improve the method of finding something not tried before
                //Decide which untried path to take
                //priorities...1)continue ahead 2)left 3)right or randomized (depending on comment below)
                Thing        newAction       = UKS.Labeled("NoAction");
                List <Thing> possibleActions = new List <Thing>();
                if (currentEvent.Children.Find(t => t.References[0].T.Label == "GoS") == null && canGoAhead)
                {
                    possibleActions.Add(UKS.Labeled("GoS"));
                }
                if (currentEvent.Children.Find(t => t.References[0].T.Label == "LTurnS") == null && canGoLeft)
                {
                    possibleActions.Add(UKS.Labeled("LTurnS"));
                }
                if (currentEvent.Children.Find(t => t.References[0].T.Label == "RTurnS") == null && canGoRight)
                {
                    possibleActions.Add(UKS.Labeled("RTurnS"));
                }
                if (possibleActions.Count == 0 && currentEvent.Children.Find(t => t.References[0].T.Label == "UTurnS") == null)
                {
                    newAction = UKS.Labeled("UTurnS");
                }
                else if (possibleActions.Count == 1)
                {
                    newAction = possibleActions[0];
                }
                else if (possibleActions.Count > 0)
                {
                    //for debugging, eliminate the ransomization by alternately commenting the 2 stmts below
                    //    newAction = possibleActions[0];
                    newAction = possibleActions[rand.Next(possibleActions.Count)];
                }

                if (newAction.Label != "NoAction")
                {
                    mostRecentAction        = newAction;
                    mostRecentDecisionPoint = currentEvent;

                    Angle angle = GetAngleFromAction(newAction);
                    nmBehavior.TurnTo(angle);
                    nmBehavior.MoveTo(1);
                }
                else
                {
                    //TODO: all actions at the current Event have been tried, is there another Event which hasn't been exhausted?
                }
                lastLandmark = currentLandmark;
                return;
            }
        }