I am working on a maze solving application that uses a Genetic Algorithm to evolve a set of genes (within Individuals) to evolve a Population of Individuals that power an Agent through a maze.

The majority of the code used appears to be working fine but when the code runs it's not selecting the best Individual's to be in the new Population correctly. When I run the application it outputs the following:

Total Fitness: 380.0 - Best Fitness: 11.0
Total Fitness: 406.0 - Best Fitness: 15.0
Total Fitness: 344.0 - Best Fitness: 12.0
Total Fitness: 373.0 - Best Fitness: 11.0
Total Fitness: 415.0 - Best Fitness: 12.0
Total Fitness: 359.0 - Best Fitness: 11.0
Total Fitness: 436.0 - Best Fitness: 13.0
Total Fitness: 390.0 - Best Fitness: 12.0
Total Fitness: 379.0 - Best Fitness: 15.0
Total Fitness: 370.0 - Best Fitness: 11.0
Total Fitness: 361.0 - Best Fitness: 11.0
Total Fitness: 413.0 - Best Fitness: 16.0

As you can clearly see the fitnesses are not improving and neither are the best fitnesses. The main code responsible for this problem is here, and I believe the problem to be within the main method, most likely where the selection methods are called:

package GeneticAlgorithm;

import GeneticAlgorithm.Individual.Action;
import Robot.Robot.Direction;
import Maze.Maze;
import Robot.Robot;
import java.util.ArrayList;
import java.util.Random;

public class RunGA {

    protected static ArrayList tmp1, tmp2 = new ArrayList();

    // Implementation of Elitism
    protected static int ELITISM_K = 5;
    // Population size
    protected static int POPULATION_SIZE = 50 + ELITISM_K;
    // Max number of Iterations
    protected static int MAX_ITERATIONS = 200;
    // Probability of Mutation
    protected static double MUTATION_PROB = 0.05;
    // Probability of Crossover
    protected static double CROSSOVER_PROB = 0.7;

    // Instantiate Random object
    private static Random rand = new Random();
    // Instantiate Population of Individuals
    private Individual[] startPopulation;
    // Total Fitness of Population
    private double totalFitness;

    Robot robot = new Robot();
    Maze maze;

    public void setElitism(int result) {
        ELITISM_K = result;
    }

    public void setPopSize(int result) {
        POPULATION_SIZE = result + ELITISM_K;
    }

    public void setMaxIt(int result) {
        MAX_ITERATIONS = result;
    }

    public void setMutProb(double result) {
        MUTATION_PROB = result;
    }

    public void setCrossoverProb(double result) {
        CROSSOVER_PROB = result;
    }

    /**
     * Constructor for Population
     */
    public RunGA(Maze maze) {
        // Create a population of population plus elitism
        startPopulation = new Individual[POPULATION_SIZE];

        // For every individual in population fill with x genes from 0 to 1
        for (int i = 0; i < POPULATION_SIZE; i++) {
            startPopulation[i] = new Individual();
            startPopulation[i].randGenes();
        }

        // Evaluate the current population's fitness
        this.evaluate(maze, startPopulation);
    }

    /**
     * Set Population
     * @param newPop
     */
    public void setPopulation(Individual[] newPop) {
        System.arraycopy(newPop, 0, this.startPopulation, 0, POPULATION_SIZE);
    }

    /**
     * Get Population
     * @return
     */
    public Individual[] getPopulation() {
        return this.startPopulation;
    }

    /**
     * Evaluate fitness
     * @return
     */
    public double evaluate(Maze maze, Individual[] newPop) {
        this.totalFitness = 0.0;
        ArrayList<Double> fitnesses = new ArrayList<Double>();
        for (int i = 0; i < POPULATION_SIZE; i++) {
            maze = new Maze(8, 8);
            maze.fillMaze();

            fitnesses.add(startPopulation[i].evaluate(maze, newPop));
            //this.totalFitness += startPopulation[i].evaluate(maze, newPop);
        }
        //totalFitness = (Math.round(totalFitness / POPULATION_SIZE));

        StringBuilder sb = new StringBuilder();

        for(Double tmp : fitnesses) {
            sb.append(tmp + ", ");
            totalFitness += tmp;
        }

        // Progress of each Individual
        //System.out.println(sb.toString());

        return this.totalFitness;
    }

    /**
     * Roulette Wheel Selection
     * @return
     */
    public Individual rouletteWheelSelection() {

        // Calculate sum of all chromosome fitnesses in population - sum S.
        double randNum = rand.nextDouble() * this.totalFitness;
        int i;
        for (i = 0; i < POPULATION_SIZE && randNum > 0; ++i) {
            randNum -= startPopulation[i].getFitnessValue();
        }
        return startPopulation[i-1];
    }

    /**
     * Tournament Selection
     * @return
     */
    public Individual tournamentSelection() {
        double randNum = rand.nextDouble() * this.totalFitness;
        // Get random number of population (add 1 to stop nullpointerexception)
        int k = rand.nextInt(POPULATION_SIZE) + 1;
        int i;
        for (i = 1; i < POPULATION_SIZE && i < k && randNum > 0; ++i) {
            randNum -= startPopulation[i].getFitnessValue();
        }
        return startPopulation[i-1];
    }

