diff --git a/cpprevolve/revolve/brains/CMakeLists.txt b/cpprevolve/revolve/brains/CMakeLists.txt
index 39e21e070e..4f147bf92f 100644
--- a/cpprevolve/revolve/brains/CMakeLists.txt
+++ b/cpprevolve/revolve/brains/CMakeLists.txt
@@ -2,6 +2,8 @@ set (CMAKE_CXX_STANDARD 14)
 
 set(CONTROLLER_SRCS
     controller/DifferentialCPG.cpp
+    controller/BrokenDifferentialCPG.cpp
+    controller/sensors/AngleToTargetDetector.cpp
 )
 set(LEARNER_SRCS
     learner/Learner.cpp
@@ -17,9 +19,12 @@ find_package(Boost REQUIRED COMPONENTS system)
 
 # Find Eigen3 - A lightweight C++ template library for vector and matrix math
 find_package(Eigen3 REQUIRED)
-
 find_package(MultiNEAT REQUIRED)
 
+# These dependencies are required for the AngleToTargetDetector Sensor
+find_package(OpenCV REQUIRED)
+#find_package(raspicam REQUIRED) #only on the raspberry side
+
 # Find NLOpt - Non Linear Optimization
 pkg_check_modules(NLOpt REQUIRED nlopt>=2.4)
 
@@ -32,7 +37,8 @@ add_library(revolve-learners SHARED ${LEARNER_SRCS})
 
 target_include_directories(revolve-controllers
         PUBLIC ${EIGEN3_INCLUDE_DIR}
-        PUBLIC ${Boost_INCLUDE_DIRS})
+        PUBLIC ${Boost_INCLUDE_DIRS}
+        PUBLIC ${OpenCV_INCLUDE_DIRS})
 
 target_include_directories(revolve-learners
         PUBLIC ${EIGEN3_INCLUDE_DIR}
@@ -44,11 +50,13 @@ target_compile_definitions(revolve-learners
         PUBLIC USE_NLOPT=1)
 
 target_link_libraries(revolve-controllers
-        MultiNEAT::MultiNEAT)
+        MultiNEAT::MultiNEAT
+        ${OpenCV_LIBS})
 
 target_link_libraries(revolve-learners
+        revolve-controllers
         MultiNEAT::MultiNEAT)
 
 install(TARGETS revolve-controllers revolve-learners
         RUNTIME DESTINATION bin
-        LIBRARY DESTINATION lib)
\ No newline at end of file
+        LIBRARY DESTINATION lib)
diff --git a/cpprevolve/revolve/brains/controller/BrokenDifferentialCPG.cpp b/cpprevolve/revolve/brains/controller/BrokenDifferentialCPG.cpp
new file mode 100644
index 0000000000..80fa89f158
--- /dev/null
+++ b/cpprevolve/revolve/brains/controller/BrokenDifferentialCPG.cpp
@@ -0,0 +1,477 @@
+/*
+ * Copyright (C) 2015-2018 Vrije Universiteit Amsterdam
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Description: TODO: <Add brief description about file purpose>
+ * Author: Milan Jelisavcic & Maarten van Hooft
+ * Date: December 29, 2018
+ *
+ */
+
+#include "BrokenDifferentialCPG.h"
+
+// STL macros
+#include <cstdlib>
+#include <map>
+#include <algorithm>
+#include <random>
+#include <tuple>
+#include <time.h>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+#include <Eigen/Geometry>
+
+// Project headers
+#include "actuators/Actuator.h"
+
+#include "sensors/Sensor.h"
+
+// TODO: Resolve odd behaviour at the end of the validation procedure
+// This behaviour is not present if you directly load a trained controller
+
+// Define namespaces
+using namespace revolve;
+
+/**
+ * Constructor for BrokenDifferentialCPG class.
+ *
+ * @param _model
+ * @param robot_config
+ */
+BrokenDifferentialCPG::BrokenDifferentialCPG(
+        const BrokenDifferentialCPG::ControllerParams &params,
+        const std::vector< std::shared_ptr< Actuator > > &actuators,
+        std::shared_ptr<AngleToTargetDetector> angle_to_target_sensor)
+          : Controller(ControllerType::DIFFERENTIAL_CPG)
+          , next_state(nullptr)
+          , n_motors(actuators.size())
+          , output(new double[actuators.size()])
+          , angle_to_target_sensor(std::move(angle_to_target_sensor))
+{
+    // Controller parameters
+    this->reset_neuron_random = params.reset_neuron_random;
+    this->init_neuron_state = params.init_neuron_state;
+    this->range_lb = -params.range_ub;
+    this->range_ub = params.range_ub;
+    this->use_frame_of_reference = params.use_frame_of_reference;
+    this->signal_factor_all_ = params.signal_factor_all;
+    this->signal_factor_mid = params.signal_factor_mid;
+    this->signal_factor_left_right = params.signal_factor_left_right;
+    this->abs_output_bound = params.abs_output_bound;
+
+    if (use_frame_of_reference and not this->angle_to_target_sensor) {
+        std::clog << "WARNING!: use_frame_of_reference is activated but no angle_to_target_sensor camera is configured. "
+                     "Disabling the use of the frame of reference" << std::endl;
+        use_frame_of_reference = false;
+    }
+
+    size_t j=0;
+    for (const std::shared_ptr<Actuator> &actuator: actuators)
+    {
+        // Pass coordinates
+        auto coord_x = actuator->coordinate_x();
+        auto coord_y = actuator->coordinate_y();
+        this->motor_coordinates[{coord_x, coord_y}] = j;
+
+        // Set frame of reference
+        int frame_of_reference = 0;
+        // We are a left neuron
+        if (coord_y < 0)
+        {
+            frame_of_reference = -1;
+        }
+            // We are a right neuron
+        else if (coord_y > 0)
+        {
+            frame_of_reference = 1;
+        }
+
+        // Save neurons: bias/gain/state. Make sure initial states are of different sign.
+        this->neurons[{coord_x, coord_y, 1}] = {0.f, 0.f, this->init_neuron_state, frame_of_reference}; //Neuron A
+        this->neurons[{coord_x, coord_y, -1}] = {0.f, 0.f, -this->init_neuron_state, frame_of_reference}; // Neuron B
+        j++;
+    }
+
+    // Add connections between neighbouring neurons
+    int i = 0;
+    for (const std::shared_ptr<Actuator> &actuator: actuators)
+    {
+        // Get name and x,y-coordinates of all neurons.
+        double x = actuator->coordinate_x();
+        double y = actuator->coordinate_y();
+
+        // Continue to next iteration in case there is already a connection between the 1 and -1 neuron.
+        // These checks feel a bit redundant.
+        // if A->B connection exists.
+        if (this->connections.count({x, y, 1, x, y, -1}) > 0)
+        {
+            continue;
+        }
+        // if B->A connection exists:
+        if (this->connections.count({x, y, -1, x, y, 1}) > 0)
+        {
+            continue;
+        }
+
+        // Loop over all positions. We call it neighbours, but we still need to check if they are a neighbour.
+        for (const std::shared_ptr<Actuator> &neighbour: actuators)
+        {
+            // Get information of this neuron (that we call neighbour).
+            double near_x = neighbour->coordinate_x();
+            double near_y = neighbour->coordinate_y();
+
+            // If there is a node that is a Moore neighbour, we set it to be a neighbour for their A-nodes.
+            // Thus the connections list only contains connections to the A-neighbourhood, and not the
+            // A->B and B->A for some node (which makes sense).
+            double dist_x = std::fabs(x - near_x);
+            double dist_y = std::fabs(y - near_y);
+
+            // TODO: Verify for non-spiders
+            if (std::fabs(dist_x + dist_y - 2) < 0.01)
+            {
+                if(std::get<0>(this->connections[{x, y, 1, near_x, near_y, 1}]) != 1 or
+                   std::get<0>(this->connections[{near_x, near_y, 1, x, y, 1}]) != 1)
+                {
+                    this->connections[{x, y, 1, near_x, near_y, 1}] = std::make_tuple(1, i);
+                    this->connections[{near_x, near_y, 1, x, y, 1}] = std::make_tuple(1, i);
+                    i++;
+                }
+            }
+        }
+    }
+
+    // Initialise array of neuron states for Update() method
+    this->next_state = new double[this->neurons.size()];
+    this->n_weights = (int)(this->connections.size()/2) + this->n_motors;
+
+    // Loading Brain
+
+    // Save weights for brain
+    assert(params.weights.size() == this->n_weights);
+    this->sample.resize(this->n_weights);
+    for(size_t j = 0; j < this->n_weights; j++)
+    {
+        this->sample(j) = params.weights.at(j);
+    }
+
+    // Set ODE matrix at initialization
+    this->set_ode_matrix();
+
+    std::cout << "Brain has been loaded." << std::endl;
+}
+
+/**
+ * Destructor
+ */
+BrokenDifferentialCPG::~BrokenDifferentialCPG()
+{
+    delete[] this->next_state;
+    delete[] this->output;
+}
+
+/**
+ * Callback function that defines the movement of the robot
+ *
+ * @param _motors
+ * @param _sensors
+ * @param _time
+ * @param _step
+ */
+void BrokenDifferentialCPG::update(
+        const std::vector< std::shared_ptr < Actuator > > &actuators,
+        const std::vector< std::shared_ptr < Sensor > > &sensors,
+        const double time,
+        const double step)
+{
+    // Send new signals to the motors
+    this->step(time, step);
+
+    unsigned int p = 0;
+    for (const auto &actuator: actuators)
+    {
+        actuator->write(this->output + p, step);
+        p += actuator->n_outputs();
+    }
+}
+
+/**
+ * Make matrix of weights A as defined in dx/dt = Ax.
+ * Element (i,j) specifies weight from neuron i to neuron j in the system of ODEs
+ */
+void BrokenDifferentialCPG::set_ode_matrix(){
+    // Initiate new matrix
+    std::vector<std::vector<double>> matrix;
+
+    // Fill with zeroes
+    for(size_t i =0; i <this->neurons.size(); i++)
+    {
+        // Initialize row in matrix with zeros
+        std::vector< double > row;
+        for (size_t j = 0; j < this->neurons.size(); j++)
+        {
+            row.push_back(0);
+        }
+        matrix.push_back(row);
+    }
+
+    // Process A<->B connections
+    int index = 0;
+    for(size_t i =0; i <this->neurons.size(); i++)
+    {
+        // Get correct index
+        int c = 0;
+        if (i%2 == 0){
+            c = i + 1;
+        }
+        else{
+            c = i - 1;
+        }
+
+        // Add a/b connection weight
+        index = (int)(i/2);
+        auto w  = this->sample(index) *
+                  (this->range_ub - this->range_lb) + this->range_lb;
+        matrix[i][c] = w;
+        matrix[c][i] = -w;
+    }
+
+    // A<->A connections
+    index++;
+    int k = 0;
+    std::vector<std::string> connections_seen;
+
+    for (auto const &connection : this->connections)
+    {
+        // Get connection information
+        int x1, y1, z1, x2, y2, z2;
+        std::tie(x1, y1, z1, x2, y2, z2) = connection.first;
+
+        // Find location of the two neurons in this->neurons list
+        int l1 = -1;
+        int l2 = -1;
+        int c = 0;
+        for(auto const &neuron : this->neurons)
+        {
+            int x, y, z;
+            std::tie(x, y, z) = neuron.first;
+            if (x == x1 and y == y1 and z == z1)
+            {
+                l1 = c;
+            }
+            else if (x == x2 and y == y2 and z == z2)
+            {
+                l2 = c;
+            }
+            // Update counter
+            c++;
+        }
+
+        // Add connection to seen connections
+        if(l1 > l2)
+        {
+            int l1_old = l1;
+            l1 = l2;
+            l2 = l1_old;
+        }
+        std::string connection_string = std::to_string(l1) + "-" + std::to_string(l2);
+
+        // if not in list, add to list
+        auto connections_list = std::find(connections_seen.begin(), connections_seen.end(), connection_string);
+        if(connections_list == connections_seen.end())
+        {
+            connections_seen.push_back(connection_string);
+        }
+            // else continue to next iteration
+        else{
+            continue;
+        }
+
+        // Get weight
+        auto w  = this->sample(index + k) *
+                  (this->range_ub - this->range_lb) + this->range_lb;
+
+        // Set connection in weight matrix
+        matrix[l1][l2] = w;
+        matrix[l2][l1] = -w;
+        k++;
+    }
+
+    // Update matrix
+    this->ode_matrix = matrix;
+
+    // Reset neuron state
+    this->reset_neuron_state();
+}
+
+
+/**
+ *  Set states back to original value (that is on the unit circle)
+ */
+void BrokenDifferentialCPG::reset_neuron_state(){
+    int c = 0;
+    for(auto const &neuron : this->neurons)
+    {
+        // Get neuron properties
+        int x, y, z, frame_of_reference;
+        double bias ,gain ,state;
+        std::tie(x, y, z) = neuron.first;
+        std::tie(bias, gain, state, frame_of_reference) = neuron.second;
+
+        if (z == -1)
+        {
+            // Neuron B
+            if (this->reset_neuron_random)
+            {
+                this->neurons[{x, y, z}] = {0.f,
+                                            0.f,
+                                            ((double) rand() / (RAND_MAX))*2*this->init_neuron_state - this->init_neuron_state,
+                                            frame_of_reference};
+            }
+            else
+            {
+                this->neurons[{x, y, z}] = {0.f, 0.f, -this->init_neuron_state, frame_of_reference};
+            }
+        }
+        else
+        {
+            // Neuron A
+            if (this->reset_neuron_random)
+            {
+                this->neurons[{x, y, z}] = {0.f,
+                                            0.f,
+                                            ((double) rand() / (RAND_MAX))*2*this->init_neuron_state - this->init_neuron_state,
+                                            frame_of_reference};
+            }
+            else
+            {
+                this->neurons[{x, y, z}] = {0.f, 0.f, +this->init_neuron_state, frame_of_reference};
+            }
+        }
+        c++;
+    }
+}
+
+/**
+ * Step function that is called from within Update()
+ *
+ * @param _time
+ * @param _output
+ */
+void BrokenDifferentialCPG::step(
+        const double time,
+        const double dt)
+{
+    int neuron_count = 0;
+    for (const auto &neuron : this->neurons)
+    {
+        // Neuron.second accesses the second 3-tuple of a neuron, containing the bias/gain/state.
+        double recipient_bias, recipient_gain, recipient_state;
+        int frame_of_reference;
+        std::tie(recipient_bias, recipient_gain, recipient_state, frame_of_reference) = neuron.second;
+
+        // Save for ODE
+        this->next_state[neuron_count] = recipient_state;
+        neuron_count++;
+    }
+
+    // Copy values from next_state into x for ODEINT
+    state_type x(this->neurons.size());
+    for (size_t i = 0; i < this->neurons.size(); i++)
+    {
+        x[i] = this->next_state[i];
+    }
+
+    // Perform one step
+    stepper.do_step(
+            [this](const state_type &x, state_type &dxdt, double t)
+            {
+                for(size_t i = 0; i < this->neurons.size(); i++)
+                {
+                    dxdt[i] = 0;
+                    for(size_t j = 0; j < this->neurons.size(); j++)
+                    {
+                        dxdt[i] += x[j]*this->ode_matrix[j][i];
+                    }
+                }
+            },
+            x,
+            time,
+            dt);
+
+    // Copy values into nextstate
+    for (size_t i = 0; i < this->neurons.size(); i++)
+    {
+        this->next_state[i] = x[i];
+    }
+
+    double angle_difference = 0.0;
+    double slow_down_factor = 1.0;
+    if (use_frame_of_reference) {
+        angle_difference = angle_to_target_sensor->detect_angle();
+        const double frame_of_reference_slower_power = 7.0;
+        slow_down_factor = std::pow(
+                (180.0 - std::abs(angle_difference))/180.0, frame_of_reference_slower_power);
+    }
+
+    // Loop over all neurons to actually update their states. Note that this is a new outer for loop
+    auto i = 0; auto j = 0;
+    for (auto &neuron : this->neurons)
+    {
+        // Get bias gain and state for this neuron. Note that we don't take the coordinates.
+        // However, they are implicit as their order did not change.
+        double bias, gain, state;
+        int frame_of_reference;
+        std::tie(bias, gain, state, frame_of_reference) = neuron.second;
+        double x, y, z;
+        std::tie(x, y, z) = neuron.first;
+        neuron.second = {bias, gain, this->next_state[i], frame_of_reference};
+        j = this->motor_coordinates[{x,y}];
+        // Should be one, as output should be based on +1 neurons, which are the A neurons
+        if (i % 2 == 1)
+        {
+            // TODO: Add Milan's function here as soon as things are working a bit
+            // f(a) = (w_ao*a - bias)*gain
+
+            // Apply saturation formula
+            auto x_input = this->next_state[i];
+
+            double output_j = this->output_function(x_input);
+
+            // Use frame of reference
+            if(use_frame_of_reference and frame_of_reference != 0)
+            {
+                if ((frame_of_reference == 1 and angle_difference < 0) or
+                    (frame_of_reference == -1 and angle_difference > 0)) //TODO >= / <= ?
+                {
+                    output_j *= slow_down_factor;
+                    //std::cout << "Slow down " << x <<','<< y <<','<< z << " with factor " << slow_down_factor << std::endl;
+                    //output_j = 0;
+                }
+            }
+
+            this->output[j] = output_j;
+        }
+        i++;
+    }
+}
+
+double BrokenDifferentialCPG::output_function(double input) const
+{
+    return this->signal_factor_all_
+           * this->abs_output_bound
+           * (
+                   (2.0) / (1.0 + std::pow(2.718, -2.0 * input / this->abs_output_bound))
+                   - 1
+           );
+}
diff --git a/cpprevolve/revolve/brains/controller/BrokenDifferentialCPG.h b/cpprevolve/revolve/brains/controller/BrokenDifferentialCPG.h
new file mode 100644
index 0000000000..799dd2573e
--- /dev/null
+++ b/cpprevolve/revolve/brains/controller/BrokenDifferentialCPG.h
@@ -0,0 +1,147 @@
+//
+// Created by matteo on 14/06/19.
+//
+
+#pragma once
+
+#include "Controller.h"
+#include "actuators/Actuator.h"
+#include "sensors/Sensor.h"
+#include "sensors/AngleToTargetDetector.h"
+#include <map>
+#include <boost/numeric/odeint.hpp>
+#include <Eigen/Geometry>
+
+typedef std::vector< double > state_type;
+
+namespace revolve
+{
+class BrokenDifferentialCPG
+        : public Controller
+{
+public:
+    struct ControllerParams {
+        bool reset_neuron_random;
+        bool use_frame_of_reference;
+        double init_neuron_state;
+        double range_ub;
+        double signal_factor_all;
+        double signal_factor_mid;
+        double signal_factor_left_right;
+        double abs_output_bound;
+        std::vector< double > weights;
+    };
+
+    /// \brief Constructor
+    /// \param[in] _modelName Name of the robot
+    /// \param[in] _node The brain node
+    /// \param[in] _motors Reference to a motor list, it be reordered
+    /// \param[in] _sensors Reference to a sensor list, it might be reordered
+    BrokenDifferentialCPG(
+            const ControllerParams &params,
+            const std::vector< std::shared_ptr < Actuator > > &_actuators,
+            std::shared_ptr<AngleToTargetDetector> angle_to_target_sensor = nullptr);
+
+    /// \brief Destructor
+    virtual ~BrokenDifferentialCPG();
+
+    /// \brief The default update method for the controller
+    /// \param[in] _motors Motor list
+    /// \param[in] _sensors Sensor list
+    /// \param[in] _time Current world time
+    /// \param[in] _step Current time step
+    virtual void update(
+            const std::vector< std::shared_ptr < Actuator > > &actuators,
+            const std::vector< std::shared_ptr < Sensor > > &sensors,
+            const double _time,
+            const double _step) override;
+
+protected:
+
+    void step(
+            const double time,
+            const double step);
+
+    void set_ode_matrix();
+
+private:
+    /// \brief Function that resets neuron state
+    void reset_neuron_state();
+
+    /// \brief function that transforms the value of the CPG A-neurons and returns the correct output for the actuators
+    double output_function(double input) const;
+
+public:
+    std::map< std::tuple< double, double >, size_t > motor_coordinates;
+
+protected:
+    /// \brief Register of motor IDs and their x,y-coordinates
+//    std::map< std::string, std::tuple< int, int > > positions;
+
+    /// \brief Register of individual neurons in x,y,z-coordinates
+    /// \details x,y-coordinates define position of a robot's module and
+    // z-coordinate define A or B neuron (z=1 or -1 respectively). Stored
+    // values are a bias, gain, state and frame of reference of each neuron.
+    std::map< std::tuple< int, int, int >, std::tuple< double, double, double, int > >
+            neurons;
+
+    /// \brief Register of connections between neighnouring neurons
+    /// \details Coordinate set of two neurons (x1, y1, z1) and (x2, y2, z2)
+    // define a connection. The second tuple contains 1: the connection value and
+    // 2: the weight index corresponding to this connection.
+    std::map< std::tuple< int, int, int, int, int, int >, std::tuple<int, int > >
+            connections;
+
+    /// \brief Runge-Kutta 45 stepper
+    boost::numeric::odeint::runge_kutta4< state_type > stepper;
+
+    /// \brief Used for ODE-int
+    std::vector<std::vector<double>> ode_matrix;
+
+private:
+    /// \brief Used to determine the next state array
+    double *next_state;
+
+    /// \brief Used to determine the output to the motors array
+    double *output;
+
+    /// \brief Limbo optimizes in [0,1]
+    double range_lb;
+
+    /// \brief Limbo optimizes in [0,1]
+    double range_ub;
+
+    /// \brief Loaded sample
+    Eigen::VectorXd sample;
+
+    /// \brief The number of weights to optimize
+    size_t n_weights;
+
+    /// \brief Factor to multiply output signal with
+    double signal_factor_all_;
+
+    /// \brief Factor to multiply output signal with
+    double signal_factor_mid;
+
+    /// \brief Factor to multiply output signal with
+    double signal_factor_left_right;
+
+    /// \brief When reset a neuron state,do it randomly:
+    bool reset_neuron_random;
+
+    /// \brief Holds the number of motors in the robot
+    size_t n_motors;
+
+    /// \brief Initial neuron state
+    double init_neuron_state;
+
+    /// \brief Use frame of reference {-1,0,1} version or not
+    bool use_frame_of_reference;
+
+    double abs_output_bound;
+
+// targeted locomotion stuff
+    std::shared_ptr<AngleToTargetDetector> angle_to_target_sensor;
+};
+
+}
diff --git a/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp b/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp
index d355cc7e5d..1223fb69f5 100644
--- a/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp
+++ b/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp
@@ -50,11 +50,13 @@ using namespace revolve;
  */
 DifferentialCPG::DifferentialCPG(
         const DifferentialCPG::ControllerParams &params,
-        const std::vector<std::shared_ptr<Actuator>> &actuators)
+        const std::vector<std::shared_ptr<Actuator>> &actuators,
+        std::shared_ptr<AngleToTargetDetector> angle_to_target_sensor)
         : Controller(ControllerType::DIFFERENTIAL_CPG)
         , next_state(nullptr)
         , n_motors(actuators.size())
         , output(new double[actuators.size()])
