robot_motor_control.c

Go to the documentation of this file.

/*
 * Copyright (c) 2017 The University of Manchester
 *
 * 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
 *
 *     https://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.
 */
 
 
#include <common/neuron-typedefs.h>
#include <common/in_spikes.h>
 
#include <data_specification.h>
#include <debug.h>
#include <simulation.h>
#include <stdbool.h>
 
// ----------------------------------------------------------------------
 
typedef struct {
    uint32_t key;
    int speed;
    uint32_t sample_time;
    uint32_t update_time;
    uint32_t delay_time;
    int delta_threshold;
    uint32_t continue_if_not_different;
} motor_control_config_t;
 
struct robot_motor_control_provenance {
    uint32_t n_input_buffer_overflows;
};
 
#define N_COUNTERS         6
 
typedef enum {
    MOTION_FORWARD = 0x01,    
    MOTION_BACK = 0x02,       
    MOTION_RIGHT = 0x03,      
    MOTION_LEFT = 0x04,       
    MOTION_CLOCKWISE = 0x05,  
    MOTION_C_CLOCKWISE = 0x06 
} direction_t;
 
#define NEURON_ID_MASK     0x7FF
 
// Globals
static uint32_t time;
static int *counters;
static int *last_speed;
static uint32_t key;
static int speed;
static uint32_t sample_time;
static uint32_t update_time;
static uint32_t delay_time;
static int delta_threshold;
static bool continue_if_not_different;
static uint32_t simulation_ticks;
static uint32_t infinite_run;
 
enum robot_motor_control_regions_e {
    SYSTEM_REGION, 
    PARAMS_REGION,  
    PROVENANCE_DATA_REGION 
};
 
enum robot_motor_control_callback_priorities {
    MC = -1,   
    SDP = 0,   
    DMA = 1,   
    TIMER = 2, 
};
 
// ----------------------------------------------------------------------
 
static inline void send_to_motor(uint32_t direction, uint32_t the_speed) {
    uint32_t direction_key = direction | key;
    while (!spin1_send_mc_packet(direction_key, the_speed, WITH_PAYLOAD)) {
        spin1_delay_us(1);
    }
    if (delay_time > 0) {
        spin1_delay_us(delay_time);
    }
}
 
static inline void do_motion(
        direction_t direction_index, direction_t opposite_index,
        const char *direction, const char *opposite) {
    int direction_count = counters[direction_index - 1];
    int opposite_count = counters[opposite_index - 1];
    int delta = direction_count - opposite_count;
    log_debug("%s = %d, %s = %d, delta = %d, threshold = %u",
            direction, direction_count, opposite, opposite_count, delta,
            delta_threshold);
 
    if (delta >= delta_threshold) {
        log_debug("Moving %s", direction);
        last_speed[direction_index - 1] = speed;
        last_speed[opposite_index - 1] = 0;
        send_to_motor(direction_index, speed);
    } else if (delta <= -delta_threshold) {
        log_debug("Moving %s", direction);
        last_speed[direction_index - 1] = 0;
        last_speed[opposite_index - 1] = speed;
        send_to_motor(opposite_index, speed);
    } else if (!continue_if_not_different) {
        log_debug("Motion is indeterminate in %s-%s direction",
                direction, opposite);
        last_speed[direction_index - 1] = 0;
        last_speed[opposite_index - 1] = 0;
        send_to_motor(direction_index, 0);
    }
}
 
static inline void do_update(
        direction_t direction_index, direction_t opposite_index,
        const char *direction, const char *opposite) {
    int direction_speed = last_speed[direction_index - 1];
    int opposite_speed = last_speed[opposite_index - 1];
    int delta = direction_speed - opposite_speed;
    if (delta > 0) {
        log_debug("Resending %s = %d", direction, direction_speed);
        send_to_motor(direction_index, direction_speed);
    } else if (delta < 0) {
        log_debug("Resending %s = %d", opposite, opposite_speed);
        send_to_motor(opposite_index, opposite_speed);
    } else {
        log_debug("Resending No Motion in the %s-%s direction", direction,
                opposite);
        send_to_motor(direction_index, 0);
    }
}
 
