File content as of revision 2:b51fb6efcd55:

/**
 *******************************************************************************
 * @file    main.cpp
 * @author  Davide Aliprandi, STMicroelectronics
 * @version V1.0.0
 * @date    October 31st, 2017
 * @brief   mbed test application for the STMicroelectronics X-NUCLEO-IHM01A1
 *          Motor Control Expansion Board and the X-NUCLEO-IDB05A1 Bluetooth
 *          Low energy Expansion Board.
 *******************************************************************************
 * @attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2018 STMicroelectronics</center></h2>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   1. Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *   2. Redistributions in binary form must reproduce the above copyright
 *      notice, this list of conditions and the following disclaimer in the
 *      documentation and/or other materials provided with the distribution.
 *   3. Neither the name of STMicroelectronics nor the names of its
 *      contributors may be used to endorse or promote products derived from
 *      this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *******************************************************************************
 */


/* Includes ------------------------------------------------------------------*/

/* mbed specific header files. */
#include <events/mbed_events.h>
#include <mbed.h>
#include "ble/BLE.h"
#include "CustomService.h"

/* Helper header files. */
#include "DevSPI.h"

/* Component specific header files. */
#include "L6474.h"


/* Definitions ---------------------------------------------------------------*/

#define BLE_ADVERTISING_INTERVAL_ms 1000
#define DELAY_1 100


/* Variables -----------------------------------------------------------------*/

/* Blinky LED to indicate system aliveness. */
//DigitalOut alivenessLED(LED1, 0); //Conflicts with SPI CLK on D13

/* Bluetooth. */
const static char    DEVICE_NAME[] = "MOTOR_DEVICE";
const static uint8_t MANUFACTURER_SPECIFIC_DATA[]= {0x01,0x80,0x00,0x00,0x20,0x00};
static EventQueue event_queue(/* event count */ 10 * EVENTS_EVENT_SIZE);
CustomService *custom_service;

/* Initialization parameters. */
L6474_init_t init = {
    160,                              /* Acceleration rate in pps^2. Range: (0..+inf). */
    160,                              /* Deceleration rate in pps^2. Range: (0..+inf). */
    1600,                             /* Maximum speed in pps. Range: (30..10000]. */
    800,                              /* Minimum speed in pps. Range: [30..10000). */
    250,                              /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
    L6474_OCD_TH_750mA,               /* Overcurrent threshold (OCD_TH register). */
    L6474_CONFIG_OC_SD_ENABLE,        /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
    L6474_CONFIG_EN_TQREG_TVAL_USED,  /* Torque regulation method (EN_TQREG field of CONFIG register). */
    L6474_STEP_SEL_1_8,               /* Step selection (STEP_SEL field of STEP_MODE register). */
    L6474_SYNC_SEL_1_2,               /* Sync selection (SYNC_SEL field of STEP_MODE register). */
    L6474_FAST_STEP_12us,             /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
    L6474_TOFF_FAST_8us,              /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
    3,                                /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
    21,                               /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
    L6474_CONFIG_TOFF_044us,          /* Target Swicthing Period (field TOFF of CONFIG register). */
    L6474_CONFIG_SR_320V_us,          /* Slew rate (POW_SR field of CONFIG register). */
    L6474_CONFIG_INT_16MHZ,           /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
    L6474_ALARM_EN_OVERCURRENT |
    L6474_ALARM_EN_THERMAL_SHUTDOWN |
    L6474_ALARM_EN_THERMAL_WARNING |
    L6474_ALARM_EN_UNDERVOLTAGE |
    L6474_ALARM_EN_SW_TURN_ON |
    L6474_ALARM_EN_WRONG_NPERF_CMD    /* Alarm (ALARM_EN register). */
};

/* Motor Control Component. */
L6474 *motor;

/* Button event. */
InterruptIn event(USER_BUTTON);


/* Bluetooth related functions -----------------------------------------------*/

void on_disconnection_callback(const Gap::DisconnectionCallbackParams_t *params)
{
    (void) params;
    BLE::Instance().gap().startAdvertising();
}