+        , angle_to_target_sensor(std::move(angle_to_target_sensor))
         , connection_weights(actuators.size(), 0)
 {
     this->init_params_and_connections(params, actuators);
@@ -82,11 +84,13 @@ DifferentialCPG::DifferentialCPG(
 DifferentialCPG::DifferentialCPG(
         const DifferentialCPG::ControllerParams &params,
         const std::vector<std::shared_ptr<Actuator>> &actuators,
-        const NEAT::Genome &gen)
+        const NEAT::Genome &gen,
+        std::shared_ptr<AngleToTargetDetector> angle_to_target_sensor)
         : Controller(ControllerType::DIFFERENTIAL_CPG)
         , next_state(nullptr)
         , n_motors(actuators.size())
         , output(new double[actuators.size()])
+        , angle_to_target_sensor(std::move(angle_to_target_sensor))
         , connection_weights(actuators.size(), 0)
 {
     this->init_params_and_connections(params, actuators);
@@ -161,12 +165,16 @@ void DifferentialCPG::init_params_and_connections(const ControllerParams &params
     this->range_lb = -params.range_ub;
     this->range_ub = params.range_ub;
     this->use_frame_of_reference = params.use_frame_of_reference;
-    this->signal_factor_all_ = params.signal_factor_all;
-    this->signal_factor_mid = params.signal_factor_mid;
-    this->signal_factor_left_right = params.signal_factor_left_right;
+    this->output_signal_factor = params.output_signal_factor;
     this->abs_output_bound = params.abs_output_bound;
     this->connection_weights = params.weights;
 
+    if (use_frame_of_reference and not angle_to_target_sensor) {
+        std::clog << "WARNING!: use_frame_of_reference is activated but no angle_to_target_sensor camera is configured. "
+                     "Disabling the use of the frame of reference" << std::endl;
+        use_frame_of_reference = false;
+    }
+
     size_t j=0;
     for (const std::shared_ptr<Actuator> &actuator: actuators)
     {
@@ -183,7 +191,7 @@ void DifferentialCPG::init_params_and_connections(const ControllerParams &params
         {
             frame_of_reference = -1;
         }
-            // We are a right neuron
+        // We are a right neuron
         else if (coord_x > 0)
         {
             frame_of_reference = 1;
@@ -323,7 +331,7 @@ void DifferentialCPG::set_ode_matrix()
         matrix.emplace_back(row);
     }
 
-    // Process A<->A connections
+    // Process A<->B connections
     int index = 0;
     for (const Neuron &neuron: neurons)
     {
@@ -349,7 +357,7 @@ void DifferentialCPG::set_ode_matrix()
         index+=2;
     }
 
-    // A<->B connections
+    // A<->A connections
     index++;
     int k = 0;
     std::vector<std::string> connections_seen;
@@ -401,7 +409,7 @@ void DifferentialCPG::set_ode_matrix()
         else // else continue to next iteration
         {
             // actually, we should never encounter this, every connection should appear only once
-            std::cerr << "Should not see the same connection appearing twice" << std::endl;
+            std::cerr << "Should not see the same connection appearing twice: " << connection_string << std::endl;
             throw std::runtime_error("Should not see the same connection appearing twice");
             continue;
         }
@@ -497,11 +505,11 @@ void DifferentialCPG::step(
         neuron_count++;
     }
 
-    // Copy values from next_state into x for ODEINT
-    state_type x(this->neurons.size());
+    // Copy values from next_state into x_state for ODEINT
+    state_type x_state(this->neurons.size());
     for (size_t i = 0; i < this->neurons.size(); i++)
     {
-        x[i] = this->next_state[i];
+        x_state[i] = this->next_state[i];
     }
 
     // Perform one step
@@ -517,14 +525,31 @@ void DifferentialCPG::step(
                     }
                 }
             },
-            x,
+            x_state,
             time,
             dt);
 
     // Copy values into nextstate
     for (size_t i = 0; i < this->neurons.size(); i++)
     {
-        this->next_state[i] = x[i];
+        this->next_state[i] = x_state[i];
+    }
+
+//    // Load the angle value from the sensor
+//    double angle_difference = this->angle_to_goal - move_angle;
+//    if (angle_difference > 180)
+//        angle_difference -= 360;
+//    else if (angle_difference < -180)
+//        angle_difference += 360;
+//    this->angle_diff = angle_difference;
+    double angle_difference = 0.0;
+    double slow_down_factor = 1.0;
+    if (use_frame_of_reference) {
+        angle_difference = angle_to_target_sensor->detect_angle();
+        std::cout << "Angle detected " << angle_difference << std::endl;
+        const double frame_of_reference_slower_power = 7.0;
+        slow_down_factor = std::pow(
+                (180.0 - std::abs(angle_difference))/180.0, frame_of_reference_slower_power);
     }
 
     // Loop over all neurons to actually update their states. Note that this is a new outer for loop
@@ -548,35 +573,30 @@ void DifferentialCPG::step(
             // Apply saturation formula
             auto x_input = this->next_state[i];
 
+            double output_j = this->output_function(x_input);
+
             // Use frame of reference
-            if(use_frame_of_reference)
+            if(use_frame_of_reference and frame_of_reference != 0)
             {
-
-                if (std::abs(frame_of_reference) == 1)
-                {
-                    this->output[j] = this->signal_factor_left_right*this->abs_output_bound*((2.0)/(1.0 + std::pow(2.718, -2.0 * x_input / this->abs_output_bound)) - 1);
-                }
-                else if (frame_of_reference == 0)
+                if ((frame_of_reference == 1 and angle_difference < 0) or
+                    (frame_of_reference == -1 and angle_difference > 0)) //TODO >= / <= ?
                 {
-                    this->output[j] = this->signal_factor_mid*this->abs_output_bound*((2.0)/(1.0 + std::pow(2.718, -2.0 * x_input / this->abs_output_bound)) - 1);
+                    output_j *= slow_down_factor;
                 }
-                else
-                {
-                    std::clog << "WARNING: frame_of_reference not in {-1,0,1}." << std::endl;
-                }
-
-            }
-            // Don't use frame of reference
-            else
-            {
-                this->output[j] = this->signal_factor_all_
-                        * this->abs_output_bound
-                        * (
-                                (2.0) / (1.0 + std::pow(2.718, -2.0 * x_input / this->abs_output_bound))
-                                - 1
-                          );
             }
+
+            this->output[j] = output_j;
         }
         i++;
     }
 }