// Callbacks
static void timer_callback(UNUSED uint unused0, UNUSED uint unused1) {
    time++;
 
    log_debug("Timer tick %d", time);
 
    if (simulation_is_finished()) {
        simulation_handle_pause_resume(NULL);
        log_info("Simulation complete.\n");
        simulation_ready_to_read();
    }
 
    // Process the incoming spikes
    spike_t s;
    uint32_t nid;
    while (in_spikes_get_next_spike(&s)) {
        nid = (s & NEURON_ID_MASK);
 
        if (nid < N_COUNTERS) {
            counters[nid]++;
        } else {
            log_debug("Received spike from unknown neuron %d", nid);
        }
    }
 
    // Work out if there is any motion
    if (time % sample_time == 0) {
        // Do motion in pairs
        do_motion(MOTION_FORWARD, MOTION_BACK, "Forwards", "Backwards");
        do_motion(MOTION_LEFT, MOTION_RIGHT, "Left", "Right");
        do_motion(MOTION_CLOCKWISE, MOTION_C_CLOCKWISE, "Clockwise",
                "Anti-clockwise");
 
        // Reset the counters
        for (uint32_t i = 0; i < N_COUNTERS; i++) {
            counters[i] = 0;
        }
    } else if ((time % update_time) == 0) {
        // Do updates in pairs
        do_update(MOTION_FORWARD, MOTION_BACK, "Forwards", "Backwards");
        do_update(MOTION_LEFT, MOTION_RIGHT, "Left", "Right");
        do_update(MOTION_CLOCKWISE, MOTION_C_CLOCKWISE, "Clockwise",
                "Anti-clockwise");
    }
}
 
static void read_parameters(motor_control_config_t *config_region) {
    log_info("Reading parameters from 0x%.8x", config_region);
    key = config_region->key;
    speed = config_region->speed;
    sample_time = config_region->sample_time;
    update_time = config_region->update_time;
    delay_time = config_region->delay_time;
    delta_threshold = config_region->delta_threshold;
    continue_if_not_different = config_region->continue_if_not_different;
 
    // Allocate the space for the schedule
    counters = spin1_malloc(N_COUNTERS * sizeof(int));
    last_speed = spin1_malloc(N_COUNTERS * sizeof(int));
 
    for (uint32_t i = 0; i < N_COUNTERS; i++) {
        counters[i] = 0;
        last_speed[i] = 0;
    }
 
    log_info("Key = %d, speed = %d, sample_time = %d, update_time = %d,"
            " delay_time = %d, delta_threshold = %d, continue_if_not_different = %d",
            key, speed, sample_time, update_time, delay_time, delta_threshold,
            continue_if_not_different);
}
 
static void incoming_spike_callback(uint key, UNUSED uint payload) {
    log_debug("Received spike %x at time %d\n", key, time);
 
    // If there was space to add spike to incoming spike queue
    in_spikes_add_spike(key);
}
 
static void incoming_spike_callback_payload(uint key, uint payload) {
    use(payload);
 
    log_debug("Received spike %x at time %d\n", key, time);
 
    // If there was space to add spike to incoming spike queue
    for (uint count = 0; count < payload; count ++){
        in_spikes_add_spike(key);
    }
}
 
static void c_main_store_provenance_data(address_t provenance_region) {
    log_debug("writing other provenance data");
    struct robot_motor_control_provenance *prov = (void *) provenance_region;
 
    // store the data into the provenance data region
    prov->n_input_buffer_overflows = in_spikes_get_n_buffer_overflows();
 
    log_debug("finished other provenance data");
}
 
static bool initialize(uint32_t *timer_period) {
    log_info("initialise: started");
 
    // Get the address this core's DTCM data starts at from SRAM
    data_specification_metadata_t *ds_regions =
            data_specification_get_data_address();
 
    // Read the header
    if (!data_specification_read_header(ds_regions)) {
        return false;
    }
 
    // Get the timing details and set up the simulation interface
    if (!simulation_initialise(
            data_specification_get_region(SYSTEM_REGION, ds_regions),
            APPLICATION_NAME_HASH, timer_period, &simulation_ticks,
            &infinite_run, &time, SDP, DMA)) {
        return false;
    }
 
    simulation_set_provenance_function(
        c_main_store_provenance_data,
        data_specification_get_region(PROVENANCE_DATA_REGION, ds_regions));
 
    // Get the parameters
    read_parameters(data_specification_get_region(PARAMS_REGION, ds_regions));
 
    log_info("initialise: completed successfully");
 
    return true;
}
 
void c_main(void) {
    // Initialise
    uint32_t timer_period = 0;
    if (!initialize(&timer_period)) {
        log_error("Error in initialisation - exiting!");
        rt_error(RTE_SWERR);
    }
 
    // Initialise the incoming spike buffer
    if (!in_spikes_initialize_spike_buffer(8192)) {
        return;
    }
 
    // Set timer_callback
    spin1_set_timer_tick(timer_period);
 
    // Register callbacks
    spin1_callback_on(MC_PACKET_RECEIVED, incoming_spike_callback, MC);
    spin1_callback_on(
        MCPL_PACKET_RECEIVED, incoming_spike_callback_payload, MC);
    spin1_callback_on(TIMER_TICK, timer_callback, TIMER);
 
    // Start the time at "-1" so that the first tick will be 0
    time = UINT32_MAX;
    simulation_run();
}