void aliveness_callback(void)
{
    //alivenessLED = !alivenessLED; /* Do blinky to indicate system aliveness. */
}

/**
 * This callback allows the custom service to read updates from the
 * characteristic.
 *
 * @param[in] params
 *     Information about the characterisitc being updated.
 */
void on_data_read_callback(const GattReadCallbackParams *params) {
    static uint16_t time_stamp = 0;

    if (params->handle == custom_service->getValueHandle()) {
        /* Getting motor's state. */
        uint8_t state = (uint8_t) (motor->get_device_state() == INACTIVE ? CustomService::MOTOR_INACTIVE : CustomService::MOTOR_RUNNING); 
        
        /* Sending data via bluetooth. */
        if (BLE::Instance().getGapState().connected) {
            event_queue.call(Callback<void(uint16_t, uint8_t)>(custom_service, &CustomService::send_state), time_stamp++, state);
        }
    }
}

/**
 * This callback allows the custom service to write updates to the
 * characteristic.
 *
 * @param[in] params
 *     Information about the characterisitc being updated.
 */
void on_data_written_callback(const GattWriteCallbackParams *params) {
    uint32_t steps = 3000;

    if (params->handle == custom_service->getValueHandle()) {
        //printf("--> COMMAND: %d\r\n", ((uint8_t *) (params->data))[0]);
        switch ((CustomService::motor_command_t) ((uint8_t *) (params->data))[0])
        {
            //Stops running with HiZ
            case CustomService::MOTOR_STOP_RUNNING_WITHOUT_TORQUE:
                //printf("--> %d %d\r\n", params->len, ((uint8_t *) (params->data))[0]);
                motor->hard_hiz();
                motor->wait_while_active();
                break;
            //Stops running with torque applied
            case CustomService::MOTOR_STOP_RUNNING_WITH_TORQUE:
                //printf("--> %d %d\r\n", params->len, ((uint8_t *) (params->data))[0]);
                motor->hard_stop();
                motor->enable();
                motor->wait_while_active();
                break;
            //Runs forward indefinitely
            case CustomService::MOTOR_RUN_FORWARD:
                //printf("--> %d %d\r\n", params->len, ((uint8_t *) (params->data))[0]);
                motor->run(StepperMotor::FWD);
                break;
            //Runs backward indefinitely
            case CustomService::MOTOR_RUN_BACKWARD:
                //printf("--> %d %d\r\n", params->len, ((uint8_t *) (params->data))[0]);
                motor->run(StepperMotor::BWD);
                break;
            //Moves steps forward
            case CustomService::MOTOR_MOVE_STEPS_FORWARD:
                steps = ((((uint8_t *) (params->data))[1]) |
                         (((uint8_t *) (params->data))[2] << 8 ) |
                         (((uint8_t *) (params->data))[3] << 16) |
                         (((uint8_t *) (params->data))[4] << 24));
                //printf("--> %d %d %d", params->len, ((uint8_t *) (params->data))[0], steps);
                motor->move(StepperMotor::FWD, steps);
                motor->wait_while_active();
                break;
            //Moves steps backward
            case CustomService::MOTOR_MOVE_STEPS_BACKWARD:
                steps = ((((uint8_t *) (params->data))[1]) |
                         (((uint8_t *) (params->data))[2] << 8 ) |
                         (((uint8_t *) (params->data))[3] << 16) |
                         (((uint8_t *) (params->data))[4] << 24));
                //printf("--> %d %d %d", params->len, ((uint8_t *) (params->data))[0], steps);
                motor->move(StepperMotor::BWD, steps);
                motor->wait_while_active();
                break;
            //Other
            default:
                break;
        }
    }
}

/**
 * This function is called when the ble initialization process has failled.
 */
void on_ble_init_error_callback(BLE &ble, ble_error_t error)
{
    /* Initialization error handling should go here */
}

/**
 * Callback triggered when the ble initialization process has finished.
 */
