public Dictionary<CommandFields, short> MiningUpdate()
{
Dictionary<CommandFields, short> outputState = new Dictionary<CommandFields, short>();
if (robot.ExpectedZone == lunabotics.RCU.Autonomy.Robot.Zone.Mining)
{
switch (state)
{
case AlgorithmState.LowerScoop:
if (robot.TelemetryFeedback.ArmSwingAngle > -0.6279659846) //Get pitch telemetry
{
outputState[CommandFields.BucketPivot] = -1000;
outputState[CommandFields.BucketPitch] = 1000;
}
else
state = AlgorithmState.DigIn;
break;
case AlgorithmState.DigIn:
if (!robot.TelemetryFeedback.ScoopLowerLimitSwitchDepressed)
{
outputState[CommandFields.BucketPivot] = -1000;
outputState[CommandFields.BucketPitch] = 1000;
outputState[CommandFields.TranslationalVelocity] = 500;
}
else
state = AlgorithmState.DigMore;
break;
case AlgorithmState.DigMore:
if (robot.TelemetryFeedback.ArmSwingAngle < -0.6279659846)
{
outputState[CommandFields.BucketPivot] = 1000;
outputState[CommandFields.TranslationalVelocity] = 500;
}
break;
case AlgorithmState.DepositScoop:
if (!robot.TelemetryFeedback.ScoopUpperLimitSwitchDepressed)
outputState[CommandFields.BucketPivot] = 1000;
else
{
outputState[CommandFields.TranslationalVelocity] = 500;
if (scoops<3)
state=AlgorithmState.Reposition;
else
state=AlgorithmState.Done;
}
break;
case AlgorithmState.Reposition:
outputState[CommandFields.RotationalVelocity]=1000;
break;
case AlgorithmState.Done:
outputState[CommandFields.RotationalVelocity]=0;
outputState[CommandFields.TranslationalVelocity] = 500;
outputState[CommandFields.BucketPitch] = 0;
outputState[CommandFields.BucketPivot] = 0;
break;
default:
break;
}
}
return outputState;
}
public AlgorithmController(AlgorthmParameters parametry)
: this()
{
Schedule prototype = new Schedule(parametry.NumberOfCrossoverPoints, parametry.MutationSize
, parametry.CrossoverProbability, parametry.MutationProbability);
this.replaceByGeneration = parametry.ReplaceByGeneration;
// Obecna liczba genomu najlepszego chromosomu，inicjacja 0
this.currentBestSize = 0;
// Chromosom inicjalizacji
this.prototype = prototype;
// Obecna generacja po pierwsze generacje tabel są obecnie chromosomy
this.currentBestSize = 0;
// stan algorytmu
this.state = AlgorithmState.AS_USER_STOPED;
// Algorytm może mieć co najmniej dwa chromosomy
if (parametry.NumberOfChromosomes < 2)
{
parametry.NumberOfChromosomes = 2;
}
// Śledzenia przynajmniej jedna z najlepszych chromosomie？
if (parametry.TrackBest < 1)
{
parametry.TrackBest = 1;
}
if (this.replaceByGeneration < 1)
{
this.replaceByGeneration = 1;
}
else if (this.replaceByGeneration > parametry.NumberOfChromosomes - parametry.TrackBest)
{
this.replaceByGeneration = parametry.NumberOfChromosomes - parametry.TrackBest;
}
// _chromosomes i _bestFlags są tej samej wielkości, jest jeden do jednego mapowania między nimi
this.chromosomes.Capacity = parametry.NumberOfChromosomes;
for (int i = 0; i < this.chromosomes.Capacity; i++)
{
chromosomes.Add(new Schedule());
}
this.bestFlags.Capacity = parametry.NumberOfChromosomes;
for (int i = 0; i < this.bestFlags.Capacity; i++)
{
this.bestFlags.Add(false);
}
this.bestChromosomes.Capacity = parametry.TrackBest;
for (int i = 0; i < this.bestChromosomes.Capacity; i++)
{
this.bestChromosomes.Add(-1);
}
}
public static AlgorithmState <TFactor, TStep> ReportAndReturn <TFactor, TStep>(this IProgress <AlgorithmState <TFactor, TStep> > observer, AlgorithmFlag flag, Node <TFactor, TStep> node, IReadOnlyCollection <Node <TFactor, TStep> > candidates)
{
var state = new AlgorithmState <TFactor, TStep>(flag, node, candidates);
observer.Report(state);
return(state);
}
public static AlgorithmState <TFactor, TStep> ReportAndReturn <TFactor, TStep>(this IProgress <AlgorithmState <TFactor, TStep> > observer, AlgorithmFlag flag, Node <TFactor, TStep> node)
{
var state = new AlgorithmState <TFactor, TStep>(flag, node);
observer.Report(state);
return(state);
}
/// <summary>
/// Pause Running algorithm
/// </summary>
/// <returns>if Try to Paused is successful then return True else false</returns>
public bool Pause()
{
if (_state == AlgorithmState.AS_RUNNING)
{
if (Monitor.TryEnter(Locker0, 10))
{
_state = AlgorithmState.AS_SUSPENDED;
Monitor.Exit(Locker0);
//for (int cpu = 0; cpu < MultiThreads.Length; cpu++)
// if (MultiThreads[cpu].ThreadState == ThreadState.WaitSleepJoin ||
// MultiThreads[cpu].ThreadState == ThreadState.Running)
// MultiThreads[cpu].Suspend();
//
// Show Final Best Chromosome's
_observer.NewBestChromosome(GetBestChromosome(), true);
return(true);
}
else
{
return(false);
}
}
return(false);
}
public void EvolutionStateChanged(AlgorithmState newState)
{
if (_window != null)
{
_window.SetNewState(newState);
}
}
protected virtual void OnStateChange(AlgorithmState state)
{
var stateChanged = this.StateChanged;
if (stateChanged != null)
stateChanged(state);
}
/// <summary>
/// Zatrzymuje wykonania algorytmu
/// </summary>
public void Stop()
{
if (state == AlgorithmState.AS_RUNNING)
{
state = AlgorithmState.AS_USER_STOPED;
}
}
// Handles event that is raised when state of execution of algorithm is changed
public void EvolutionStateChanged(AlgorithmState newState)
{
//if (_window)
// _window->SetNewState(newState);
//newState != AS_RUNNING ? ReleaseEvent() : BlockEvent();
Console.WriteLine($"The state of execution of algorithm is changed: {newState.ToString()}");
}
public void Report(AlgorithmState <TFactor, Point> p)
{
Callback(new AlgorithmProgressDetail()
{
Level = p.Node != null ? p.Node.Level : -1,
Step = p.Node != null ? p.Node.Step : new Point(-1, -1),
Candidates = p.Candidates.Select(c => c.Step).ToArray()
});
}
public void Start()
{
if (_prototype==null) return;
State = AlgorithmState.RUNNING;
ClearBest();
for (int i = 0; i < _numberOfChromosomes; i++)
{
_chromosomes.Add(_prototype.CreateNewFromPrototype());
AddToBest(i);
}
_currentGeneration = 0;
while (true)
{
if (_state != AlgorithmState.RUNNING)
{
break;
}
ScheduleOld best = GetBestChromosome();
if (best.Fitness >= 120)
{
State = AlgorithmState.CRITERIA_STOPPED;
break;
}
ScheduleOld[] offspring = new ScheduleOld[_replaceByGeneration];
for (int j = 0; j < _replaceByGeneration; j++)
{
ScheduleOld p1 = _chromosomes[_random.Next(_chromosomes.Count)];
ScheduleOld p2 = _chromosomes[_random.Next(_chromosomes.Count)];
offspring[j] = p1.CrossOver(p2);
offspring[j].Mutation();
offspring[j].Repair();
}
for (int j = 0; j < _replaceByGeneration; j++)
{
int ci=0;
do
{
ci = _random.Next(_chromosomes.Count);
}
while (IsInBest(ci));
_chromosomes[ci] = offspring[j];
AddToBest(ci);
}
var newbest = GetBestChromosome();
if (best != newbest)
{
OnNewBestChromosome(newbest);
}
_currentGeneration++;
OnEvolutionStateChanged(EventArgs.Empty);
}
}
public Algorithm(int iterationNumber, int populationCapacity, string filename)
{
// Initializing of File logging
mOutputFileName = filename + "_output.txt";
if (!Directory.Exists("output"))
{
Directory.CreateDirectory("output");
}
sw = new StreamWriter(new FileStream("output/" + mOutputFileName, FileMode.Create, FileAccess.Write));
mFileName = filename;
try
{
mMWrapper = new SalesmanMatrixWrapper(filename + ".txt");
}
catch (WrongMatrixException e)
{
Console.WriteLine(e.Message);
mState = AlgorithmState.WrongMatrix;
return;
}
catch (AggregateException ae)
{
foreach (var e in ae.Flatten().InnerExceptions)
{
Console.WriteLine(e.Message);
}
mState = AlgorithmState.WrongMatrix;
return;
}
mLocker = new object();
mCrossMutGate = new AutoResetEvent(false);
mRandomizer = new Random();
mIterationNumber = iterationNumber;
mPopulationCapacity = populationCapacity;
mSelOperator = new RouletteSelection();
mGenOperator = new RouletteGeneration();
// Generating start population:
mMainPopulation = mGenOperator.GeneratePop(mMWrapper, mPopulationCapacity);
mBufferPopulation = new Person[2 * populationCapacity];
// ... for statistics and something else
SetBestPerson();
SetAveFintess();
SetDiversity();
for (int i = 0; i < mCrossRoulette.Length; i++)
{
mCrossRoulette[i] = mMWrapper.FitnessFunction(mBestPerson);
mMutRoulette[i] = mAveFit;
}
}
/// <summary>
/// Resume Stopped algorithm
/// </summary>
/// <returns>if Successfully Resume then return true else false</returns>
public bool Resume()
{
try
{
if (_state == AlgorithmState.AS_SUSPENDED)
{
if (ResultControls.ResultForm._setting.Parallel_Process) // For Multi Process
{
_state = AlgorithmState.AS_RUNNING;
for (int cpu = 0; cpu < MultiThreads.Length; cpu++)
{
if (MultiThreads[cpu].ThreadState == ThreadState.Suspended)
{
MultiThreads[cpu].Resume();
}
}
System.Diagnostics.Process.GetCurrentProcess().Threads.AsParallel();
}
else // For Single Process
{
_state = AlgorithmState.AS_RUNNING;
if (MultiThreads[0].ThreadState == ThreadState.Suspended)
{
MultiThreads[0].Resume();
}
}
return(true);
}
else
{
return(false);
}
}
catch (Exception ex)
{
System.Windows.Forms.MessageBox.Show(ex.Message, ex.Source);
ResultControls.ResultForm.state = ThreadState.Stopped;
_state = AlgorithmState.AS_USER_STOPPED;
Thread.Sleep(1000);
for (int cpu = 0; cpu < MultiThreads.Length; cpu++)
{
if (MultiThreads[cpu].IsAlive)
{
MultiThreads[cpu].Abort();
}
}
return(false);
}
}
// Stops execution of algorithm
public void Stop()
{
if (_state == AlgorithmState.AS_RUNNING)
{
_state = AlgorithmState.AS_USER_STOPPED;
//for (int cpu = 0; cpu < MultiThreads.Length; cpu++)
// MultiThreads[cpu].Abort();
//
// Show Final Best Chromosome's
_observer.NewBestChromosome(GetBestChromosome(), true);
}
}
protected virtual void OnStateChange(AlgorithmState state)
{
if(state == AlgorithmState.Finished ||
state == AlgorithmState.Aborted)
OnFinished();
Interlocked.CompareExchange(ref _state, (int)state, _state);
var stateChanged = StateChanged;
if (stateChanged != null)
stateChanged(state);
}
protected Algorithm(int numberOfChromosomes, int replaceByGeneration, int trackBest, Schedule prototype)
{
// 会被后代替换掉的染色体的数量
this.replaceByGeneration = replaceByGeneration;
// 当前染色体组中最好的染色体的数量，初始化为0
this.currentBestSize = 0;
// 初始化的染色体
this.prototype = prototype;
// 当前代，表是当前是第几代染色体
this.currentBestSize = 0;
// 算法运行的状态
this.state = AlgorithmState.AS_USER_STOPED;
// 算法中至少要有两条染色体
if (numberOfChromosomes < 2)
numberOfChromosomes = 2;
// 跟踪至少一条最好的染色体？
if (trackBest < 1)
trackBest = 1;
if (this.replaceByGeneration < 1)
this.replaceByGeneration = 1;
else if (this.replaceByGeneration > numberOfChromosomes - trackBest)
this.replaceByGeneration = numberOfChromosomes - trackBest;
// _chromosomes与_bestFlags大小一样，它们之间有一对一的映射关系
this.chromosomes.Capacity = numberOfChromosomes;
for (int i = 0; i < this.chromosomes.Capacity; i++)
{
chromosomes.Add(new Schedule());
}
this.bestFlags.Capacity = numberOfChromosomes;
for (int i = 0; i < this.bestFlags.Capacity; i++)
{
this.bestFlags.Add(false);
}
this.bestChromosomes.Capacity = trackBest;
for (int i = 0; i < this.bestChromosomes.Capacity; i++)
{
this.bestChromosomes.Add(-1);
}
}
/// <summary>
/// A* algorithm that traverses the graph to find shortest path between set vertices
/// </summary>
/// <param name="graph"> <see cref="Graph"/> instance. </param>
/// <param name="startVertex"> The start <see cref="Vertex"/> of path to calculate minimum distance for. </param>
/// <param name="endVertex"> The end <see cref="Vertex"/> of path to calculate minimum distance for. </param>
/// <returns> The shortest (minimum cost) path from starting point to ending point. </returns>
public Pathway <T> FindPath(IAdjacencyMatrix <T> graph, Vertex <T> start, Vertex <T> target)
{
/// System.Collections.Generic.SortedList by default does not allow duplicate items.
/// Since items are keyed by TotalCost there can be duplicate entries per key.
var priorityComparer = Comparer <int> .Create((x, y) => (x <= y)? -1 : 1);
var opened = new PriorityQueue <int, Vertex <T> >(priorityComparer);
var visited = new PriorityQueue <int, Vertex <T> >(priorityComparer);
var path = new Dictionary <Vertex <T>, PathStep <T> >();
// Resets the AStar algorithm with the newly specified start node and goal node.
var current = start;
int estimatedDistance = _distanceMesureable.MesureDistance(start, target);
path[start] = new PathStep <T>(start, null, 0, estimatedDistance);
opened.Enqueue(path[start].TotalCost, start);
// Continue searching until either failure or the goal node has been found.
AlgorithmState state = AlgorithmState.Searching;
while (state == AlgorithmState.Searching)
{
current = GetNext(opened, visited);
if (current == null)
{
state = AlgorithmState.PathDoesNotExist;
break;
}
// Remove from the open list and place on the closed list
// since this node is now being searched.
visited.Enqueue(path[current].TotalCost, current);
// Found the goal, stop searching.
if (current.Equals(target))
{
state = AlgorithmState.PathFound;
break;
}
ExtendOpened(opened, visited, current, target, path);
}
ICollection <Vertex <T> > vertices = ReconstructPath(current, path);
int totalDistance = current != null && path.ContainsKey(current) ? path[current].MovementCost : 0;
return(new Pathway <T>(vertices, totalDistance, state));
}
/// <summary>
/// Creates the initial algorithm state (before the run starts).
/// </summary>
/// <remarks>On each call re-creates the initial state (i.e. returns
/// new object each time).</remarks>
public void InitializeInternal()
{
Debug.Assert(this.Settings != null, "Settings is null");
Debug.Assert(this.InitialSparkGenerator != null, "Initial spark generator is null");
Debug.Assert(this.BestWorstFireworkSelector != null, "Best-Worst firework selector is null");
InitialExplosion initialExplosion = new InitialExplosion(this.Settings.LocationsNumber);
Debug.Assert(initialExplosion != null, "Initial explosion is null");
IEnumerable <Firework> fireworks = this.InitialSparkGenerator.CreateSparks(initialExplosion);
Debug.Assert(fireworks != null, "Initial firework collection is null");
this.CalculateQualities(fireworks);
this.state = new AlgorithmState(fireworks, 0, this.BestWorstFireworkSelector.SelectBest(fireworks));
}
/// <summary>
/// 创建一个 <see cref="GeneticAlgorithm"/> 类实例。
/// </summary>
/// <param name="configuration">算法参数</param>
/// <param name="observer">观察者</param>
public GeneticAlgorithm(Configuration configuration, IScheduleObserver observer)
{
this.Configuration = configuration;
this.observer = observer;
this.currentBestSize = 0;
this.CurrentGeneration = 0;
this.state = AlgorithmState.AS_USER_STOPED;
// 创建染色体原型
this.prototype = new Schedule(this.Configuration);
// 为族群保留空间
this.Chromosomes = new Schedule[this.Configuration.Parameters.NumberOfChromosomes];
// 初始化最优染色体索引
this.BestFlags = new bool[this.Configuration.Parameters.NumberOfChromosomes];
this.bestChromosomes = new int[this.Configuration.Parameters.TrackBestNumber];
}
public bool Pause()
{
if (_state == AlgorithmState.AS_RUNNING)
{
if (Monitor.TryEnter(Locker0, 10))
{
_state = AlgorithmState.AS_SUSPENDED;
Monitor.Exit(Locker0);
_observer.NewBestChromosome(GetBestChromosome(), true);
return(true);
}
else
{
return(false);
}
}
return(false);
}
// Initializes genetic algorithm
public Algorithm(int numberOfChromosomes, int replaceByGeneration, int trackBest,
Schedule prototype, Schedule.ScheduleObserver observer)
{
NumberOfChromosomes = numberOfChromosomes;
TrackBest = trackBest;
ReplaceByGeneration = replaceByGeneration;
_currentBestSize = 0;
_prototype = prototype;
_observer = observer;
_currentGeneration = 0;
_state = AlgorithmState.AS_USER_STOPPED;
// reserve space for population
_chromosomes = new Schedule[NumberOfChromosomes];
_bestFlags = new bool[NumberOfChromosomes];
// reserve space for best chromosome group
_bestChromosomes = new int[TrackBest];
// clear population
for (int i = _chromosomes.Length - 1; i >= 0; --i)
{
_chromosomes[i] = null;
_bestFlags[i] = false;
}
_instance = this;
#region Find number of Active CPU or CPU core's for this Programs
long Affinity_Dec = System.Diagnostics.Process.GetCurrentProcess().ProcessorAffinity.ToInt64();
string Affinity_Bin = Convert.ToString(Affinity_Dec, 2); // toBase 2
foreach (char anyOne in Affinity_Bin.ToCharArray())
{
if (anyOne == '1')
{
numCore++;
}
}
#endregion
}
// new Thread for Start GA
private void GA_Start(object Parallel_Mutex_On)
{
if ((Boolean)Parallel_Mutex_On) // Parallel Process Requirement's
{
#region GA for Mutex On
while (true) //------------------------------------------------------------------------
{
// user has stopped execution?
if (_state == AlgorithmState.AS_CRITERIA_STOPPED || _state == AlgorithmState.AS_USER_STOPPED)
{
break;
}
else if (_state == AlgorithmState.AS_SUSPENDED)
{
if (Thread.CurrentThread.IsAlive)
{
Thread.CurrentThread.Suspend();
}
}
// Save a Elite Chromosome for protection in Mutation and etc.
Schedule best = GetBestChromosome();
// algorithm has reached criteria?
if (best.GetFitness() >= 1)
{
_state = AlgorithmState.AS_CRITERIA_STOPPED;
break;
}
// produce offspring
Schedule[] offspring;
offspring = new Schedule[_replaceByGeneration];
Random rand = new Random();
for (int j = 0; j < _replaceByGeneration; j++)
{
Schedule p1;
Schedule p2;
// selects parent randomly
lock (Locker1)
{
p1 = _chromosomes[(rand.Next() % _chromosomes.Length)].MakeCopy(false);
}
lock (Locker1)
{
p2 = _chromosomes[(rand.Next() % _chromosomes.Length)].MakeCopy(false);
}
offspring[j] = p1.Crossover(p2);
lock (Locker1)
{
offspring[j].Mutation();
offspring[j].CalculateFitness();
}
}
// replace chromosomes of current operation with offspring
for (int j = 0; j < _replaceByGeneration; j++)
{
int ci;
do
{
// select chromosome for replacement randomly
ci = rand.Next() % _chromosomes.Length;
// protect best chromosomes from replacement
} while (IsInBest(ci));
lock (Locker1)
{
// replace chromosomes
_chromosomes[ci] = null;
_chromosomes[ci] = offspring[j];
}
// try to add new chromosomes in best chromosome group
AddToBest_Parallel(ci);
}
// algorithm has found new best chromosome
if (best != GetBestChromosome())
// notify observer
{
lock (Locker1)
{
_observer.NewBestChromosome(GetBestChromosome(), ResultControls.ResultForm._setting.Display_RealTime);
}
}
_currentGeneration++;
}
// The GA job's is Complete!
if (_observer != null)
{
lock (Locker0)
{
// notify observer that execution of algorithm has changed it state
_observer.EvolutionStateChanged(_state);
}
}
Thread.CurrentThread.Abort();
#endregion
}
else
{
#region GA for Mutex Off
while (true) //------------------------------------------------------------------------
{
// user has stopped execution?
if (_state == AlgorithmState.AS_CRITERIA_STOPPED || _state == AlgorithmState.AS_USER_STOPPED)
{
break;
}
else if (_state == AlgorithmState.AS_SUSPENDED)
{
if (Thread.CurrentThread.IsAlive)
{
Thread.CurrentThread.Suspend();
}
}
// Save a Elite Chromosome for protection in Mutation and etc.
Schedule best = GetBestChromosome();
// algorithm has reached criteria?
if (best.GetFitness() >= 1)
{
_state = AlgorithmState.AS_CRITERIA_STOPPED;
break;
}
// produce offspring
Schedule[] offspring;
offspring = new Schedule[_replaceByGeneration];
Random rand = new Random();
for (int j = 0; j < _replaceByGeneration; j++)
{
// selects parent randomly
Schedule p1 = _chromosomes[(rand.Next() % _chromosomes.Length)];
Schedule p2 = _chromosomes[(rand.Next() % _chromosomes.Length)];
offspring[j] = p1.Crossover(p2);
offspring[j].Mutation();
offspring[j].CalculateFitness();
}
// replace chromosomes of current operation with offspring
for (int j = 0; j < _replaceByGeneration; j++)
{
int ci;
do
{
// select chromosome for replacement randomly
ci = rand.Next() % _chromosomes.Length;
// protect best chromosomes from replacement
} while (IsInBest(ci));
// replace chromosomes
_chromosomes[ci] = null;
_chromosomes[ci] = offspring[j];
// try to add new chromosomes in best chromosome group
AddToBest_Sequence(ci);
}
// algorithm has found new best chromosome
if (best != GetBestChromosome())
// notify observer
{
_observer.NewBestChromosome(GetBestChromosome(), ResultControls.ResultForm._setting.Display_RealTime);
}
_currentGeneration++;
}
// The GA job's is Complete!
if (_observer != null)
{
// notify observer that execution of algorithm has changed it state
_observer.EvolutionStateChanged(_state);
}
Thread.CurrentThread.Abort();
#endregion
}
}
private void GA_Start(object Parallel_Mutex_On)
{
if ((Boolean)Parallel_Mutex_On)
{
#region GA for Mutex On
while (true)
{
if (_state == AlgorithmState.AS_CRITERIA_STOPPED || _state == AlgorithmState.AS_USER_STOPPED)
{
break;
}
else if (_state == AlgorithmState.AS_SUSPENDED)
{
if (Thread.CurrentThread.IsAlive)
{
Thread.CurrentThread.Suspend();
}
}
Schedule best = GetBestChromosome();
if (best.Fitness >= 1)
{
_state = AlgorithmState.AS_CRITERIA_STOPPED;
break;
}
Schedule[] offspring;
offspring = new Schedule[_replaceByGeneration];
Random rand = new Random();
for (int j = 0; j < _replaceByGeneration; j++)
{
Schedule p1;
Schedule p2;
lock (Locker1)
{
p1 = _chromosomes[(rand.Next() % _chromosomes.Length)].MakeCopy(false);
}
lock (Locker1)
{
p2 = _chromosomes[(rand.Next() % _chromosomes.Length)].MakeCopy(false);
}
offspring[j] = p1.Crossover(p2);
lock (Locker1)
{
offspring[j].Mutation();
offspring[j].CalculateFitness();
}
}
for (int j = 0; j < _replaceByGeneration; j++)
{
int ci;
do
{
ci = rand.Next() % _chromosomes.Length;
} while (IsInBest(ci));
lock (Locker1)
{
_chromosomes[ci] = null;
_chromosomes[ci] = offspring[j];
}
AddToBest_Parallel(ci);
}
if (best != GetBestChromosome())
{
lock (Locker1)
{
_observer.NewBestChromosome(GetBestChromosome(), Views.ResultForm._setting.Display_RealTime);
}
}
_currentGeneration++;
}
if (_observer != null)
{
lock (Locker0)
{
_observer.EvolutionStateChanged(_state);
}
}
Thread.CurrentThread.Abort();
#endregion
}
else
{
#region GA for Mutex Off
while (true)
{
if (_state == AlgorithmState.AS_CRITERIA_STOPPED || _state == AlgorithmState.AS_USER_STOPPED)
{
break;
}
else if (_state == AlgorithmState.AS_SUSPENDED)
{
if (Thread.CurrentThread.IsAlive)
{
Thread.CurrentThread.Suspend();
}
}
Schedule best = GetBestChromosome();
if (best.Fitness >= 1)
{
_state = AlgorithmState.AS_CRITERIA_STOPPED;
break;
}
Schedule[] offspring;
offspring = new Schedule[_replaceByGeneration];
Random rand = new Random();
for (int j = 0; j < _replaceByGeneration; j++)
{
Schedule p1 = _chromosomes[(rand.Next() % _chromosomes.Length)];
Schedule p2 = _chromosomes[(rand.Next() % _chromosomes.Length)];
offspring[j] = p1.Crossover(p2);
offspring[j].Mutation();
offspring[j].CalculateFitness();
}
for (int j = 0; j < _replaceByGeneration; j++)
{
int ci;
do
{
ci = rand.Next() % _chromosomes.Length;
} while (IsInBest(ci));
_chromosomes[ci] = null;
_chromosomes[ci] = offspring[j];
AddToBest_Sequence(ci);
}
if (best != GetBestChromosome())
{
_observer.NewBestChromosome(GetBestChromosome(), Views.ResultForm._setting.Display_RealTime);
}
_currentGeneration++;
}
if (_observer != null)
{
_observer.EvolutionStateChanged(_state);
}
Thread.CurrentThread.Abort();
#endregion
}
}
// Stops execution of algorithm
public void Stop()
{
if (State == AlgorithmState.Running)
State = AlgorithmState.Userstopped;
//TODO: add synchronization
}
public Dictionary<CommandFields, short> Update(Robot robot, Stopwatch stopwatch)
{
Dictionary<CommandFields, short> outputState = new Dictionary<CommandFields, short>();
// Check for start pulse
if (!started)
{
// Set flag
started = true;
// Record start time
startTime = stopwatch.ElapsedMilliseconds;
// Set state
state = AlgorithmState.LiftingBin;
}
switch (state)
{
case AlgorithmState.FinePositioning:
// Read proximity sensors
outputState[Comms.CommandEncoding.CommandFields.LeftBucketActuator] = 0;
outputState[Comms.CommandEncoding.CommandFields.RightBucketActuator] = 0;
// Calcualte error
double error = robot.TelemetryFeedback.RearProximityRight - robot.TelemetryFeedback.RearProximityLeft;
// Check for position converged
if ((robot.TelemetryFeedback.RearProximityLeft - robot.TelemetryFeedback.RearProximityRight) < 10)
state = AlgorithmState.LiftingBin;
break;
case AlgorithmState.LiftingBin:
// Set state
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.LeftBucketActuator] = 1000;
outputState[Comms.CommandEncoding.CommandFields.RightBucketActuator] = 1000;
// Check for bin fully raised
if (robot.TelemetryFeedback.BinUpperSwitchDepressed)
{
state = AlgorithmState.WiggleRotational;
// Store start time
startTime = stopwatch.ElapsedMilliseconds;
}
break;
case AlgorithmState.WiggleRotational:
// Calculate time
long elapsedTime = stopwatch.ElapsedMilliseconds - startTime;
if (elapsedTime > 1500)
{
// Change direction
if (turnDirection == TurnDirection.Left)
turnDirection = TurnDirection.Right;
else
turnDirection = TurnDirection.Left;
// Increment counter
turnCount++;
// Reset start timer
startTime = stopwatch.ElapsedMilliseconds;
}
// Check convergence of MCL
if (turnCount > 3)
{
Console.WriteLine("Done wiggling");
// Set flag
state = AlgorithmState.LoweringBin;
// Set state
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.LeftBucketActuator] = 0;
outputState[Comms.CommandEncoding.CommandFields.RightBucketActuator] = 0;
}
else
{
// Set static state
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = (short)turnDirection;
outputState[Comms.CommandEncoding.CommandFields.LeftBucketActuator] = 0;
outputState[Comms.CommandEncoding.CommandFields.RightBucketActuator] = 0;
}
break;
case AlgorithmState.LoweringBin:
Console.WriteLine("Lowering bin");
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.LeftBucketActuator] = -1000;
outputState[Comms.CommandEncoding.CommandFields.RightBucketActuator] = -1000;
// Check for bin fully raised
if (robot.TelemetryFeedback.BinLowerSwitchDepressed)
state = AlgorithmState.Done;
break;
case AlgorithmState.Done:
Console.WriteLine("Done");
// Set state
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.LeftBucketActuator] = 0;
outputState[Comms.CommandEncoding.CommandFields.RightBucketActuator] = 0;
break;
default:
break;
}
// Static outputs
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
outputState[Comms.CommandEncoding.CommandFields.BucketPitch] = 0;
outputState[Comms.CommandEncoding.CommandFields.RangeFinderServo] = 90;
return outputState;
}
public void Stop()
{
if (_state == AlgorithmState.RUNNING)
State = AlgorithmState.USER_STOPPED;
}
public AlgorithmStateTests()
{
this.state = new AlgorithmState(Enumerable.Empty <Firework>(), 0, new Solution(0.0));
}
// Stops execution of algorithm
public void Stop()
{
if (_state == AlgorithmState.AS_RUNNING)
{
_state = AlgorithmState.AS_USER_STOPPED;
//for (int cpu = 0; cpu < MultiThreads.Length; cpu++)
// MultiThreads[cpu].Abort();
//
// Show Final Best Chromosome's
_observer.NewBestChromosome(GetBestChromosome(), true);
}
}
// Initializes genetic algorithm
public Algorithm(int numberOfChromosomes, int replaceByGeneration, int trackBest,
Schedule prototype, Schedule.ScheduleObserver observer)
{
NumberOfChromosomes = numberOfChromosomes;
TrackBest = trackBest;
ReplaceByGeneration = replaceByGeneration;
_currentBestSize = 0;
_prototype = prototype;
_observer = observer;
_currentGeneration = 0;
_state = AlgorithmState.AS_USER_STOPPED;
// reserve space for population
_chromosomes = new Schedule[NumberOfChromosomes];
_bestFlags = new bool[NumberOfChromosomes];
// reserve space for best chromosome group
_bestChromosomes = new int[TrackBest];
// clear population
for (int i = _chromosomes.Length - 1; i >= 0; --i)
{
_chromosomes[i] = null;
_bestFlags[i] = false;
}
_instance = this;
#region Find number of Active CPU or CPU core's for this Programs
long Affinity_Dec = System.Diagnostics.Process.GetCurrentProcess().ProcessorAffinity.ToInt64();
string Affinity_Bin = Convert.ToString(Affinity_Dec, 2); // toBase 2
foreach (char anyOne in Affinity_Bin.ToCharArray())
if (anyOne == '1') numCore++;
#endregion
}
public void EvolutionStateChanged(AlgorithmState newState)
{
// throw new NotImplementedException();
}
public void GetSchedule(ref List<Professor> professors, out Schedule best, List<CourseClass> courseList)
{
//if (prototype == null)
// return null;
//if (state == AlgorithmState.AS_RUNNING)
// return;
state = AlgorithmState.AS_RUNNING;
int i = 0;
for (; i < chromosomes.Capacity; i++)
{
chromosomes[i] = prototype.MakeNewFromPrototype(courseList);
//chromosomes.Add(prototype.MakeNewFromPrototype());
AddToBest(i);
}
// 设置当前代为0生代,属于原始人类
currentGeneration = 0;
while (true)
{
//if (state != AlgorithmState.AS_RUNNING)
//{
// break;
//}
// 获取最好的染色体，即获取最好的课表方案
best = GetBestChromosome();
// 当当前最好的染色体的fitness值大于等于1时，课表满足所有约束条件，停止排课
// 利用break跳出while(1)
if (best.GetFitness() >= 1)
{
//state = AlgorithmState.AS_CRITERIA_STOPPED;
for (int b = 0; b < best.GetSlots().Count; b++)
{
List <List< CourseClass>> slot = best.GetSlots();
if (slot[b].Count >= 1)
{
for (int p = 0; p < professors.Count; p++)
{
if (professors[p].GetId() == slot[b].First().GetProfessor().GetId())
{
professors[p].AddOccupiedTime(b);
}
}
}
}
return;
}
// 繁殖后代染色体
List<Schedule> offspring = new List<Schedule>(); // 定义后代染色体
offspring.Capacity = this.replaceByGeneration; // 设定后代染色体数量
for (int o = 0; o < offspring.Capacity; o++) // 初始化后代染色体
{
offspring.Add(new Schedule());
}
for (int j1 = 0; j1 < this.replaceByGeneration; j1++)
{
Schedule p2 = chromosomes[RandomNbr.GetRandomNbr() % chromosomes.Count];
Schedule p1 = chromosomes[RandomNbr.GetRandomNbr() % chromosomes.Count];
offspring[j1] = p1.Crossover(p2);
offspring[j1].Mutation();
}
// 用新繁殖后代染色体取代老一辈染色体,取代的个数为_replaceByGeneration
for (int j = 0; j < replaceByGeneration; j++)
{
int ci;
do
{
// 随机选择一个将被取代的染色体
ci = RandomNbr.GetRandomNbr() % chromosomes.Count;
// 如果该染色体属于最好的染色之一，则重新选一个;否则进行下面的操作，用新代取代老染色体
} while (IsInBest(ci));
// 删掉老一代的染色体，换成新的染色体
chromosomes[ci] = offspring[j];
// 看这个新生代染色体是否有足够条件成为最好的染色体之一
AddToBest(ci);
}// 到此，_replaceByGeneration个新生代染色体取代了_replaceByGeneration个老的染色体
currentGeneration++;
//Console.WriteLine("currentGeneration is " + currentGeneration);
//if (currentGeneration % 1000 == 0)
//{
// //Console.WriteLine("fitness is " + best.GetFitness());
// //System.Threading.Thread.Sleep(2000);
//}
}
}
// Starts and executes algorithm
public void Start()
{
if (Prototype == null)
return;
// do not run already running algorithm
if (State == AlgorithmState.Running)
return;
State = AlgorithmState.Running;
// clear best chromosome group from previous execution
ClearBest();
// initialize new population with chromosomes randomly built using prototype
for (int i = 0; i < Chromosomes.Count; ++i)
{
// remove chromosome from previous execution
//if (Chromosomes[i] != null)
// Chromosomes.RemoveAt(i);
// add new chromosome to population
Chromosomes[i] = Prototype.MakeNewFromPrototype();
AddToBest(i);
}
Random random = new Random(12345);
CurrentGeneration = 0;
while (true)
{
// user has stopped execution?
if (State != AlgorithmState.Running)
break;
Schedule best = GetBestChromosome();
// algorithm has reached criteria?
if (best.Fitness >= 1)
{
State = AlgorithmState.Criteriastopped;
break;
}
// produce offepsing
List<Schedule> offspring = new List<Schedule>(ReplaceByGeneration);
for(int i = 0; i < ReplaceByGeneration; ++i)
offspring.Add(null);
for (int j = 0; j < ReplaceByGeneration; ++j)
{
// selects parent randomly
Schedule p1 = Chromosomes[random.Next(1000) % Chromosomes.Count];
Schedule p2 = Chromosomes[random.Next(1000) % Chromosomes.Count];
offspring[j] = p1.Crossover(p2);
offspring[j].Mutation();
}
// replace chromosomes of current operation with offspring
for (int j = 0; j < ReplaceByGeneration; ++j)
{
int ci;
do
{
// select chromosome for replacement randomly
ci = random.Next() % Chromosomes.Count;
// protect best chromosomes from replacement
} while (IsInBest(ci));
// replace chromosomes
Chromosomes[ci] = offspring[j];
// try to add new chromosomes in best chromosome group
AddToBest(ci);
}
++CurrentGeneration;
}
}
public async Task AStarAlgorithmSteppedAsync(AlgorithmState last = null)
{
// first time check
if (last is null)
{
last = new AlgorithmState(new ObservableCollection <PFNode>(SquaresList), new List <PFNode>()
{
PFNode.StartPoint
}, PFNode.StartPoint, null);
Debug.WriteLine($"Last:{Environment.NewLine}{last}");
PFNode.StartPoint.Visited = true;
PFNode.StartPoint.PreviousNode = null;
States.Add(CurrentState);
}
else
{
last = CurrentState;
}
// end condition
if (last.CurrentlyChecking == PFNode.EndPoint)
{
Debug.WriteLine($"Solution found");
solved = true;
var curr = PFNode.EndPoint;
for (; ;)
{
if (curr.PreviousNode is null)
{
break;
}
if (curr != PFNode.StartPoint && curr != PFNode.EndPoint)
{
curr.VisualType = VisualSquareType.FinishPath;
}
curr = curr.PreviousNode;
}
return;
}
Debug.WriteLine($"Last checked square is not the endpoint, continue");
// calculate current square
PFNode currentlyChecking = last.Available[0];
Debug.WriteLine($"Currently checking the square {currentlyChecking}");
currentlyChecking.Visited = true;
// get valid sorroundings
List <PFNode> sorroundings = await CalculateSquareSorroundings(currentlyChecking);
Debug.WriteLine($"Got sorroundings of {currentlyChecking}:");
sorroundings = RemoveVisited(sorroundings);
sorroundings.ForEach(x =>
{
if (x != PFNode.EndPoint && x != PFNode.StartPoint)
{
x.VisualType = VisualSquareType.Sorrounding;
}
});
sorroundings.ForEach(x =>
{
if (x.PreviousNode is null)
{
x.PreviousNode = currentlyChecking;
}
});
Debug.WriteLine($"Removed visited squares");
last.Available.Remove(currentlyChecking);
last.Available = last.Available.Distinct(new AStarSquareComparer()).ToList();
var newAvailable = new List <PFNode>(last.Available);
newAvailable.AddRange(sorroundings);
RemoveVisited(newAvailable);
newAvailable.Sort(new AStarSquareComparer());
Debug.WriteLine($"Added last's sorroundings and removed all visited.");
CurrentState = new AlgorithmState(SquaresList, newAvailable, currentlyChecking, last);
States.Add(CurrentState);
if (last.CurrentlyChecking != PFNode.StartPoint && last.CurrentlyChecking != PFNode.EndPoint)
{
last.CurrentlyChecking.VisualType = VisualSquareType.Visited;
}
if (currentlyChecking != PFNode.StartPoint && currentlyChecking != PFNode.EndPoint)
{
currentlyChecking.VisualType = VisualSquareType.Visited;
}
Debug.WriteLine("");
Debug.WriteLine($"CurrentAmountOfSquares = {newAvailable.Count}");
Step++;
MaxStep++;
//if (currentlyChecking != PFNode.StartPoint)
//{
// currentlyChecking.PreviousNode = last.CurrentlyChecking;
//}
//else
//{
// currentlyChecking.PreviousNode = null;
//}
}
// 开始执行算法
public void Start()
{
// 如果没有原型染色体，说明当前算法未被初始化，不能开始计算
if (this.prototype == null)
{
throw new Exception("没有原型染色体，当前算法未被初始化，不能开始计算。");
}
// 如果正在运算则返回
if (this.state == AlgorithmState.AS_RUNNING)
{
return;
}
// 将状态修改为正在计算
this.state = AlgorithmState.AS_RUNNING;
// 清空上次执行的最优染色体组
this.ClearBest();
// 使用染色体初始化新族群，使用原型创建出所有的染色体
for (int i = 0; i < this.Chromosomes.Length; i++)
{
// 通过原型创建染色体
this.Chromosomes[i] = this.prototype.MakeNewFromPrototype();
// 尝试加入最优组，初始化新族群时，最优解组也被填充了
this.AddToBest(i);
}
// 当前代设为0
this.CurrentGeneration = 0;
// 开始运算，直到用户停止或算法找到最优解
while (true)
{
// 用户停止了执行？
if (this.state != AlgorithmState.AS_RUNNING)
{
break;
}
// 获得当前最优染色体
Schedule best = this.GetBestChromosome();
// 如果当前最优染色体已经是最优，结束计算
if ((best.Fitness >= this.Configuration.Parameters.MinFitness && best.Evenness >= this.Configuration.Parameters.MinEvenness) ||
this.CurrentGeneration > this.Configuration.Parameters.MaxGeneration)
{
this.state = AlgorithmState.AS_CRITERIA_STOPPED;
break;
}
// 按需要替换的染色体生产相应数量的后代
Schedule[] offspring = new Schedule[this.Configuration.Parameters.ReplaceByGeneration];
for (int j = 0; j < this.Configuration.Parameters.ReplaceByGeneration; j++)
{
// selects parent randomly
// 在当前的族群中随机选取两个染色体作为双亲
Schedule p1 = this.Chromosomes[this.Rand() % this.Chromosomes.Length];
Schedule p2 = this.Chromosomes[this.Rand() % this.Chromosomes.Length];
// 进行杂交
offspring[j] = p1.Crossover(p2);
// 进行突变
offspring[j].Mutation();
}
// 用后代替换当前操作中的染色体
foreach (var os in offspring)
{
// 随机取得一个槽位用于替换
int ci;
do
{
// 随机选择一个将被替换的染色体
ci = this.Rand() % this.Chromosomes.Length;
}while (this.IsInBest(ci)); // 保证最后的染色体不被替换掉
// 用后代替换染色体
this.Chromosomes[ci] = os;
// 尝试将新染色体放入最优组中
this.AddToBest(ci);
}
// 如果算法获得了更好的染色体
var newBest = this.GetBestChromosome();
if (best != newBest && this.observer != null)
{
this.observer.NewBestChromosome(this.CurrentGeneration, newBest.Fitness, newBest.Evenness);
}
this.CurrentGeneration++;
}
}
/// <summary>
/// Resume Stopped algorithm
/// </summary>
/// <returns>if Successfully Resume then return true else false</returns>
public bool Resume()
{
try
{
if (_state == AlgorithmState.AS_SUSPENDED)
{
if (ResultControls.ResultForm._setting.Parallel_Process) // For Multi Process
{
_state = AlgorithmState.AS_RUNNING;
for (int cpu = 0; cpu < MultiThreads.Length; cpu++)
if (MultiThreads[cpu].ThreadState == ThreadState.Suspended)
MultiThreads[cpu].Resume();
System.Diagnostics.Process.GetCurrentProcess().Threads.AsParallel();
}
else // For Single Process
{
_state = AlgorithmState.AS_RUNNING;
if (MultiThreads[0].ThreadState == ThreadState.Suspended)
MultiThreads[0].Resume();
}
return true;
}
else return false;
}
catch (Exception ex)
{
System.Windows.Forms.MessageBox.Show(ex.Message, ex.Source);
ResultControls.ResultForm.state = ThreadState.Stopped;
_state = AlgorithmState.AS_USER_STOPPED;
Thread.Sleep(1000);
for (int cpu = 0; cpu < MultiThreads.Length; cpu++)
if (MultiThreads[cpu].IsAlive)
MultiThreads[cpu].Abort();
return false;
}
}
public bool Start()
{
#region Start by initialize new population
if (_prototype == null)
{
return(false);
}
if (Monitor.TryEnter(Locker0, 10))
{
if (_state == AlgorithmState.AS_RUNNING)
{
Monitor.Exit(Locker0);
return(false);
}
_state = AlgorithmState.AS_RUNNING;
Monitor.Exit(Locker0);
}
else
{
return(false);
}
if (_observer != null)
{
_observer.EvolutionStateChanged(_state);
}
if (!Views.ResultForm._setting.Parallel_Process)
{
ClearBest_Sequence();
for (int i = 0; i < _chromosomes.Length; i++)
{
if (_chromosomes[i] != null)
{
_chromosomes[i] = null;
}
_chromosomes[i] = _prototype.MakeNewFromPrototype();
AddToBest_Sequence(i);
}
}
else
{
ClearBest_Parallel();
ParallelOptions options = new ParallelOptions();
options.MaxDegreeOfParallelism = numCore;
Parallel.For(0, _chromosomes.Length, options, i =>
{
if (_chromosomes[i] != null)
{
_chromosomes[i] = null;
}
_chromosomes[i] = _prototype.MakeNewFromPrototype();
AddToBest_Parallel(i);
});
}
_currentGeneration = 0;
#endregion
if (Views.ResultForm._setting.Parallel_Process)
{
MultiThreads = new Thread[numCore];
for (int cpu = 0; cpu < numCore; ++cpu)
{
MultiThreads[cpu] = new Thread(new ParameterizedThreadStart(GA_Start));
MultiThreads[cpu].Name = "MultiThread_" + cpu.ToString();
MultiThreads[cpu].Start(true as object);
}
System.Diagnostics.Process.GetCurrentProcess().Threads.AsParallel();
}
else
{
MultiThreads = new Thread[1];
MultiThreads[0] = new Thread(new ParameterizedThreadStart(GA_Start));
MultiThreads[0].Name = "MultiThread_0";
MultiThreads[0].Start((object)false);
}
return(true);
}
public StringMatchingState(AlgorithmState incompleteAlgorithmState, AlgorithmState completeAlgorithmState)
{
IncompleteAlgorithmState = incompleteAlgorithmState;
CompleteAlgorithmState = completeAlgorithmState;
}
public Pathway(IEnumerable <Vertex <T> > vertices, long distance, AlgorithmState state)
{
_vertices = vertices.ToList();
Distance = distance;
State = state;
}
public void SetNewState(AlgorithmState state)
{
_running = state == AlgorithmState.AS_RUNNING;
}
// Starts and executes algorithm
public bool Start()
{
#region Start by initialize new population
if (_prototype == null)
{
return(false);
}
if (Monitor.TryEnter(Locker0, 10))
{
// do not run already running algorithm
if (_state == AlgorithmState.AS_RUNNING)
{
Monitor.Exit(Locker0);
return(false);
}
_state = AlgorithmState.AS_RUNNING;
Monitor.Exit(Locker0);
}
else
{
return(false);
}
if (_observer != null)
{
// notify observer that execution of algorithm has changed it state
_observer.EvolutionStateChanged(_state);
}
if (!ResultControls.ResultForm._setting.Parallel_Process) // Single Process for GA
{
// clear best chromosome group from previous execution
ClearBest_Sequence();
// initialize new population with chromosomes randomly built using prototype
for (int i = 0; i < _chromosomes.Length; i++)
{
// remove chromosome from previous execution
if (_chromosomes[i] != null)
{
_chromosomes[i] = null;
}
// add new chromosome to population
_chromosomes[i] = _prototype.MakeNewFromPrototype();
AddToBest_Sequence(i);
}
}
else // Multi Process for GA
{
// clear best chromosome group from previous execution
ClearBest_Parallel();
// initialize new population with chromosomes randomly built using prototype
ParallelOptions options = new ParallelOptions();
options.MaxDegreeOfParallelism = numCore;
Parallel.For(0, _chromosomes.Length, options, i =>
{
// remove chromosome from previous execution
if (_chromosomes[i] != null)
{
_chromosomes[i] = null;
}
// add new chromosome to population
_chromosomes[i] = _prototype.MakeNewFromPrototype();
AddToBest_Parallel(i);
});
}
_currentGeneration = 0;
#endregion
//
// create new Threads as for manual selected CPU core's
//
if (ResultControls.ResultForm._setting.Parallel_Process) // Multi Process
{
MultiThreads = new Thread[numCore];
for (int cpu = 0; cpu < numCore; ++cpu)
{
MultiThreads[cpu] = new Thread(new ParameterizedThreadStart(GA_Start));
MultiThreads[cpu].Name = "MultiThread_" + cpu.ToString();
MultiThreads[cpu].Start(true as object);
}
System.Diagnostics.Process.GetCurrentProcess().Threads.AsParallel();
}
else // Single Process
{
MultiThreads = new Thread[1];
MultiThreads[0] = new Thread(new ParameterizedThreadStart(GA_Start));
MultiThreads[0].Name = "MultiThread_0";
MultiThreads[0].Start((object)false);
}
return(true);
}
public Dictionary<CommandFields, short> Update(Robot robot, Stopwatch stopwatch, Position position)
{
Dictionary<CommandFields, short> outputState = new Dictionary<CommandFields, short>();
switch (state)
{
case AlgorithmState.ParkScoop:
if (!robot.TelemetryFeedback.ScoopUpperLimitSwitchDepressed)
{
// Sit still and raise scoop
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 1000;
}
else
{
// Set state to next
state = AlgorithmState.MoveToHeadingInitial;
// Stop
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
}
break;
case AlgorithmState.MoveToHeadingInitial:
// Calculate angle error (normalized -180,180)
double error = (robot.Angle - position.Angle) % 180.0d;
// Check error
if (error > AngleThreshold)
{
if (error < 0)
{
// Move left
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 500;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
}
else
{
// Move right
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = -500;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
}
}
else
{
// Move to next state
state = AlgorithmState.MoveToPosition;
// Converged, set state
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
}
break;
case AlgorithmState.MoveToPosition:
// Calculate distance from goal
double distance = Math.Sqrt(
Math.Pow((robot.X - position.X), 2.0d) +
Math.Pow((robot.Y - position.Y), 2.0d));
// Check threshold
if (distance > PositionThreshold)
{
// Not converged, so move toward position
double F_att_x = (robot.X - position.X) * GainAttractive;
double F_att_y = (robot.Y - position.Y) * GainAttractive;
// Calculate angle
double angle = MathHelper.RadianToDegree(Math.Atan2(F_att_y, F_att_x));
// Calculate mixed commands
double omega = angle * GainPFC;
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 500;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = Convert.ToInt16(angle * GainPFC);
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
}
break;
case AlgorithmState.MoveToHeadingFinal:
// Calculate angle error (normalized -180,180)
double error = (robot.Angle - position.Angle) % 180.0d;
// Check error
if (error > AngleThreshold)
{
if (error < 0)
{
// Move left
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 500;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
}
else
{
// Move right
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = -500;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
}
}
else
{
// Move to next state
state = AlgorithmState.Done;
// Converged, set state
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
}
break;
case AlgorithmState.Done:
break;
default:
break;
}
// Static output
outputState[Comms.CommandEncoding.CommandFields.BucketPitch] = 0;
outputState[Comms.CommandEncoding.CommandFields.LeftBucketActuator] = 0;
outputState[Comms.CommandEncoding.CommandFields.RightBucketActuator] = 0;
outputState[Comms.CommandEncoding.CommandFields.RangeFinderServo] = 90;
return outputState;
}
/// <summary>
/// Pause Running algorithm
/// </summary>
/// <returns>if Try to Paused is successful then return True else false</returns>
public bool Pause()
{
if (_state == AlgorithmState.AS_RUNNING)
{
if (Monitor.TryEnter(Locker0, 10))
{
_state = AlgorithmState.AS_SUSPENDED;
Monitor.Exit(Locker0);
//for (int cpu = 0; cpu < MultiThreads.Length; cpu++)
// if (MultiThreads[cpu].ThreadState == ThreadState.WaitSleepJoin ||
// MultiThreads[cpu].ThreadState == ThreadState.Running)
// MultiThreads[cpu].Suspend();
//
// Show Final Best Chromosome's
_observer.NewBestChromosome(GetBestChromosome(), true);
return true;
}
else return false;
}
return false;
}
/// <summary>
/// 算法停止计算
/// </summary>
public void Stop()
{
if (state == AlgorithmState.AS_RUNNING)
state = AlgorithmState.AS_USER_STOPED;
}
public StatusMessage(AlgorithmState setFrom)
{
string newline = System.Environment.NewLine;
completeMessage = "";
if (!programLaunchMessage)
{
programLaunchMessage = true;
completeMessage = "The program has been launched.\nBender's starting position is (";
completeMessage += (setFrom.boardData.GetUnitSquare[UnitType.Bender].x + 1).ToString();
completeMessage += ", " + (setFrom.boardData.GetUnitSquare[UnitType.Bender].y + 1).ToString() + ").";
}
else if (!AlgorithmManager.algorithmStarted)
{
completeMessage = "The board was reset. Progress has been erased.";
}
//Starting data
else if (setFrom.GetStepNumber() == 0)
{
completeMessage = "A new episode has been created.\n";
completeMessage += "Starting turn [Episode: " + setFrom.GetEpisodeNumber().ToString();
completeMessage += ", Step: " + setFrom.GetStepNumber().ToString() + "]";
completeMessage += " at position (" + (setFrom.boardData.GetUnitSquare[UnitType.Bender].x + 1).ToString();
completeMessage += ", " + (setFrom.boardData.GetUnitSquare[UnitType.Bender].y + 1).ToString() + ").";
foreach (var unit in UnitType.List)
{
completeMessage += System.Environment.NewLine + unit.unitName + "'s initial perception is:";
completeMessage += System.Environment.NewLine + setFrom.GetPerception(unit).ToString() + ".";
}
}
else
{
//"Episode #, Step # beginning."
startingData = "Starting turn [Episode: " + setFrom.GetEpisodeNumber().ToString() + ", Step: " + setFrom.GetStepNumber().ToString() + "]";
startingData += " at position (" + (setFrom.locationInitial.x + 1).ToString() + ", " + (setFrom.locationInitial.y + 1).ToString() + ").";
initialPerceptData = "Bender's initial perception is: " + System.Environment.NewLine;
initialPerceptData += setFrom.GetPerception(UnitType.Bender).ToString();
moveBeingAppliedData = "";
if (setFrom.liveQmatrix.randomlyMoved)
{
moveBeingAppliedData += "A the move was randomly generated." + System.Environment.NewLine;
}
else
{
moveBeingAppliedData += "The move was greedily chosen." + System.Environment.NewLine;
}
//if (setFrom.movesThisStep.Count == 0)
// moveBeingAppliedData += "No move this turn.";
if (setFrom.GetUnit(UnitType.Bender).GetMoveThisStep() == Move.Grab)
{
moveBeingAppliedData += "A [Grab] was attempted.";
}
else
{
moveBeingAppliedData += "A [" + setFrom.GetUnit(UnitType.Bender).GetMoveThisStep().longName + "] was attempted.";
}
//moveresult
if (setFrom.resultThisStep != null)
{
moveResultData = "The result of the move was [" + setFrom.resultThisStep.result_data + "].";
}
//The reward from this action was:
string math_sign = "+";
if (setFrom.obtainedReward < 0)
{
math_sign = "";
}
rewardData = "The reward for this action was: [" + math_sign + setFrom.obtainedReward.ToString() + "]";
//reward_data + = ", applied to state ";
//Add bender perception data in his new location here
//"Bender's position is:
newPositionData = "The resulting position was (" + (setFrom.boardData.GetUnitSquare[UnitType.Bender].x + 1).ToString();
newPositionData += ", " + (setFrom.boardData.GetUnitSquare[UnitType.Bender].y + 1).ToString() + ").";
//New percept
newPerceptData = "The percept at the new location is: " + System.Environment.NewLine;
newPerceptData += setFrom.boardData.units[UnitType.Bender].perceptionData.ToString() + ".";
//"The calculation used on the q matrix was:"
qmatrixAdjustmentData = "No qmatrix entry was made.";
if (setFrom.liveQmatrix.didWeUpdate)
{
qmatrixAdjustmentData = "A q-matrix entry was made for this perception.";
}
urlData = "Url made a move of " + setFrom.GetUnit(UnitType.Url).GetMoveThisStep().longName + ".";
if (setFrom.urlRandomlyStopped)
{
urlData += newline + "Url randomly hit his change to stop chasing this turn.";
}
if (setFrom.startedChasing)
{
urlData += newline + "Url started chasing Bender.";
}
else if (setFrom.boardData.units[UnitType.Url].chasing)
{
urlData += System.Environment.NewLine + "Url is chasing bender.";
}
else
{
urlData += System.Environment.NewLine + "Url is wandering randomly.";
}
//ending data
if (setFrom.benderAttacked)
{
endingData += "Bender was attacked this turn, and the board was reset." + newline;
}
endingData += "Move [" + setFrom.GetStepNumber().ToString() + "] complete.";
completeMessage = startingData;
completeMessage += newline + initialPerceptData;
completeMessage += newline + moveBeingAppliedData;
completeMessage += newline + moveResultData;
completeMessage += newline + rewardData;
completeMessage += newline + newPositionData;
completeMessage += newline + newPerceptData;
completeMessage += newline + urlData;
completeMessage += newline + qmatrixAdjustmentData;
completeMessage += newline + endingData;
}
}
public static AlgorithmState <TFactor, TStep> ReportAndReturn <TFactor, TStep>(this IProgress <AlgorithmState <TFactor, TStep> > observer, AlgorithmState <TFactor, TStep> state)
{
observer.Report(state);
return(state);
}
public static string GetMessageFromState(AlgorithmState set_from)
{
string complete_message;
complete_message = "";
if (!program_launch_message)
{
program_launch_message = true;
complete_message = "The program has been launched.\nBender's starting position is (";
complete_message += (set_from.board_data.bender.x_coordinate + 1).ToString() + ", " + (set_from.board_data.bender.y_coordinate + 1).ToString() + ").";
}
else if (!AlgorithmState.algorithm_started)
{
complete_message = "The board was reset. Progress has been erased.";
}
//Starting data
else if (set_from.GetStepNumber() == 0)
{
complete_message = "A new episode has been created.\n";
complete_message += "Starting turn [Episode: " + set_from.GetEpisodeNumber().ToString();
complete_message += ", Step: " + set_from.GetStepNumber().ToString() + "]";
complete_message += " at position (" + (set_from.board_data.bender.x_coordinate + 1).ToString();
complete_message += ", " + (set_from.board_data.bender.y_coordinate + 1).ToString() + ").";
complete_message += System.Environment.NewLine + "Bender's initial perception is:";
complete_message += System.Environment.NewLine + set_from.bender_perception_ending.ToString() + ".";
}
else
{
//"Episode #, Step # beginning."
string starting_data = "Starting turn [Episode: " + set_from.GetEpisodeNumber().ToString() + ", Step: " + set_from.GetStepNumber().ToString() + "]";
starting_data += " at position (" + (set_from.location_initial[0] + 1).ToString() + ", " + (set_from.location_initial[1] + 1).ToString() + ").";
string initial_percept_data = "Bender's initial perception is: " + System.Environment.NewLine;
initial_percept_data += set_from.bender_perception_starting.ToString();
string move_being_applied_data = "";
if (set_from.live_qmatrix.randomly_moved)
{
move_being_applied_data += "A the move was randomly generated." + System.Environment.NewLine;
}
else
{
move_being_applied_data += "The move was greedily chosen." + System.Environment.NewLine;
}
if (set_from.move_this_step == Move.grab())
{
move_being_applied_data += "A [Grab] was attempted.";
}
else
{
move_being_applied_data += "A [" + set_from.move_this_step.long_name + "] was attempted.";
}
//moveresult
string moveresult_data = "The result of the move was [" + set_from.result_this_step.result_data + "].";
//The reward from this action was:
string math_sign = "+";
if (set_from.obtained_reward < 0)
{
math_sign = "";
}
string reward_data = "The reward for this action was: [" + math_sign + set_from.obtained_reward.ToString() + "]";
//reward_data + = ", applied to state ";
//Add bender perception data in his new location here
//"Bender's position is:
string new_position_data = "The resulting position was (" + (set_from.location_result[0] + 1).ToString();
new_position_data += ", " + ((set_from.location_result[1]) + 1).ToString() + ").";
//New percept
string new_percept_data = "The percept at the new location is: " + System.Environment.NewLine + set_from.bender_perception_ending.ToString() + ".";
//"The calculation used on the q matrix was:"
string qmatrix_adjustment_data = "No qmatrix entry was made.";
if (set_from.live_qmatrix.did_we_update)
{
qmatrix_adjustment_data = "A q-matrix entry was made for this perception.";
}
//ending data
string ending_data = "Move [" + set_from.GetStepNumber().ToString() + "] complete.";
string newline = System.Environment.NewLine;
complete_message = starting_data;
complete_message += newline + initial_percept_data;
complete_message += newline + move_being_applied_data;
complete_message += newline + moveresult_data;
complete_message += newline + reward_data;
complete_message += newline + new_position_data;
complete_message += newline + new_percept_data;
complete_message += newline + qmatrix_adjustment_data;
complete_message += newline + ending_data;
}
return(complete_message);
}
// Starts and executes algorithm
public bool Start()
{
#region Start by initialize new population
if (_prototype == null)
return false;
if (Monitor.TryEnter(Locker0, 10))
{
// do not run already running algorithm
if (_state == AlgorithmState.AS_RUNNING)
{
Monitor.Exit(Locker0);
return false;
}
_state = AlgorithmState.AS_RUNNING;
Monitor.Exit(Locker0);
}
else return false;
if (_observer != null)
{
// notify observer that execution of algorithm has changed it state
_observer.EvolutionStateChanged(_state);
}
if (!ResultControls.ResultForm._setting.Parallel_Process) // Single Process for GA
{
// clear best chromosome group from previous execution
ClearBest_Sequence();
// initialize new population with chromosomes randomly built using prototype
for (int i = 0; i < _chromosomes.Length; i++)
{
// remove chromosome from previous execution
if (_chromosomes[i] != null)
_chromosomes[i] = null;
// add new chromosome to population
_chromosomes[i] = _prototype.MakeNewFromPrototype();
AddToBest_Sequence(i);
}
}
else // Multi Process for GA
{
// clear best chromosome group from previous execution
ClearBest_Parallel();
// initialize new population with chromosomes randomly built using prototype
ParallelOptions options = new ParallelOptions();
options.MaxDegreeOfParallelism = numCore;
Parallel.For(0, _chromosomes.Length, options, i =>
{
// remove chromosome from previous execution
if (_chromosomes[i] != null)
_chromosomes[i] = null;
// add new chromosome to population
_chromosomes[i] = _prototype.MakeNewFromPrototype();
AddToBest_Parallel(i);
});
}
_currentGeneration = 0;
#endregion
//
// create new Threads as for manual selected CPU core's
//
if (ResultControls.ResultForm._setting.Parallel_Process) // Multi Process
{
MultiThreads = new Thread[numCore];
for (int cpu = 0; cpu < numCore; ++cpu)
{
MultiThreads[cpu] = new Thread(new ParameterizedThreadStart(GA_Start));
MultiThreads[cpu].Name = "MultiThread_" + cpu.ToString();
MultiThreads[cpu].Start(true as object);
}
System.Diagnostics.Process.GetCurrentProcess().Threads.AsParallel();
}
else // Single Process
{
MultiThreads = new Thread[1];
MultiThreads[0] = new Thread(new ParameterizedThreadStart(GA_Start));
MultiThreads[0].Name = "MultiThread_0";
MultiThreads[0].Start((object)false);
}
return true;
}
public void SetNewState(AlgorithmState state)
{
_running = state == AlgorithmState.AS_RUNNING;
}
// new Thread for Start GA
private void GA_Start(object Parallel_Mutex_On)
{
if ((Boolean)Parallel_Mutex_On) // Parallel Process Requirement's
{
#region GA for Mutex On
while (true) //------------------------------------------------------------------------
{
// user has stopped execution?
if (_state == AlgorithmState.AS_CRITERIA_STOPPED || _state == AlgorithmState.AS_USER_STOPPED)
{
break;
}
else if (_state == AlgorithmState.AS_SUSPENDED)
{
if (Thread.CurrentThread.IsAlive)
Thread.CurrentThread.Suspend();
}
// Save a Elite Chromosome for protection in Mutation and etc.
Schedule best = GetBestChromosome();
// algorithm has reached criteria?
if (best.GetFitness() >= 1)
{
_state = AlgorithmState.AS_CRITERIA_STOPPED;
break;
}
// produce offspring
Schedule[] offspring;
offspring = new Schedule[_replaceByGeneration];
Random rand = new Random();
for (int j = 0; j < _replaceByGeneration; j++)
{
Schedule p1;
Schedule p2;
// selects parent randomly
lock (Locker1)
{
p1 = _chromosomes[(rand.Next() % _chromosomes.Length)].MakeCopy(false);
}
lock (Locker1)
{
p2 = _chromosomes[(rand.Next() % _chromosomes.Length)].MakeCopy(false);
}
offspring[j] = p1.Crossover(p2);
lock (Locker1)
{
offspring[j].Mutation();
offspring[j].CalculateFitness();
}
}
// replace chromosomes of current operation with offspring
for (int j = 0; j < _replaceByGeneration; j++)
{
int ci;
do
{
// select chromosome for replacement randomly
ci = rand.Next() % _chromosomes.Length;
// protect best chromosomes from replacement
} while (IsInBest(ci));
lock (Locker1)
{
// replace chromosomes
_chromosomes[ci] = null;
_chromosomes[ci] = offspring[j];
}
// try to add new chromosomes in best chromosome group
AddToBest_Parallel(ci);
}
// algorithm has found new best chromosome
if (best != GetBestChromosome())
// notify observer
{
lock (Locker1)
{
_observer.NewBestChromosome(GetBestChromosome(), ResultControls.ResultForm._setting.Display_RealTime);
}
}
_currentGeneration++;
}
// The GA job's is Complete!
if (_observer != null)
{
lock (Locker0)
{
// notify observer that execution of algorithm has changed it state
_observer.EvolutionStateChanged(_state);
}
}
Thread.CurrentThread.Abort();
#endregion
}
else
{
#region GA for Mutex Off
while (true) //------------------------------------------------------------------------
{
// user has stopped execution?
if (_state == AlgorithmState.AS_CRITERIA_STOPPED || _state == AlgorithmState.AS_USER_STOPPED)
{
break;
}
else if (_state == AlgorithmState.AS_SUSPENDED)
{
if (Thread.CurrentThread.IsAlive)
Thread.CurrentThread.Suspend();
}
// Save a Elite Chromosome for protection in Mutation and etc.
Schedule best = GetBestChromosome();
// algorithm has reached criteria?
if (best.GetFitness() >= 1)
{
_state = AlgorithmState.AS_CRITERIA_STOPPED;
break;
}
// produce offspring
Schedule[] offspring;
offspring = new Schedule[_replaceByGeneration];
Random rand = new Random();
for (int j = 0; j < _replaceByGeneration; j++)
{
// selects parent randomly
Schedule p1 = _chromosomes[(rand.Next() % _chromosomes.Length)];
Schedule p2 = _chromosomes[(rand.Next() % _chromosomes.Length)];
offspring[j] = p1.Crossover(p2);
offspring[j].Mutation();
offspring[j].CalculateFitness();
}
// replace chromosomes of current operation with offspring
for (int j = 0; j < _replaceByGeneration; j++)
{
int ci;
do
{
// select chromosome for replacement randomly
ci = rand.Next() % _chromosomes.Length;
// protect best chromosomes from replacement
} while (IsInBest(ci));
// replace chromosomes
_chromosomes[ci] = null;
_chromosomes[ci] = offspring[j];
// try to add new chromosomes in best chromosome group
AddToBest_Sequence(ci);
}
// algorithm has found new best chromosome
if (best != GetBestChromosome())
// notify observer
{
_observer.NewBestChromosome(GetBestChromosome(), ResultControls.ResultForm._setting.Display_RealTime);
}
_currentGeneration++;
}
// The GA job's is Complete!
if (_observer != null)
{
// notify observer that execution of algorithm has changed it state
_observer.EvolutionStateChanged(_state);
}
Thread.CurrentThread.Abort();
#endregion
}
}
public Dictionary<CommandFields, short> Update(Robot robot, MonteCarloLocalization localization, Stopwatch stopwatch)
{
Dictionary<CommandFields, short> outputState = new Dictionary<CommandFields, short>();
// Check for start pulse
if (!started)
{
// Set flag
started = true;
// Record start time
startTime = stopwatch.ElapsedMilliseconds;
// Set state
state = AlgorithmState.LiftingScoop;
}
switch (state)
{
case AlgorithmState.LiftingScoop:
// Check arm swing limit
if (robot.TelemetryFeedback.ScoopUpperLimitSwitchDepressed)
{
Console.WriteLine("Lifting scoop");
// Move to next stage
state = AlgorithmState.Spinning;
// Save time
startTime = stopwatch.ElapsedMilliseconds;
// Set state
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
}
else
{
Console.WriteLine("Scoop lifted");
// Set state
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 1000;
}
break;
case AlgorithmState.Spinning:
long elapsedTime = stopwatch.ElapsedMilliseconds - startTime;
if (elapsedTime > configuration.LocalizationTurnDuration)
{
// Change direction
if (turnDirection == TurnDirection.Left)
turnDirection = TurnDirection.Right;
else
turnDirection = TurnDirection.Left;
// Reset start timer
startTime = stopwatch.ElapsedMilliseconds;
}
// Check convergence of MCL
if (localization.Confidence > configuration.DesiredLocalizationConfidence)
{
Console.WriteLine("MCL converged");
// Set flag
state = AlgorithmState.Done;
// Set state
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
}
else
{
Console.WriteLine("Spinning in place, MCL confidence: " + localization.Confidence);
// Set static state
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = (short)turnDirection;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 0;
// Check reinitialize
if (elapsedTime > configuration.ConvergenceAllowance)
{
// Reinitialize the MCL algorithm
localization.Initialize(robot);
// Reset timer
startTime = stopwatch.ElapsedMilliseconds;
}
}
break;
case AlgorithmState.Done:
// Set state
outputState[Comms.CommandEncoding.CommandFields.TranslationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.RotationalVelocity] = 0;
outputState[Comms.CommandEncoding.CommandFields.BucketPivot] = 1000;
break;
default:
break;
}
// Static state
outputState[Comms.CommandEncoding.CommandFields.BucketPitch] = 0;
outputState[Comms.CommandEncoding.CommandFields.LeftBucketActuator] = 0;
outputState[Comms.CommandEncoding.CommandFields.RightBucketActuator] = 0;
outputState[Comms.CommandEncoding.CommandFields.RangeFinderServo] = 90;
return outputState;
}
// Handles event that is raised when state of execution of algorithm is changed
public void EvolutionStateChanged(AlgorithmState newState)
{
if (_window != null)
_window.SetNewState(newState);
}