+
+double DifferentialCPG::output_function(double input) const
+{
+    return this->output_signal_factor
+            * this->abs_output_bound
+            * (
+                    (2.0) / (1.0 + std::pow(2.718, -2.0 * input / this->abs_output_bound))
+                    - 1
+            );
+}
diff --git a/cpprevolve/revolve/brains/controller/DifferentialCPG.h b/cpprevolve/revolve/brains/controller/DifferentialCPG.h
index 377555d52e..c058dd2639 100644
--- a/cpprevolve/revolve/brains/controller/DifferentialCPG.h
+++ b/cpprevolve/revolve/brains/controller/DifferentialCPG.h
@@ -8,6 +8,7 @@
 #include "Controller.h"
 #include "actuators/Actuator.h"
 #include "sensors/Sensor.h"
+#include "sensors/AngleToTargetDetector.h"
 
 #include <map>
 #include <memory>
@@ -27,9 +28,7 @@ class DifferentialCPG
         bool use_frame_of_reference;
         double init_neuron_state;
         double range_ub;
-        double signal_factor_all;
-        double signal_factor_mid;
-        double signal_factor_left_right;
+        double output_signal_factor;
         double abs_output_bound;
         std::vector< double > weights;
         /// can be null, indicating that there is no map
@@ -41,7 +40,8 @@ class DifferentialCPG
     /// \param[in] _actuators Reference to a actuator list
     DifferentialCPG(
             const DifferentialCPG::ControllerParams &params,
-            const std::vector<std::shared_ptr<Actuator>> &_actuators);
+            const std::vector<std::shared_ptr<Actuator>> &_actuators,
+            std::shared_ptr<AngleToTargetDetector> angle_to_target_sensor = nullptr);
 
     /// \brief Constructor for Controller with config CPPN
     /// \param[in] params Parameters for the controller
@@ -50,7 +50,8 @@ class DifferentialCPG
     DifferentialCPG(
             const DifferentialCPG::ControllerParams &params,
             const std::vector<std::shared_ptr<Actuator>> &_actuators,
-            const NEAT::Genome &config_cppn_genome);
+            const NEAT::Genome &config_cppn_genome,
+            std::shared_ptr<AngleToTargetDetector> angle_to_target_sensor = nullptr);
 
     /// \brief Destructor
     virtual ~DifferentialCPG();
@@ -63,8 +64,8 @@ class DifferentialCPG
     virtual void update(
             const std::vector<std::shared_ptr<Actuator>> &actuators,
             const std::vector<std::shared_ptr<Sensor>> &sensors,
-            const double _time,
-            const double _step) override;
+            double _time,
+            double _step) override;
 
     /// \brief Set the connection weights of the Controller and make sure the matrix is set appropriately
     /// \param[in] The weights to be set
@@ -75,9 +76,7 @@ class DifferentialCPG
 
 protected:
 
-    void step(
-            const double time,
-            const double step);
+    void step(double time, double step);
 
     void init_params_and_connections(const ControllerParams &params, const std::vector<std::shared_ptr<Actuator>> &actuators);
 
@@ -87,10 +86,12 @@ class DifferentialCPG
     /// \brief Function that resets neuron state
     void reset_neuron_state();
 
+    /// \brief function that transforms the value of the CPG A-neurons and returns the correct output for the actuators
+    double output_function(double input) const;
+
 public:
     std::map< std::tuple< int, int, int >, size_t > motor_coordinates;
 
-
 protected:
     /// \brief Register of motor IDs and their x,y-coordinates
 //    std::map< std::string, std::tuple< int, int > > positions;
@@ -119,6 +120,9 @@ class DifferentialCPG
     /// \brief Used for ODE-int
     std::vector<std::vector<double>> ode_matrix;
 