void ble_init_complete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE& ble   = params->ble;
    ble_error_t error = params->error;

    if (error != BLE_ERROR_NONE) {
        /* In case of error, forward the error handling to on_ble_init_error_callback */
        on_ble_init_error_callback(ble, error);
        return;
    }

    /* Ensure that it is the default instance of BLE */
    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }

    ble.gap().onDisconnection(on_disconnection_callback);
    ble.gattServer().onDataWritten(on_data_written_callback);
    ble.gattServer().onDataRead(on_data_read_callback);

    custom_service = new CustomService(ble);

    /* Setup advertising data. */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, (uint8_t *) CUSTOM_SERVICE_UUID, sizeof(CUSTOM_SERVICE_UUID));
    ble.gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, MANUFACTURER_SPECIFIC_DATA, sizeof(MANUFACTURER_SPECIFIC_DATA));
    ble.gap().accumulateScanResponse(GapAdvertisingData::COMPLETE_LOCAL_NAME, (const uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME) - 1);
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL_ms);
    ble.gap().startAdvertising();
}

void schedule_ble_events_processing(BLE::OnEventsToProcessCallbackContext* context) {
    BLE &ble = BLE::Instance();
    event_queue.call(Callback<void()>(&ble, &BLE::processEvents));
}


/* Custom service related functions ------------------------------------------*/

/**
 * @brief  This is an example of user handler for the flag interrupt.
 * @param  None
 * @retval None
 * @note   If needed, implement it, and then attach and enable it:
 *           + motor->attach_flag_irq(&flag_irq_handler);
 *           + motor->enable_flag_irq();
 *         To disable it:
 *           + motor->disble_flag_irq();
 */
void flag_irq_handler(void)
{
    /* Set ISR flag. */
    motor->isr_flag = TRUE;

    /* Get the value of the status register. */
    unsigned int status = motor->get_status();

    /* Check NOTPERF_CMD flag: if set, the command received by SPI can't be performed. */
    /* This often occures when a command is sent to the L6474 while it is not in HiZ state. */
    if ((status & L6474_STATUS_NOTPERF_CMD) == L6474_STATUS_NOTPERF_CMD) {
        printf("    WARNING: \"FLAG\" interrupt triggered. Non-performable command detected when updating L6474's registers while not in HiZ state.\r\n");
    }
    
    /* Reset ISR flag. */
    motor->isr_flag = FALSE;
}


/* Motor related functions ------------------------------------------*/

void motor_run_callback(void)
{
    motor->run(StepperMotor::FWD);
}

void motor_stop_callback(void)
{
    motor->hard_hiz();
    motor->wait_while_active();
}


/* Main function -------------------------------------------------------------*/

int main()
{
    /*----- Initialization. -----*/

    /* Printing to the console. */
    printf("Motor Node Application Example\r\n\n");

    /* Aliveness callback. */
    //event_queue.call_every(500, aliveness_callback);

    /* Bluetooth. */
    BLE &ble = BLE::Instance();
    ble.onEventsToProcess(schedule_ble_events_processing);
    ble.init(ble_init_complete);
    
    /* Initializing SPI bus. */
    DevSPI dev_spi(D11, D12, D13);

    /* Initializing Motor Control Component. */
    /* D7 conflicts with BNRG_RST (when motor changes direction 0-1 BLE resets). */
    /* Used Morpho PB_2 and made HW wiring. */
    motor = new L6474(D2, D8, PB_2, D9, D10, dev_spi);
    if (motor->init(&init) != COMPONENT_OK) {
        exit(EXIT_FAILURE);
    }

    /* Attaching and enabling interrupt handlers. */
    motor->attach_flag_irq(&flag_irq_handler);
    motor->enable_flag_irq();
    event.fall(motor_run_callback);
    event.rise(motor_stop_callback);

    /* Stopping with HiZ. */
    motor->hard_hiz();
    motor->wait_while_active();

    /* Start. */
    event_queue.dispatch_forever();

    return 0;
}