    /**
     * Finds the best individual
     * @return
     */
    public Individual findBestIndividual() {
        int idxMax = 0;
        double currentMax = 0.0;
        double currentMin = 1.0;
        double currentVal;

        for (int idx = 0; idx < POPULATION_SIZE; ++idx) {
            currentVal = startPopulation[idx].getFitnessValue();
            if (currentMax < currentMin) {
                currentMax = currentMin = currentVal;
                idxMax = idx;
            }
            if (currentVal > currentMax) {
                currentMax = currentVal;
                idxMax = idx;
            }
        }

        // Double check to see if this has the right one
        //System.out.println(startPopulation[idxMax].getFitnessValue());

        // Maximisation
        return startPopulation[idxMax];
    }

    /**
     * One Point Crossover
     * @param firstPerson
     * @param secondPerson
     * @return
     */
    public static Individual[] onePointCrossover(Individual firstPerson, Individual secondPerson) {
        Individual[] newPerson = new Individual[2];
        newPerson[0] = new Individual();
        newPerson[1] = new Individual();

        int size = Individual.SIZE;

        int randPoint = rand.nextInt(size);
        int i;
        for (i = 0; i < randPoint; ++i) {
            newPerson[0].setGene(i, firstPerson.getGene(i));
            newPerson[1].setGene(i, secondPerson.getGene(i));
        }
        for (; i < Individual.SIZE; ++i) {
            newPerson[0].setGene(i, secondPerson.getGene(i));
            newPerson[1].setGene(i, firstPerson.getGene(i));
        }

        return newPerson;
    }

    /**
     * Uniform Crossover
     * @param firstPerson
     * @param secondPerson
     * @return
     */
    public static Individual[] uniformCrossover(Individual firstPerson, Individual secondPerson) {
        Individual[] newPerson = new Individual[2];
        newPerson[0] = new Individual();
        newPerson[1] = new Individual();

        for(int i = 0; i < Individual.SIZE; ++i) {
            double r = rand.nextDouble();
            if (r > 0.5) {
                newPerson[0].setGene(i, firstPerson.getGene(i));
                newPerson[1].setGene(i, secondPerson.getGene(i));
            } else {
                newPerson[0].setGene(i, secondPerson.getGene(i));
                newPerson[1].setGene(i, firstPerson.getGene(i));
            }
        }

        return newPerson;
    }

    public double getTotalFitness() {
        return totalFitness;
    }

    public static void main(String[] args) {

        // Initialise Environment
        Maze maze = new Maze(8, 8);
        maze.fillMaze();

        // Instantiate Population
        //Population pop = new Population();
        RunGA pop = new RunGA(maze);
        // Instantiate Individuals for Population
        Individual[] newPop = new Individual[POPULATION_SIZE];
        // Instantiate two individuals to use for selection
        Individual[] people = new Individual[2];

        Action action = null;
        Direction direction = null;

        String result = "";

        /*result += "Total Fitness: " +
                pop.getTotalFitness() + " - Best Fitness: " +
                pop.findBestIndividual().getFitnessValue();*/

        // Print Current Population
        System.out.println("Total Fitness: " +
                pop.getTotalFitness() + " - Best Fitness: " +
                pop.findBestIndividual().getFitnessValue());

        // Instantiate counter for selection
        int count;

        for (int i = 0; i < MAX_ITERATIONS; i++) {
            count = 0;
            // Elitism
            for (int j = 0; j < ELITISM_K; ++j) {
                // This one has the best fitness
                newPop[count] = pop.findBestIndividual();
                count++;
            }
            // Build New Population (Population size = Steps (28))
            while (count < POPULATION_SIZE) {
                // Roulette Wheel Selection
                people[0] = pop.rouletteWheelSelection();
                people[1] = pop.rouletteWheelSelection();
                // Tournament Selection
                //people[0] = pop.tournamentSelection();
                //people[1] = pop.tournamentSelection();
                // Crossover
                if (rand.nextDouble() < CROSSOVER_PROB) {
                    // One Point Crossover
                    //people = onePointCrossover(people[0], people[1]);
                    // Uniform Crossover
                    people = uniformCrossover(people[0], people[1]);
                }
                // Mutation
                if (rand.nextDouble() < MUTATION_PROB) {
                    people[0].mutate();
                }
                if (rand.nextDouble() < MUTATION_PROB) {
                    people[1].mutate();
                }
                // Add to New Population
                newPop[count] = people[0];
                newPop[count+1] = people[1];
                count += 2;
            }
            // Make new population the current population
            pop.setPopulation(newPop);

            // Re-evaluate the current population
            //pop.evaluate();
            pop.evaluate(maze, newPop);

            // Print results to screen
            System.out.println("Total Fitness: " + pop.totalFitness + " - Best Fitness: " + pop.findBestIndividual().getFitnessValue());
            //result += "\nTotal Fitness: " + pop.totalFitness + " - Best Fitness: " + pop.findBestIndividual().getFitnessValue();
        }

        // Best Individual
        Individual bestIndiv = pop.findBestIndividual();

        //return result;
    }

}

I have uploaded the full project to RapidShare if you require the extra files, although if needed I can add the code to them here.

This problem has been depressing me for days now and if you guys can help me I will forever be in your debt.