+    /// \brief Angle sensor holder
+    std::shared_ptr<::revolve::AngleToTargetDetector> angle_to_target_sensor;
+
 private:
     /// \brief Used to determine the next state array
     double *next_state;
@@ -139,13 +143,7 @@ class DifferentialCPG
     size_t n_weights;
 
     /// \brief Factor to multiply output signal with
-    double signal_factor_all_;
-
-    /// \brief Factor to multiply output signal with
-    double signal_factor_mid;
-
-    /// \brief Factor to multiply output signal with
-    double signal_factor_left_right;
+    double output_signal_factor;
 
     /// \brief When reset a neuron state,do it randomly:
     bool reset_neuron_random;
@@ -161,7 +159,7 @@ class DifferentialCPG
 
     double abs_output_bound;
 
-    };
+};
 
 }
 
diff --git a/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.cpp b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.cpp
new file mode 100644
index 0000000000..cefac09940
--- /dev/null
+++ b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.cpp
@@ -0,0 +1,355 @@
+//
+// Created by matteo on 2/28/20.
+//
+
+#include "AngleToTargetDetector.h"
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <iostream>
+#include <cmath>
+
+revolve::AngleToTargetDetector::AngleToTargetDetector(const unsigned int shrink_factor, const bool show_image)
+    : Sensor(1)
+    , show_image(show_image)
+    , shrink_factor(shrink_factor)
+//    , angle(std::atan(img.cols/img.rows) * 180 / M_PI)
+    , angle(NAN)
+{}
+
+void revolve::AngleToTargetDetector::read(double *input)
+{
+    input[0] = detect_angle();
+}
+
+float revolve::AngleToTargetDetector::detect_angle()
+{
+    get_image(raw_image);
+    unsigned int image_cols = raw_image.cols/shrink_factor;
+    unsigned int image_rows = raw_image.rows/shrink_factor;
+    cv::resize(raw_image, image, cv::Size(image_cols, image_rows));
+
+    cv::medianBlur(image, image_blur, 5);
+    cv::cvtColor(image_blur, image_hsv, cv::COLOR_BGR2HSV);
+
+    //green
+    const int gLowH1=35,gHighH1=40,gLowH2=41,gHighH2=59,gLowS1=140,gLowS2=69,gHighS=255,gLowV=104,gHighV=255;
+    //blue
+    const int bLowH=99,bHighH=121,bLowS=120,bHighS=255,bLowV=57,bHighV=211;
+
+    //detecting Blue
+    cv::inRange(image_hsv, cv::Scalar(bLowH,bLowS, bLowV), cv::Scalar(bHighH,bHighS, bHighV)   ,image_blue);
+    //detecting Green
+    cv::inRange(image_hsv, cv::Scalar(gLowH1,gLowS1, gLowV), cv::Scalar(gHighH1,gHighS, gHighV),image_green1);
+    cv::inRange(image_hsv, cv::Scalar(gLowH2,gLowS2, gLowV), cv::Scalar(gHighH2,gHighS, gHighV),image_green2);
+    cv::add(image_green1, image_green2, image_green);
+
+    std::vector<std::vector<cv::Point>> contours_blue, contours_green;; //contours_red, contours_yellow;
+
+    cv::findContours(image_blue, contours_blue, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
+    cv::findContours(image_green, contours_green, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
+    //cv::findContours(image_red, contours_red, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
+    //cv::findContours(image_yellow, contours_yellow, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
+
+    std::vector<cv::Rect> rect_coord, rect_coord_blue, rect_coord_green; //rect_coord_red, rect_coord_yellow;
+
+    // blue contours
+    for (const std::vector<cv::Point> &contours_blue_line : contours_blue) {
+        double image_blue_area_buf = cv::contourArea(contours_blue_line);
+
+        if (image_blue_area_buf > 5) {
+            cv::Rect bounding_rect = cv::boundingRect(contours_blue_line);
+            rect_coord_blue.emplace_back(bounding_rect);
+        }
+    }
+
+    // green contours
+    for (const std::vector<cv::Point> &contours_green_line : contours_green) {
+        double image_blue_area_buf = cv::contourArea(contours_green_line);
+
+        if (image_blue_area_buf > 5) {
+            cv::Rect bounding_rect = cv::boundingRect(contours_green_line);
+            rect_coord_green.emplace_back(bounding_rect);
+        }
+    }
+
+    //// red contours
+    //for (const std::vector<cv::Point> &contours_red_line : contours_red) {
+    //    double image_blue_area_buf = cv::contourArea(contours_red_line);
+    //
+    //    if (image_blue_area_buf > 5) {
+    //        cv::Rect bounding_rect = cv::boundingRect(contours_red_line);
+    //        rect_coord_red.emplace_back(bounding_rect);
+    //    }
+    //}
+    //
+    //// yellow contours
+    //for (const std::vector<cv::Point> &contours_yellow_line : contours_yellow) {
+    //    double image_blue_area_buf = cv::contourArea(contours_yellow_line);
+    //
+    //    if (image_blue_area_buf > 5) {
+    //        cv::Rect bounding_rect = cv::boundingRect(contours_yellow_line);
+    //        rect_coord_yellow.emplace_back(bounding_rect);
+    //    }
+    //}
+
+    rect_coord.reserve( rect_coord_blue.size() + rect_coord_green.size() ); // preallocate memory
+    // + rect_coord_red.size() + rect_coord_yellow.size()
+    rect_coord.insert( rect_coord.end(), rect_coord_blue.begin(), rect_coord_blue.end() );
+    rect_coord.insert( rect_coord.end(), rect_coord_green.begin(), rect_coord_green.end() );
+    //rect_coord.insert( rect_coord.end(), rect_coord_red.begin(), rect_coord_red.end() );
+    //rect_coord.insert( rect_coord.end(), rect_coord_yellow.begin(), rect_coord_yellow.end() );
+
+// ----- MAGIC GONGJIN CODE HERE ----------------------------------------------
+    unsigned int num = rect_coord.size();
+    int distanceBox[num][num], distanceBoxSum[num], numBox[num], minDistanceBox[num], min2DistanceBox[num],rectBoxHeight = 0, rectBoxHeightMax = 0;
+    for (int i = 0; i < num; i++) //calculating the suitable(medium) value of height
+    {
+        if (rect_coord[i].height > rectBoxHeightMax)
+        {
+            rectBoxHeight = rectBoxHeightMax; // set this value as the height of box
+            rectBoxHeightMax = rect_coord[i].height;
+        }
+        else if (rect_coord[i].height > rectBoxHeight)
+            rectBoxHeight = rect_coord[i].height;
+    }
+
+    for (int j = 0; j < num; j++) //calculating the value of minimum and the second minimum distance for each box
+    {
+        minDistanceBox[j] = 800;
+        min2DistanceBox[j] = 800;
+        for (int x = 0; x < num; x++)
+        {
+            if (j != x)
+            {
+                distanceBox[j][x] = std::min(
+                        std::abs(rect_coord[j].tl().x - rect_coord[x].br().x),
+                        std::abs(rect_coord[j].br().x - rect_coord[x].tl().x)
+                );
+
+                if (distanceBox[j][x] < minDistanceBox[j])
+                {
+                    min2DistanceBox[j] = minDistanceBox[j]; //the second minimum distance
+                    minDistanceBox[j] = distanceBox[j][x]; //the minimun distance
+                }
+                else if (distanceBox[j][x] < min2DistanceBox[j])
+                {
+                    min2DistanceBox[j] = distanceBox[j][x];
+                }
+            }
+        }
+        distanceBoxSum[j] = minDistanceBox[j] + min2DistanceBox[j];
+    }
+
+    for (int i =0; i < num; i++) //sequence from minimum distance to maximum distance
+    {
+        numBox[i] = 0;
+        for (int j=0; j < num; j++)
+        {
+            if (i != j) // get the Box[i] sequence
+            {
+                if (distanceBoxSum[i] > distanceBoxSum[j])
+                    numBox[i]+=1; //numBox[i] = numBox[i] +1, save the number
+                if (distanceBoxSum[i] == distanceBoxSum[j])
+                {
+                    if (minDistanceBox[i] >= minDistanceBox[j]) //always have the same distance between two points each other
+                        numBox[i]+=1; //
+                }
+            }
+        }
+    }
+    //-------------difine the ROIs of robot------------
+    int lastnum = num, robNum, minRectCoorX[num], minRectCoorY[num], maxRectCoorX[num], maxRectCoorY[num];
+    for (robNum = 0; lastnum >= 2 && robNum < num; robNum++)
+    {
+        int minNumBox=100;
+        for (int k = 0; k <num; k++) //get the minNumBox between the rest
+        {
+            minNumBox = std::min(numBox[k], minNumBox);
+        }
+        for (int i = 0; i < num; i++) //get the coordination of rectangle of robot from boxes
+        {
+            if (numBox[i] == minNumBox) //find the minimum one between the rest (usually it is 0 when 1 robot)
+            {
+                lastnum --;
+                if (num > 2) //when robot only have 2 boxes at least, just combine the two boxes
+                    numBox[i] = 100; //make it not included in the rest
+                minRectCoorX[robNum] = rect_coord[i].tl().x;
+                minRectCoorY[robNum] = rect_coord[i].tl().y;
+                maxRectCoorX[robNum] = rect_coord[i].br().x;
+                maxRectCoorY[robNum] = rect_coord[i].br().y;
+                int bufnum = 0, jBox[50] = {0};
+                for (int j = 0; j < num; j++) //calculating the coordination of rectangle incluing boxes belong to the distance area
+                {
+                    //-------------the first threshold condition-------------------
+                    if (j != i && numBox[j] != 100 && distanceBox[i][j] < 4.3 * rectBoxHeight) //3.4, 3.5, 4.5, 4.3 justify if the box belong to the same robot by distance of boxeswith the center box
+                    {
+                        jBox[bufnum] = j;
+                        lastnum --;
+                        bufnum ++; //the number of boxes that match the threshold of (distanceBox[i][j] < 3.4 * rectBoxHeight)
+                    }
+                    //----calculating the max distance between boxes after the first threshold condition, preparing for next--------
+                    if (j == num - 1 && bufnum >= 1) //bufnum >= 1 (it have two candidate at least)
+                    {
+                        int maxBoxDisOut[num], max_in_out[num][num],maxBoxDisOutNum[num];
+                        for (int buf = 0; buf < bufnum; buf++) //calculating the max distance between boxes in jBox[bufnum]
+                        {
+                            maxBoxDisOut[jBox[buf]] = 0;
+                            int rectCoor_tl_br, rectCoor_br_tl;
+                            if (bufnum == 1) // one other box and one center box
+                            {
+                                rectCoor_tl_br = std::abs(rect_coord[i].tl().x - rect_coord[jBox[0]].br().x); //calculating the inside or outside distance between the same boxes
+                                rectCoor_br_tl = std::abs(rect_coord[i].br().x - rect_coord[jBox[0]].tl().x); //calculating the inside or outside distance between the same boxes
+                                maxBoxDisOut[jBox[0]] = std::min(rectCoor_tl_br,rectCoor_br_tl); //max, min
+                            }
+                            else
+                            {
+                                for (int buff = 0; buff < bufnum; buff++)
+                                {
+                                    rectCoor_tl_br = std::abs(rect_coord[jBox[buf]].tl().x - rect_coord[jBox[buff]].br().x); //calculating the inside or outside distance between the same boxes
+                                    rectCoor_br_tl = std::abs(rect_coord[jBox[buf]].br().x - rect_coord[jBox[buff]].tl().x); //calculating the inside or outside distance between the same boxes
+                                    max_in_out[jBox[buf]][jBox[buff]] = std::min(rectCoor_tl_br,rectCoor_br_tl); //max,min
+                                    if (max_in_out[jBox[buf]][jBox[buff]] > maxBoxDisOut[jBox[buf]])
+                                    {
+                                        maxBoxDisOut[jBox[buf]] = max_in_out[jBox[buf]][jBox[buff]];
+                                        maxBoxDisOutNum[buf] = jBox[buff];
+                                    }
+                                }
+                            }
+                        }
+                        //bufnum >1 guarantte the robot have center box and two other box (bufnum=2) at least, or not go to compare center box and another one box
+                        if (bufnum >= 2)
+                        {
+                            int delNum = 0;
+                            for (int bufff = 0; bufff < bufnum; bufff++) //compare the max distance (robot size from left to right) of boxes in jBox[bufnum]
+                            {
+                                if (maxBoxDisOut[jBox[bufff]] < 6.2 * rectBoxHeight) //if > the length of robot, delete far one, get the near one as rectangle
+                                {
+                                    minRectCoorX[robNum] = std::min(rect_coord[jBox[bufff]].tl().x, minRectCoorX[robNum]);
+                                    minRectCoorY[robNum] = std::min(rect_coord[jBox[bufff]].tl().y, minRectCoorY[robNum]);
+                                    maxRectCoorX[robNum] = std::max(rect_coord[jBox[bufff]].br().x, maxRectCoorX[robNum]);
+                                    maxRectCoorY[robNum] = std::max(rect_coord[jBox[bufff]].br().y, maxRectCoorY[robNum]);
+                                    numBox[jBox[bufff]] = 100; //set a constant not zero and more than all of the numBox
+                                }
+                                //TODO this else if is doing exactly the same code as above, remove it
+                                else if (distanceBox[i][jBox[bufff]] < distanceBox[i][maxBoxDisOutNum[bufff]]) //always have two boxes match this condition at the same time, choice one of them
+                                {
+                                    minRectCoorX[robNum] = std::min(rect_coord[jBox[bufff]].tl().x, minRectCoorX[robNum]);
+                                    minRectCoorY[robNum] = std::min(rect_coord[jBox[bufff]].tl().y, minRectCoorY[robNum]);
+                                    maxRectCoorX[robNum] = std::max(rect_coord[jBox[bufff]].br().x, maxRectCoorX[robNum]);
+                                    maxRectCoorY[robNum] = std::max(rect_coord[jBox[bufff]].br().y, maxRectCoorY[robNum]);
+                                    numBox[jBox[bufff]] = 100; //set a constant not zero and more than all of the numBox
+                                }
+                                else
+                                {
+                                    minRectCoorX[robNum] = std::min(rect_coord[maxBoxDisOutNum[bufff]].tl().x, minRectCoorX[robNum]);
+                                    minRectCoorY[robNum] = std::min(rect_coord[maxBoxDisOutNum[bufff]].tl().y, minRectCoorY[robNum]);
+                                    maxRectCoorX[robNum] = std::max(rect_coord[maxBoxDisOutNum[bufff]].br().x, maxRectCoorX[robNum]);
+                                    maxRectCoorY[robNum] = std::max(rect_coord[maxBoxDisOutNum[bufff]].br().y, maxRectCoorY[robNum]);
+                                    numBox[maxBoxDisOutNum[bufff]] = 100;
+                                    delNum ++;
+                                }
+                            }
+                            lastnum = lastnum + delNum; //plus for the cancelled more one
+                            bufnum = bufnum - delNum;
+                        }
+                        else //compare center box and another one box, when bufnum = 1
+                        {
+                            if (maxBoxDisOut[jBox[0]] < 6.2 * rectBoxHeight) //the length of robot 9.4
+                            {
+                                minRectCoorX[robNum] = std::min(rect_coord[jBox[0]].tl().x, minRectCoorX[robNum]);
+                                minRectCoorY[robNum] = std::min(rect_coord[jBox[0]].tl().y, minRectCoorY[robNum]);
+                                maxRectCoorX[robNum] = std::max(rect_coord[jBox[0]].br().x, maxRectCoorX[robNum]);
+                                maxRectCoorY[robNum] = std::max(rect_coord[jBox[0]].br().y, maxRectCoorY[robNum]);
+                                numBox[jBox[0]] = 100; //set a constant not zero and more than all of the numBox
+                            }
+                            else //just one center to rest
+                            {
+                                robNum --;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    // calculate the angle
+    if (std::isnan(angle) and robNum == 0)
+    {
+        // init first angle
+        angle = atan(image.cols / static_cast<double>(image.rows)) * 180.0 / M_PI;
+    }
+    else
+    {
+        for (int i = 0; i < robNum; i++)
+        {
+            const int robCenterCoorX = 2*(minRectCoorX[i] + maxRectCoorX[i]);
+            const int robCenterCoorY = 2*(minRectCoorY[i] + maxRectCoorY[i]);
+            char textRobCenterCoor[64], textDistance[64];
+
+            if (show_image) {
+                cv::rectangle(raw_image, cv::Point(shrink_factor*minRectCoorX[i],shrink_factor*minRectCoorY[i]), cv::Point(shrink_factor*maxRectCoorX[i],shrink_factor*maxRectCoorY[i]), cv::Scalar(0,255,0),1);
+                cv::circle(raw_image, cv::Point(robCenterCoorX,robCenterCoorY),3, cv::Scalar(0,255,0),4);
+
+                std::snprintf(textRobCenterCoor, sizeof(textRobCenterCoor), "(%d,%d)", robCenterCoorX, robCenterCoorY);
+                cv::putText(raw_image, textRobCenterCoor, cv::Point(robCenterCoorX + 10, robCenterCoorY + 3),
+                        cv::FONT_HERSHEY_DUPLEX, 0.4, cv::Scalar(0, 255, 0), 1);
+            }
+
+            const int leftLine = raw_image.cols / 2;
+            const int rightLine = raw_image.cols / 2;
+            if (robCenterCoorX < leftLine)
+            {
+                double distance = robCenterCoorX - leftLine;
+                angle = std::atan(distance/robCenterCoorY) * 180.0 / M_PI;
+                if (show_image) {
+                    std::snprintf(textDistance, sizeof(textDistance), "L:%f Angle: %f", distance, angle);
+                    cv::putText(raw_image, textDistance, cv::Point(0.0 * raw_image.cols, 15), cv::FONT_HERSHEY_DUPLEX, 0.5,
+                            cv::Scalar(0, 255, 0), 1);
+                }
+            }
+
+            if (robCenterCoorX > rightLine)
+            {
+                double distance = robCenterCoorX - rightLine;
+                angle = std::atan(distance/robCenterCoorY) * 180.0 / M_PI;
+                if (show_image) {
+                    std::snprintf(textDistance, sizeof(textDistance), "R:%f Angle: %f", distance, angle);
+                    cv::putText(raw_image, textDistance, cv::Point(0.5 * raw_image.cols, 15), cv::FONT_HERSHEY_DUPLEX, 0.5,
+                            cv::Scalar(0, 255, 0), 1);
+                }
+            }
+
+            if (show_image) {
+                cv::line(raw_image, cv::Point(shrink_factor * minRectCoorX[i], shrink_factor * minRectCoorY[i]),
+                         cv::Point(shrink_factor * maxRectCoorX[i], shrink_factor * maxRectCoorY[i]), cv::Scalar(0, 255, 0), 1);
+                cv::line(raw_image, cv::Point(shrink_factor * minRectCoorX[i], shrink_factor * maxRectCoorY[i]),
+                         cv::Point(shrink_factor * maxRectCoorX[i], shrink_factor * minRectCoorY[i]), cv::Scalar(0, 255, 0), 1);
+                cv::line(raw_image, cv::Point(leftLine, 0), cv::Point(leftLine, raw_image.rows), cv::Scalar(0, 255, 0), 1);
+                cv::line(raw_image, cv::Point(rightLine, 0), cv::Point(rightLine, raw_image.rows), cv::Scalar(0, 255, 0), 1);
+            }
+        }
+    }
+
+
+    if (robNum == 0 and show_image) // no robots in the field of view
+    {
+        // show image if no robot is detected
+        char textDistance[64];
+        float text_pos;
+        if (angle < 0) text_pos = 0.0;
+        else text_pos = 0.5;
+        std::snprintf(textDistance, sizeof(textDistance), "Angle: %f", angle);
+        std::snprintf(textDistance, sizeof(textDistance), "Angle: %f", angle);
+        cv::putText(raw_image, textDistance, cv::Point(text_pos * raw_image.cols, 15), cv::FONT_HERSHEY_DUPLEX, 0.5,
+                    cv::Scalar(255, 0, 0), 1);
+    }
+
+    assert(not std::isnan(angle));
+
+    if (show_image) {
+        cv::imshow("revolve-controller", raw_image);
+        cv::waitKey(5);
+    }
+    return angle;
+}
diff --git a/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.h b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.h
new file mode 100644
index 0000000000..4ac5016db9
--- /dev/null
+++ b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.h
@@ -0,0 +1,35 @@
+//
+// Created by matteo on 2/28/20.
+//
+
+#ifndef REVOLVE_ANGLETOTARGETDETECTOR_H
+#define REVOLVE_ANGLETOTARGETDETECTOR_H
+
+#include "Sensor.h"
+#include <opencv2/core/core.hpp>
+
+namespace revolve {
+
+class AngleToTargetDetector : public Sensor {
+public:
+    explicit AngleToTargetDetector(unsigned int shrink_factor = 4, bool show_image = false);
+    virtual ~AngleToTargetDetector() = default;
+
+    void read(double *input) override;
+    virtual float detect_angle();
+
+private:
+    virtual void get_image(cv::Mat &image) = 0;
+
+protected:
+    const bool show_image;
+    const unsigned int shrink_factor;
+    double angle;
+    cv::Mat raw_image, image;
+    cv::Mat image_blur, image_hsv, image_blue, image_green1, image_green2, image_green;
+};
+
+}
+
+
+#endif //REVOLVE_ANGLETOTARGETDETECTOR_H
diff --git a/cpprevolve/revolve/brains/controller/sensors/AngleToTargetSensor.h b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetSensor.h
new file mode 100644
index 0000000000..5059c6eac0
--- /dev/null
+++ b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetSensor.h
@@ -0,0 +1,29 @@
+//
+// Created by Matteo De Carlo on 25/02/2020.
+//
+
+#ifndef REVOLVE_ANGLETOTARGETSENSOR_H
+#define REVOLVE_ANGLETOTARGETSENSOR_H
+
+#include "Sensor.h"
+
+namespace revolve
+{
+
+class AngleToTargetSensor : public Sensor {
+public:
+    explicit AngleToTargetSensor()
+        : Sensor(1)
+    {}
+
+    virtual double angle_to_target() = 0;
+
+    void read(double *input) override
+    {
+        *input = angle_to_target();
+    }
+};
+
+}
+
+#endif //REVOLVE_ANGLETOTARGETSENSOR_H
diff --git a/cpprevolve/revolve/brains/controller/sensors/Sensor.h b/cpprevolve/revolve/brains/controller/sensors/Sensor.h
index 3f293da27f..f2678ac3e1 100644
--- a/cpprevolve/revolve/brains/controller/sensors/Sensor.h
+++ b/cpprevolve/revolve/brains/controller/sensors/Sensor.h
@@ -14,6 +14,8 @@ class Sensor
         : _n_inputs(n_inputs)
     {}
 
+    virtual ~Sensor() = default;
+
     /// \brief Read the value of the sensor into the
     /// \param[in] _input: array.
     /// \brief[in,out] _input Input value to write on
diff --git a/cpprevolve/revolve/brains/learner/BayesianOptimizer.cpp b/cpprevolve/revolve/brains/learner/BayesianOptimizer.cpp
index ff66683e74..27315a7105 100644
--- a/cpprevolve/revolve/brains/learner/BayesianOptimizer.cpp
+++ b/cpprevolve/revolve/brains/learner/BayesianOptimizer.cpp
@@ -9,9 +9,9 @@
 #include <typeinfo>
 
 #include "BayesianOptimizer.h"
+#include "BoDefinitions.h"
 #include "../controller/DifferentialCPG.h"
 #include "../controller/Controller.h"
-#include "BoDefinitions.h"
 
 using namespace revolve;
 
diff --git a/cpprevolve/revolve/brains/learner/BoDefinitions.h b/cpprevolve/revolve/brains/learner/BoDefinitions.h
index 8b32af8806..115b9be24c 100644
--- a/cpprevolve/revolve/brains/learner/BoDefinitions.h
+++ b/cpprevolve/revolve/brains/learner/BoDefinitions.h
@@ -124,7 +124,7 @@ namespace limbo {
 
                 while (!this->_stop(*this, afun)) {
 
-                    gettimeofday(&timeStart,NULL);
+                    gettimeofday(&timeStart, nullptr);
 
                     acquisition_function_t acqui(_model, this->_current_iteration);
 
@@ -149,7 +149,7 @@ namespace limbo {
                     this->_current_iteration++;
                     this->_total_iterations++;
 
-                    gettimeofday(&timeEnd,NULL);
+                    gettimeofday(&timeEnd, nullptr);
 
                     timeDiff = 1000000 * (timeEnd.tv_sec - timeStart.tv_sec)
                                + timeEnd.tv_usec - timeStart.tv_usec; //tv_sec: value of second, tv_usec: value of microsecond
diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp
index 9db1ef6561..1b71192c64 100644
--- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp
+++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp
@@ -57,9 +57,7 @@ revolve::DifferentialCPG::ControllerParams DifferentialCPG::load_params_from_sdf
     params.use_frame_of_reference = (controller_sdf->GetAttribute("use_frame_of_reference")->GetAsString() == "true");
     params.init_neuron_state = stod(controller_sdf->GetAttribute("init_neuron_state")->GetAsString());
     params.range_ub = stod(controller_sdf->GetAttribute("range_ub")->GetAsString());
-    params.signal_factor_all = stod(controller_sdf->GetAttribute("signal_factor_all")->GetAsString());
-    params.signal_factor_mid = stod(controller_sdf->GetAttribute("signal_factor_mid")->GetAsString());
-    params.signal_factor_left_right = stod(controller_sdf->GetAttribute("signal_factor_left_right")->GetAsString());
+    params.output_signal_factor = stod(controller_sdf->GetAttribute("output_signal_factor")->GetAsString());
     params.abs_output_bound = stod(controller_sdf->GetAttribute("abs_output_bound")->GetAsString());
 
     // Get the weights from the sdf:
diff --git a/cpprevolve/revolve/raspberry/AngleToTargetDetector.cpp b/cpprevolve/revolve/raspberry/AngleToTargetDetector.cpp
new file mode 100644
index 0000000000..3576830bc4
--- /dev/null
+++ b/cpprevolve/revolve/raspberry/AngleToTargetDetector.cpp
@@ -0,0 +1,56 @@
+//
+// Created by matteo on 2/28/20.
+//
+
+#include "AngleToTargetDetector.h"
+#include <iostream>
+#include <opencv2/imgproc/imgproc.hpp>
+
+using namespace revolve::raspberry;
+
+AngleToTargetDetector::AngleToTargetDetector(int camera_index, unsigned int shrink_factor, bool show_image)
+    : ::revolve::AngleToTargetDetector(shrink_factor, show_image)
+    , camera(nullptr)
+    , usb_camera(nullptr)
+{
+    if (camera_index == -1) {
+        camera = new raspicam::RaspiCam_Cv();
+        if (not camera->open())
+            throw std::runtime_error("Error opening the raspberry camera");
+    } else {
+        usb_camera = new cv::VideoCapture(camera_index);
+        if (not usb_camera->isOpened())
+            throw std::runtime_error("Error opening the usb camera at index " + std::to_string(camera_index));
+    }
+}
+
+AngleToTargetDetector::~AngleToTargetDetector()
+{
+    delete camera;
+    delete usb_camera;
+}
+
+void AngleToTargetDetector::get_image(cv::Mat &raw_image)
+{
+    bool result = false;
+
+    if (camera) {
+        result = camera->grab();
+        camera->retrieve(raw_image);
+    } else if (usb_camera) {
+        result = usb_camera->read(raw_image);
+    } else {
+        throw std::runtime_error("Camera device not found");
+    }
+
+    if (not result or raw_image.empty()) {
+        throw std::runtime_error("Error! could not capture image from the camera");
+    }
+}
+
+float AngleToTargetDetector::detect_angle()
+{
+    float angle = ::revolve::AngleToTargetDetector::detect_angle();
+    std::cout << "Detected angle: " << angle << std::endl;
+    return angle;
+}
diff --git a/cpprevolve/revolve/raspberry/AngleToTargetDetector.h b/cpprevolve/revolve/raspberry/AngleToTargetDetector.h
new file mode 100644
index 0000000000..cac8d20835
--- /dev/null
+++ b/cpprevolve/revolve/raspberry/AngleToTargetDetector.h
@@ -0,0 +1,38 @@
+//
+// Created by matteo on 2/28/20.
+//
+
+#ifndef REVOLVE_RASPBERRY_ANGLETOTARGETDETECTOR_H
+#define REVOLVE_RASPBERRY_ANGLETOTARGETDETECTOR_H
+
+#include <opencv2/core/core.hpp>
+#include <raspicam/raspicam_cv.h>
+#include "../brains/controller/sensors/AngleToTargetDetector.h"
+
+namespace revolve {
+namespace raspberry {
+
+class AngleToTargetDetector : public ::revolve::AngleToTargetDetector {
+public:
+    /**
+     * @param camera_index pass -1 to load the raspberry camera
+     */
+    explicit AngleToTargetDetector(int camera_index, unsigned int shrink_factor = 4, bool show_image = false);
+    virtual ~AngleToTargetDetector();
+
+public:
+    float detect_angle() override;
+
+private:
+    void get_image(cv::Mat &image) override;
+
+protected:
+    raspicam::RaspiCam_Cv *camera;
+    cv::VideoCapture *usb_camera;
+};
+
+}
+}
+
+
+#endif // REVOLVE_RASPBERRY_ANGLETOTARGETDETECTOR_H
diff --git a/cpprevolve/revolve/raspberry/CMakeLists.txt b/cpprevolve/revolve/raspberry/CMakeLists.txt
index 20101930cf..ecc2d7c2a3 100644
--- a/cpprevolve/revolve/raspberry/CMakeLists.txt
+++ b/cpprevolve/revolve/raspberry/CMakeLists.txt
@@ -2,6 +2,8 @@ message(WARNING "Building Raspberry code")
 
 # Find Yaml-cpp
 find_package(yaml-cpp REQUIRED)
+find_package(raspicam REQUIRED)
+
 include_directories(${YAML_CPP_INCLUDE_DIR})
 
 file(GLOB_RECURSE
@@ -15,9 +17,11 @@ target_link_libraries(revolve-raspberry
         PUBLIC revolve-controllers
         PUBLIC pigpio_if2
         ${YAML_CPP_LIBRARIES}
+        ${raspicam_CV_LIBS}
 )
 
-include_directories(${PIGPIO_HEADER_DIR})
+target_include_directories(revolve-raspberry
+        PUBLIC ${PIGPIO_HEADER_DIR})
 
 install(TARGETS revolve-raspberry
         RUNTIME DESTINATION bin
diff --git a/cpprevolve/revolve/raspberry/PIGPIOConnection.h b/cpprevolve/revolve/raspberry/PIGPIOConnection.h
index f0239d0272..474c0724ff 100644
--- a/cpprevolve/revolve/raspberry/PIGPIOConnection.h
+++ b/cpprevolve/revolve/raspberry/PIGPIOConnection.h
@@ -2,8 +2,8 @@
 // Created by matteo on 14/06/19.
 //
 
-#ifndef REVOLVE_PIGPIOCONNECTION_H
-#define REVOLVE_PIGPIOCONNECTION_H
+#ifndef REVOLVE_RASPBERRY_PIGPIOCONNECTION_H
+#define REVOLVE_RASPBERRY_PIGPIOCONNECTION_H
 
 extern "C" {
 #include "pigpiod_if2.h"
@@ -47,4 +47,4 @@ class PIGPIOConnection {
 };
 
 
-#endif //REVOLVE_PIGPIOCONNECTION_H
+#endif //REVOLVE_RASPBERRY_PIGPIOCONNECTION_H
diff --git a/cpprevolve/revolve/raspberry/RaspController.cpp b/cpprevolve/revolve/raspberry/RaspController.cpp
index 1a424b2fc0..f98bb16097 100644
--- a/cpprevolve/revolve/raspberry/RaspController.cpp
+++ b/cpprevolve/revolve/raspberry/RaspController.cpp
@@ -3,31 +3,36 @@
 //
 
 #include "RaspController.h"
+#include <chrono>
+#include <memory>
+#include <thread>
 #include "../brains/controller/DifferentialCPG.h"
+#include "../brains/controller/BrokenDifferentialCPG.h"
+#include "AngleToTargetDetector.h"
 #include <yaml-cpp/yaml.h>
 
-using namespace revolve;
+using namespace revolve::raspberry;
 
 RaspController::RaspController(
-        std::vector<std::unique_ptr<Actuator> > actuators,
-        std::vector<std::unique_ptr<Sensor> > sensors,
+        std::vector<std::shared_ptr<Actuator> > actuators,
+        std::vector<std::shared_ptr<Sensor> > sensors,
         const YAML::Node &conf)
     : revolve_controller(nullptr)
+    , camera(nullptr)
     , actuators(std::move(actuators))
     , sensors(std::move(sensors))
 {
+    // camera can be nullptr
+    this->load_camera(conf["angle_to_target_detector"]);
     this->set_new_controller(conf);
 }
 
 RaspController::~RaspController() = default;
 
-#include <chrono>
-#include <thread>
-
 void RaspController::update()
 {
-    double step = this->timer.step();
-    double time = this->timer.elapsed();
+    const double step = this->timer.step_double();
+    const double time = this->timer.elapsed_double();
     if (step == 0)
         return;
     this->revolve_controller->update(
@@ -36,33 +41,89 @@ void RaspController::update()
         time,
         step
     );
-//    std::this_thread::sleep_for(std::chrono::milliseconds(125));
+
+    // negative update rate means run as fast as possible
+    if (this->update_rate <= std::chrono::milliseconds::zero()) return;
+
+    const Timer::Seconds cpu_time_spent = this->timer.step_elapsed();
+    const Timer::Seconds remaining_wait_time = this->update_rate - cpu_time_spent;
+    if (remaining_wait_time > Timer::Seconds::zero()) {
+        std::this_thread::sleep_for(remaining_wait_time);
+    } else {
+        std::clog << "CPU too slow, we missed the cycle deadline of " << (remaining_wait_time).count() / -1000  << " ms " << std::endl;
+    }
+}
+
+void RaspController::load_camera(const YAML::Node &conf)
+{
+    if (not conf) {
+        std::cout << "Camera not found, the camera will be deactivated." << std::endl;
+        return;
+    }
+
+    int camera_index;
+    std::string camera_type = conf["type"].as<std::string>();
+    if (camera_type == "raspberry-camera") {
+        camera_index = -1;
+        std::cout << "Loading Raspberry camera" << std::endl;
+    } else if (camera_type == "usb") {
+        camera_index = conf["index"].as<int>();
+        std::cout << "Loading usb camera at index " << camera_index << std::endl;
+    } else {
+        throw std::runtime_error("Camera type " + camera_type + "not recognized");
+    }
+
+    const unsigned int camera_shrink_factor = conf["shrink_factor"].as<unsigned int>(4);
+    const bool show_camera = conf["show_camera"].as<bool>(false);
+
+    std::cout << "Camera shrink_factor( " << camera_shrink_factor << " ) show_camera( " << (show_camera ? "true" : "false") << " )" << std::endl;
+
+    camera.reset(new ::revolve::raspberry::AngleToTargetDetector(camera_index, camera_shrink_factor, show_camera));
 }
 
 void RaspController::set_new_controller(const YAML::Node &conf)
 {
+    // Update rate in ms
+    int64_t _update_rate = conf["update_rate"].as<int64_t>(-1);
+    this->update_rate = std::chrono::duration_cast<Timer::Seconds>(std::chrono::milliseconds(_update_rate));
+
     std::string type = conf["type"].as<std::string>("");
     if (type.empty()) {
         throw std::runtime_error("Controller type not set");
+    } else if (type == "broken-differential-cpg") {
+        BrokenDifferentialCPG::ControllerParams params;
+        params.reset_neuron_random = conf["reset_neuron_random"].as<bool>(false);
+        params.use_frame_of_reference = conf["use_frame_of_reference"].as<bool>(false);
+        params.init_neuron_state = conf["init_neuron_state"].as<double>(0.707);
+        params.range_ub = conf["range_ub"].as<double>(1.0);
+        params.signal_factor_all = conf["output_signal_factor"].as<double>(1.0);
+        params.abs_output_bound = conf["abs_output_bound"].as<double>(1.0);
+
+        YAML::Node yaml_weights = conf["weights"];
+        for(const YAML::Node &weight: yaml_weights) {
+            params.weights.emplace_back(weight.as<double>());
+        }
+
+        revolve_controller = std::make_unique<revolve::BrokenDifferentialCPG>(
+                params, this->actuators, camera
+        );
     } else if (type == "differential-cpg") {
 
         DifferentialCPG::ControllerParams params;
-        params.reset_neuron_random = false;
-        params.use_frame_of_reference = false;
-        params.init_neuron_state = 0.707;
-        params.range_ub = 1.0;
-        params.signal_factor_all = 1.0;
-        params.signal_factor_mid = 2.5;
-        params.signal_factor_left_right = 2.5;
-        params.abs_output_bound = 1.0;
+        params.reset_neuron_random = conf["reset_neuron_random"].as<bool>(false);
+        params.use_frame_of_reference = conf["use_frame_of_reference"].as<bool>(false);
+        params.init_neuron_state = conf["init_neuron_state"].as<double>(0.707);
+        params.range_ub = conf["range_ub"].as<double>(1.0);
+        params.output_signal_factor = conf["output_signal_factor"].as<double>(1.0);
+        params.abs_output_bound = conf["abs_output_bound"].as<double>(1.0);
 
         YAML::Node yaml_weights = conf["weights"];
         for(const YAML::Node &weight: yaml_weights) {
             params.weights.emplace_back(weight.as<double>());
         }
 
-        revolve_controller.reset(
-                new DifferentialCPG(params,this->actuators)
+        revolve_controller = std::make_unique<revolve::DifferentialCPG>(
+                params, this->actuators, camera
         );
     } else {
         throw std::runtime_error("Controller " + type + " not supported (yet)");
diff --git a/cpprevolve/revolve/raspberry/RaspController.h b/cpprevolve/revolve/raspberry/RaspController.h
index 00ba47996e..c327b8abca 100644
--- a/cpprevolve/revolve/raspberry/RaspController.h
+++ b/cpprevolve/revolve/raspberry/RaspController.h
@@ -2,38 +2,43 @@
 // Created by matteo on 14/06/19.
 //
 
-#ifndef REVOLVE_RASPCONTROLLER_H
-#define REVOLVE_RASPCONTROLLER_H
+#ifndef REVOLVE_RASPBERRY_RASPCONTROLLER_H
+#define REVOLVE_RASPBERRY_RASPCONTROLLER_H
 
 #include <ctime>
 #include <yaml-cpp/node/node.h>
 #include "../brains/controller/Controller.h"
+#include "../brains/controller/sensors/AngleToTargetDetector.h"
 #include "Timer.h"
 
 namespace revolve {
+namespace raspberry {
 
-class RaspController
-{
+class RaspController {
 public:
     explicit RaspController(
-            std::vector<std::unique_ptr< Actuator > > actuators,
-            std::vector<std::unique_ptr< Sensor > > sensors,
+            std::vector<std::shared_ptr<::revolve::Actuator> > actuators,
+            std::vector<std::shared_ptr<::revolve::Sensor> > sensors,
             const YAML::Node &conf);
 
     ~RaspController();
 
     void update();
 
+    void load_camera(const YAML::Node &conf);
     void set_new_controller(const YAML::Node &conf);
 
 private:
-    std::unique_ptr<revolve::Controller> revolve_controller;
+    std::unique_ptr<::revolve::Controller> revolve_controller;
+    std::shared_ptr<::revolve::AngleToTargetDetector> camera;
     Timer timer;
-    std::vector< std::unique_ptr< revolve::Actuator > > actuators;
-    std::vector< std::unique_ptr< revolve::Sensor > > sensors;
+    std::vector<std::shared_ptr<::revolve::Actuator> > actuators;
+    std::vector<std::shared_ptr<::revolve::Sensor> > sensors;
+
+    /// Update rate in seconds
+    Timer::Seconds update_rate;
 };
 
 }
-
-
-#endif //REVOLVE_RASPCONTROLLER_H
+}
+#endif //REVOLVE_RASPBERRY_RASPCONTROLLER_H
diff --git a/cpprevolve/revolve/raspberry/Servo.cpp b/cpprevolve/revolve/raspberry/Servo.cpp
index 2f221c0c23..23e4f9ef2c 100644
--- a/cpprevolve/revolve/raspberry/Servo.cpp
+++ b/cpprevolve/revolve/raspberry/Servo.cpp
@@ -5,7 +5,7 @@
 #include "Servo.h"
 #include "PIGPIOConnection.h"
 
-using namespace revolve;
+using namespace revolve::raspberry;
 
 #define POSITION_OFF 0
 #define POSITION_BEGIN 40
diff --git a/cpprevolve/revolve/raspberry/Servo.h b/cpprevolve/revolve/raspberry/Servo.h
index 6c2ee1ef3c..33fdb6efa7 100644
--- a/cpprevolve/revolve/raspberry/Servo.h
+++ b/cpprevolve/revolve/raspberry/Servo.h
@@ -2,17 +2,17 @@
 // Created by matteo on 14/06/19.
 //
 
-#ifndef REVOLVE_SERVO_H
-#define REVOLVE_SERVO_H
+#ifndef REVOLVE_RASPBERRY_SERVO_H
+#define REVOLVE_RASPBERRY_SERVO_H
 
 #include "PIGPIOConnection.h"
 #include "../brains/controller/actuators/Actuator.h"
 #include <iostream>
 
 namespace revolve {
+namespace raspberry {
 
-class Servo: public revolve::Actuator
-{
+class Servo : public ::revolve::Actuator {
 public:
     explicit Servo(
             double coordinate_x,
@@ -20,12 +20,13 @@ class Servo: public revolve::Actuator
             double coordinate_z,
             PIGPIOConnection *connection,
             unsigned short pin,
-            unsigned int frequency=50,
-            int range=1000,
-            bool inverse=false
+            unsigned int frequency = 50,
+            int range = 1000,
+            bool inverse = false
     );
 
-    ~Servo() {
+    ~Servo()
+    {
         this->off();
     }
 
@@ -36,7 +37,8 @@ class Servo: public revolve::Actuator
      */
     void move_to_position(double position);
 
-    void center() { move_to_position(0); }
+    void center()
+    { move_to_position(0); }
 
     void off();
 
@@ -45,7 +47,8 @@ class Servo: public revolve::Actuator
     std::ostream &print(std::ostream &os) const
     {
         os << "Servo pin:\t" << this->pin << std::endl;
-        os << "\tcoordinates [" << this->coordinate_x() << ',' << this->coordinate_y() << ',' << this->coordinate_z() << ']' << std::endl;
+        os << "\tcoordinates [" << this->coordinate_x() << ',' << this->coordinate_y() << ','
+           << this->coordinate_z() << ']' << std::endl;
         os << "\tfrequency:\t" << this->frequency << std::endl;
         os << "\trange:    \t" << this->range << std::endl;
         os << "\tinverse:  \t" << this->inverse;
@@ -63,8 +66,9 @@ class Servo: public revolve::Actuator
     float maxPWM;
 };
 
+}
 }
 
-std::ostream &operator<<(std::ostream &os, revolve::Servo const &s);
+std::ostream &operator<<(std::ostream &os, revolve::raspberry::Servo const &s);
 
-#endif //REVOLVE_SERVO_H
+#endif //REVOLVE_RASPBERRY_SERVO_H
diff --git a/cpprevolve/revolve/raspberry/Timer.h b/cpprevolve/revolve/raspberry/Timer.h
index 16d0c17cfc..fec49c7fd3 100644
--- a/cpprevolve/revolve/raspberry/Timer.h
+++ b/cpprevolve/revolve/raspberry/Timer.h
@@ -2,38 +2,48 @@
 // Created by matteo on 17/06/19.
 //
 
-#ifndef REVOLVE_TIMER_H
-#define REVOLVE_TIMER_H
+#ifndef REVOLVE_RASPBERRY_TIMER_H
+#define REVOLVE_RASPBERRY_TIMER_H
+
 #include <iostream>
 #include <chrono>
 
 class Timer
 {
 public:
+    typedef std::chrono::high_resolution_clock Clock;
+    typedef std::chrono::duration<double, std::ratio<1> > Seconds;
+
     Timer()
-        : beg_(clock_::now())
+        : beg_(Clock::now())
         , last_step_(beg_)
     {}
-    void reset() { beg_ = clock_::now(); }
-    double step() {
-        std::chrono::time_point<clock_> prev = last_step_;
-        last_step_ = clock_::now();
-        return time_difference(prev, last_step_);
+    void reset() { beg_ = Clock::now(); }
+
+    Seconds step() {
+        std::chrono::time_point<Clock> prev = last_step_;
+        last_step_ = Clock::now();
+        return last_step_ - prev;
     }
-    double elapsed() const { return time_difference(beg_, last_step_); }
-    double elapsed_now() const { return time_difference(beg_, clock_::now()); }
+
+    Seconds step_elapsed() const { return Clock::now() - last_step_; }
+    Seconds elapsed() const { return last_step_ - beg_; }
+    Seconds elapsed_now() const { return Clock::now() - beg_; }
+
+    double step_double() { return into_double(step()); }
+    double step_elapsed_double() const { return into_double(step_elapsed()); }
+    double elapsed_double() const { return into_double(elapsed()); }
+    double elapsed_now_double() const { return into_double(elapsed_now()); }
 
 private:
-    typedef std::chrono::high_resolution_clock clock_;
-    typedef std::chrono::duration<double, std::ratio<1> > second_;
-    std::chrono::time_point<clock_> beg_;
-    std::chrono::time_point<clock_> last_step_;
+    std::chrono::time_point<Clock> beg_;
+    std::chrono::time_point<Clock> last_step_;
 
-    static double time_difference(const std::chrono::time_point<clock_> start, const std::chrono::time_point<clock_> end)
+    template <typename T>
+    static double into_double(const T &duration)
     {
-        return std::chrono::duration_cast<second_>
-                (end - start).count();
+        return std::chrono::duration_cast<Seconds>(duration).count();
     }
 };
 
-#endif //REVOLVE_TIMER_H
+#endif //REVOLVE_RASPBERRY_TIMER_H
diff --git a/cpprevolve/revolve/raspberry/rasp_main.cpp b/cpprevolve/revolve/raspberry/rasp_main.cpp
index 5f7681b4a3..c26ccaf6b2 100644
--- a/cpprevolve/revolve/raspberry/rasp_main.cpp
+++ b/cpprevolve/revolve/raspberry/rasp_main.cpp
@@ -8,7 +8,7 @@
 #include <iostream>
 #include <yaml-cpp/yaml.h>
 
-typedef std::unique_ptr<revolve::Servo> Servo_p;
+typedef std::shared_ptr<revolve::raspberry::Servo> Servo_p;
 
 std::vector<Servo_p> read_conf(PIGPIOConnection &pigpio, const YAML::Node &yaml_servos);
 void reset(std::vector<Servo_p> &servos);
@@ -93,7 +93,7 @@ std::vector<Servo_p> read_conf(PIGPIOConnection &pigpio, const YAML::Node &yaml_
         auto frequency = yaml_servo["frequency"].as<unsigned>(50);
         auto range     = yaml_servo["range"]    .as<int>(1000);
         auto inverse   = yaml_servo["inverse"]  .as<bool>(false);
-        servos.emplace_back(new revolve::Servo(
+        servos.emplace_back(new revolve::raspberry::Servo(
                 x,
                 y,
                 z,
@@ -112,14 +112,14 @@ std::vector<Servo_p> read_conf(PIGPIOConnection &pigpio, const YAML::Node &yaml_
 void control(std::vector<Servo_p> &servos, const YAML::Node &controller_conf)
 {
     std::cout << "Staring controller" << std::endl;
-    std::vector<std::unique_ptr<revolve::Sensor>> sensors;
-    std::vector<std::unique_ptr<revolve::Actuator>> actuators;
+    std::vector<std::shared_ptr<revolve::Sensor>> sensors;
+    std::vector<std::shared_ptr<revolve::Actuator>> actuators;
     actuators.reserve(servos.size());
     for (Servo_p &servo: servos) {
         actuators.emplace_back(std::move(servo));
     }
 
-    revolve::RaspController controller(
+    revolve::raspberry::RaspController controller(
         std::move(actuators),
         std::move(sensors),
         controller_conf
@@ -133,14 +133,14 @@ void control(std::vector<Servo_p> &servos, const YAML::Node &controller_conf)
 void learner(std::vector<Servo_p> &servos, const YAML::Node &controllers_conf)
 {
     std::cout << "Staring controller" << std::endl;
-    std::vector<std::unique_ptr<revolve::Sensor>> sensors;
-    std::vector<std::unique_ptr<revolve::Actuator>> actuators;
+    std::vector<std::shared_ptr<revolve::Sensor>> sensors;
+    std::vector<std::shared_ptr<revolve::Actuator>> actuators;
     actuators.reserve(servos.size());
     for (Servo_p &servo: servos) {
         actuators.emplace_back(std::move(servo));
     }
 
-    revolve::RaspController controller(
+    revolve::raspberry::RaspController controller(
             std::move(actuators),
             std::move(sensors),
             controllers_conf[0]
@@ -152,7 +152,7 @@ void learner(std::vector<Servo_p> &servos, const YAML::Node &controllers_conf)
         std::cout << "Loading controller[" << ++counter << "] fitness: " << controller_conf["fitness"].as<double>(-1) << std::endl;
         controller.set_new_controller(controller_conf);
         timer.reset();
-        while (timer.elapsed_now() < 30.0) {
+        while (timer.elapsed_now() < Timer::Seconds(30.0)) {
             controller.update();
         }
     }
diff --git a/cpprevolve/revolve/raspberry/robot_conf.yaml b/cpprevolve/revolve/raspberry/robot_conf.yaml
index e139ac18e2..59b25a5402 100644
--- a/cpprevolve/revolve/raspberry/robot_conf.yaml
+++ b/cpprevolve/revolve/raspberry/robot_conf.yaml
@@ -1,7 +1,7 @@
 robot_name: "spider9"
 robot_id: 1,
 robot_address:
-  ip: "192.168.1.25"
+  #ip: "192.168.1.25"
   #port: 8888
 
 #"servo_pins": [17,18,27,22,23,10,9,25],
@@ -34,31 +34,39 @@ servos:
 
 rgb_pins: [15,14,4]
 
-#controller:
+
+
+controller:
   # spider weights
-  #type: "differential-cpg"
-  #weights: [0.545275, 0.48118, 0.677335, 0.834078, 0.331732, 0.479091, 0.87384, 0.527239, 0.0148421, 0.131508, 0.711216, 0.672872, 0.648163, 0.204883, 0.788699, 0.38614, 0.483561, 0.0777244]
+  type: "differential-cpg"
+  weights: [0.545275, 0.48118, 0.677335, 0.834078, 0.331732, 0.479091, 0.87384, 0.527239, 0.0148421, 0.131508, 0.711216, 0.672872, 0.648163, 0.204883, 0.788699, 0.38614, 0.483561, 0.0777244]
+  angle_to_target_detector:
+    type: "raspberry-camera"
+#    type: "usb"
+#    index: 0
+    shrink_factor: 4
+    show_camera: False
 
-controllers:
-  # baby+1 learning weights, crescendo
-  - type: "differential-cpg"
-    weights: [0.395516, 0.83811,  0.463123, 0.702373, 0.936804, 0.166412, 0.631632,  0.287821,  0.799552,  0.218267, 0.0623519, 0.799128]
-    fitness: 0.000503405
-  - type: "differential-cpg"
-    weights: [0.871967, 0.441235, 0.684449, 0.58511,  0.899357, 0.497721, 0.342563,  0.164466,  0.676871,  0.475722, 0.405172,  0.370701]
-    fitness: 0.202879
-  - type: "differential-cpg"
-    weights: [0.223283, 0.912134, 0.374909, 0.931379, 0.733706, 0.283053, 0.0923915, 0.0706944, 0.0915673, 0.53245,  0.962511,  0.512361]
-    fitness: 0.403299
-  - type: "differential-cpg"
-    weights: [0.302763, 0.913937, 0.376248, 0.911346, 0.712692, 0.298535, 0.0272732, 0.0706944, 0.095008,  0.46318,  0.990895,  0.564726]
-    fitness: 0.600739
-  - type: "differential-cpg"
-    weights: [0.319842, 0.848368, 0.415024, 0.946114, 0.665586, 0.26537,  0.0609814, 0.0757356, 0.100649,  0.461241, 0.972517,  0.485218]
-    fitness: 0.801518
-  - type: "differential-cpg"
-    weights: [0.698179, 0.828084, 0.381213, 0.974549, 0.663302, 0.22419,  0.163161,  0.0362292, 0.0558966, 0.357249, 0.960544,  0.512658]
-    fitness: 1.00555
-  - type: "differential-cpg"
-    weights: [0.65835,  0.723842, 0.431859, 0.919208, 0.721147, 0.160043, 0.138266,  0.0751595, 0.0335994, 0.395186, 0.88612,   0.601392]
-    fitness: 1.27441
+#controllers:
+#  # baby+1 learning weights, crescendo
+#  - type: "differential-cpg"
+#    weights: [0.395516, 0.83811,  0.463123, 0.702373, 0.936804, 0.166412, 0.631632,  0.287821,  0.799552,  0.218267, 0.0623519, 0.799128]
+#    fitness: 0.000503405
+#  - type: "differential-cpg"
+#    weights: [0.871967, 0.441235, 0.684449, 0.58511,  0.899357, 0.497721, 0.342563,  0.164466,  0.676871,  0.475722, 0.405172,  0.370701]
+#    fitness: 0.202879
+#  - type: "differential-cpg"
+#    weights: [0.223283, 0.912134, 0.374909, 0.931379, 0.733706, 0.283053, 0.0923915, 0.0706944, 0.0915673, 0.53245,  0.962511,  0.512361]
+#    fitness: 0.403299
+#  - type: "differential-cpg"
+#    weights: [0.302763, 0.913937, 0.376248, 0.911346, 0.712692, 0.298535, 0.0272732, 0.0706944, 0.095008,  0.46318,  0.990895,  0.564726]
+#    fitness: 0.600739
+#  - type: "differential-cpg"
+#    weights: [0.319842, 0.848368, 0.415024, 0.946114, 0.665586, 0.26537,  0.0609814, 0.0757356, 0.100649,  0.461241, 0.972517,  0.485218]
+#    fitness: 0.801518
+#  - type: "differential-cpg"
+#    weights: [0.698179, 0.828084, 0.381213, 0.974549, 0.663302, 0.22419,  0.163161,  0.0362292, 0.0558966, 0.357249, 0.960544,  0.512658]
+#    fitness: 1.00555
+#  - type: "differential-cpg"
+#    weights: [0.65835,  0.723842, 0.431859, 0.919208, 0.721147, 0.160043, 0.138266,  0.0751595, 0.0335994, 0.395186, 0.88612,   0.601392]
+#    fitness: 1.27441