LP Long Distance IR Vision Robot

Dependencies:   max77650_charger_sample BufferedSerial SX1276GenericLib Adafruit-MotorShield NEO-6m-GPS MAX17055_EZconfig Adafruit_GFX USBDeviceHT Adafruit-PWM-Servo-Driver

main.cpp

Committer:
dev_alexander
Date:
2018-07-31
Revision:
37:f0fcc0341654
Parent:
36:09e8b51fc91d
Child:
38:c10d2a04a0c2

File content as of revision 37:f0fcc0341654:

/*
 * Copyright (c) 2018 HELIOS Software GmbH
 * 30826 Garbsen (Hannover) Germany
 * Licensed under the Apache License, Version 2.0);
 */
  
 /*
 * For Wiring Instructions, please visit the link below:
 * https://www.hackster.io/DevinAlex64/getting-started-with-the-max32620fthr-and-lora-f9d8dd\
 */

/***************************************************************************
 * Misc Instantiation
 **************************************************************************/
#include "main.h"
#include "global_buffers.h"
//#define DEBUG_GRID_EYE
#define RASPBERRY_PI
DigitalOut myled(LED);
Serial pc(USBTX, USBRX);


/***************************************************************************
 * MAX32630FTHR Instantiation
 **************************************************************************/
#if defined(TARGET_MAX32630FTHR) 
    /* If the board that is compiled is the master device (BLE enabled MAX32630FTHR),
     * then it needs this library and needs to be configed in order for the device to 
     * work with a 3.3volt output instead of a 1.8 volt output. 
     * 
     * This is only needed for the MAX32630FTHR. The MAX325620FTHR is exampt from this 
     * additional setup in the main program.
     */ 
    #include "max32630fthr.h"
    MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
#endif

/***************************************************************************
 * MAX32620FTHR Instantiation
 **************************************************************************/
#if defined(TARGET_MAX32620FTHR)
    // Definitions for running board off of battery
    #define POWER_HOLD_ON 1
    #define POWER_HOLD_OFF 0
    
// MAX77650
    DigitalOut pw_Hold(P2_2, POWER_HOLD_ON);
#endif


/***************************************************************************
 * Grid Eye Sensor Instantiation
 **************************************************************************/
#include "GridEye.h"

#if   defined(TARGET_MAX32630FTHR) // Master Device: MAX32630FTHR BLE-to-LoRa Gateway
    /* No grid eye object needed. The function the master uses to convert the raw 
     * data that is sent over from the slave deevice that contains the data from the
     * actual grid eye sensor is actually a function that is not part of the grid eye
     * class. this is due to the fact that the grid eye class requires n i2c bus to 
     * be assigned to a phyiscal sensor. In this case, the library for the Grid Eye 
     * sensor has a support function that is used to convert data that is aquired from 
     * the grid eye sensor. So it is not supposed to be a class specific function.
     */
#elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
    // I2C i2cGridEye(SDA,SCL);
    //I2C i2cGridEye(P3_4,P3_5);
    I2C i2cGridEye(P1_6,P1_7);
    GridEye gridEye(i2cGridEye);
#endif

/***************************************************************************
 * GPS Instantiation
 **************************************************************************/
#if   defined(TARGET_MAX32630FTHR) // Master Device: MAX32630FTHR BLE-to-LoRa Gateway
    // No GPS module on the master device
#elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
    #include "GPS.h"
    GPS gps(P0_1, P0_0, 115200); // (Tx, Rx, Baud rate)
#endif

/***************************************************************************
 * MAX77650 Instatiation
 **************************************************************************/
 
/***************************************************************************
 * MAX17055 Instatiation
 **************************************************************************/
 
/***************************************************************************
 * OLED Instatiation
 **************************************************************************/

/***************************************************************************
 * BLE Instatiation and Related Functions
 **************************************************************************/
#if   defined(TARGET_MAX32630FTHR) // Master Device: MAX32630FTHR BLE-to-LoRa Gateway
    #include "ble/BLE.h"
    //#include "ble/Gap.h"
    #include "UARTService_custom.h"
    #include <events/mbed_events.h>
    
    DigitalOut led2(LED2);
    UARTService *uart;
    
    #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
                                   * it will have an impact on code-size and power consumption. */
 
    #if NEED_CONSOLE_OUTPUT
    #define DEBUG(STR) { if (uart) uart->write(STR, strlen(STR)); }
    #define READ(STR) { if (uart) uart->read(STR, strlen(STR)); }
    
    #else
    #define DEBUG(...) /* nothing */
    #endif /* #if NEED_CONSOLE_OUTPUT */
    
    //Triggered when the robot is disconnected from the mobile app.
    void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
    {
        BLE &ble = BLE::Instance();
        DEBUG("Disconnected!\n\r");
        DEBUG("Restarting the advertising process\n\r");
        ble.gap().startAdvertising();
    }
    
    /* This pointer is needed to reference data in the main program when filling 
     * with data from the BLE 
     */
    uint8_t * onDataWritten_curr_ble_to_slave;
    
    // Triggered when there is data written
    void onDataWritten(const GattWriteCallbackParams *params)
    {    
        //LED1 will be off when button is pressed, and on when button is released.
        led2 = !led2;
        uint8_t button_state;
        uint8_t direction;

        /* If there is a valid data sent by the mobile app */
        if ((uart != NULL) && (params->handle == uart ->getTXCharacteristicHandle())) {
            const uint8_t *packet = params->data;
            button_state = packet[3];
            direction = packet[2];
    
            // make parameter to send over Lora        
            onDataWritten_curr_ble_to_slave[0] = direction;
            onDataWritten_curr_ble_to_slave[1] = button_state;
            
            //dprintf("direction: %d\n", direction);
            //dprintf("button_state: %d\n", button_state);      
        }
    }
#elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
    // No BLE chip on the MAX32620FTHR so this is not needed
#endif
 
/***************************************************************************
 * Motor Controller Instatiation and Related Functions
 **************************************************************************/
#if   defined(TARGET_MAX32630FTHR) // Master Device: MAX32630FTHR BLE-to-LoRa Gateway
    // There is no motor on this device.
#elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
    /* This is a port of a library that was developed by Adafruit for the Arduino. 
     * This port was done by mbed user syundo0730. */
    #include "Adafruit_MotorShield.h"
    #include "Adafruit_PWMServoDriver.h"
    
    // These are the defined speeds that are selected out of the number range (0, 255).
    #define MOTOR_OFF 0
    #define MOTOR_FORWARD 150
    #define MOTOR_BACKWARD 150
    #define MOTOR_TURN 100
    
    // Create the motor shield object with the default I2C address
    Adafruit_MotorShield AFMS = Adafruit_MotorShield(P3_4, P3_5, 0x60 << 1); // You have to shift it over by one due to how mbed does its I2C Addressing. 
    // Or, create it with a different I2C address (say for stacking)
    // Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61); 
    // Select which 'port' M1, M2, M3 or M4. In this case, M1
    Adafruit_DCMotor *rightMotor = AFMS.getMotor(1);
    // You can also make another motor on port M4
    Adafruit_DCMotor *leftMotor = AFMS.getMotor(4);
    
    
    void MotorController(uint8_t (BLE_data_server)[2])
    {
        uint8_t button_state;
        uint8_t direction;
        /*Set up constants for direction of motion */
        const char FRONT = '5';
        const char BACK = '6';
        const char LEFT = '7';
        const char RIGHT = '8';
        
        /*Set up constants for button state */
        const char PRESSED = '1';
        const char RELEASED = '0';
        
    
        button_state = BLE_data_server[1];
        direction = BLE_data_server[0];
        
        switch (button_state)
        {
            case PRESSED:
                switch (direction)
                {
                    case FRONT:
                        rightMotor->run(FORWARD);
                        leftMotor->run(FORWARD);
                        rightMotor->setSpeed(MOTOR_FORWARD);
                        leftMotor->setSpeed(MOTOR_FORWARD);
                        break;
                    case BACK:
                        rightMotor->run(BACKWARD);
                        leftMotor->run(BACKWARD);
                        rightMotor->setSpeed(MOTOR_BACKWARD);
                        leftMotor->setSpeed(MOTOR_BACKWARD);
                        break;
                    case LEFT:
                        rightMotor->run(FORWARD);
                        leftMotor->run(BACKWARD);
                        rightMotor->setSpeed(MOTOR_TURN);
                        leftMotor->setSpeed(MOTOR_TURN);
                        break;
                    case RIGHT:
                        rightMotor->run(BACKWARD);
                        leftMotor->run(FORWARD);
                        rightMotor->setSpeed(MOTOR_TURN);
                        leftMotor->setSpeed(MOTOR_TURN);
                        break;
                }      
                break;
            case RELEASED:
                rightMotor->setSpeed(MOTOR_OFF);
                leftMotor->setSpeed(MOTOR_OFF);
                break;
        }
    }
#endif






int main() 
{
    /* Setup begins here: */
    #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
        dprintf("MAX32630FTHR: Master");
    #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
        dprintf("MAX32620FTHR: Slave");
    #endif
    

    #if defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
        //Set Power Hold High for battery operation MAX77650 on MAX32620FTHR
        pw_Hold = POWER_HOLD_ON; 
    #endif    
    
    /*
     * inits the Serial or USBSerial when available (230400 baud).
     * If the serial uart is not is not connected it swiches to USB Serial
     * blinking LED means USBSerial detected, waiting for a connect.
     * It waits up to 30 seconds for a USB terminal connections 
     */
    InitSerial(30*1000, &myled);
    dprintf("Welcome to the SX1276GenericLib");
  
    dprintf("Starting a simple LoRa PingPong");
    

    
    
    /***************************************************************************
     * Grid Eye Sensor: Non-Buffer Program Variables
     **************************************************************************/
    #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
        int frame_idx = 0;
    #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
        #if defined(DEBUG_GRID_EYE_SLAVE)
            int frame_idx = 0;
        #endif
    #endif
    
    
    /***************************************************************************
     * Combined Payload Buffers for LoRa Communications
     **************************************************************************/
    uint8_t BufferTx[BufferSizeTx];
    uint8_t BufferRx[BufferSizeRx];
    
    /***************************************************************************
     * Identification Buffers
     **************************************************************************/
    #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
        uint8_t ID_of_slave[size_signature]; 
    #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
        uint8_t ID_of_master[size_signature];
    #endif
    
    /***************************************************************************
     * BLE Data Buffers
     **************************************************************************/
    #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
        uint8_t curr_ble_to_slave[size_of_ble]; 
    #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
        uint8_t curr_ble_from_master[size_of_ble];
        uint8_t prev_ble_from_master[size_of_ble];
    #endif
    
    /***************************************************************************
     * Grid Eye Sensor Data Buffers
     **************************************************************************/
    #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
        char curr_raw_frame_from_slave[size_of_grid_eye];
        char prev_raw_frame_from_slave[size_of_grid_eye];
        int16_t conv_frame_from_slave[64];
    #elif defined(TARGET_MAX32620FTHR) // Client Device: LoRa Controlled Robot
        char curr_raw_frame_to_master[size_of_grid_eye];
        char prev_raw_frame_to_master[size_of_grid_eye];
        int16_t conv_frame_to_master[64];
    #endif
    
    /***************************************************************************
     * GPS Data Buffers
     **************************************************************************/
    #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
        uint8_t curr_gps_from_slave[size_of_gps];
        uint8_t prev_gps_from_slave[size_of_gps];
    #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
        uint8_t curr_gps_to_master[size_of_gps];
    #endif
    
    /***************************************************************************
     * MAX17055 Data Buffers
     **************************************************************************/
    #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
        uint8_t curr_MAX17055_from_slave[size_of_MAX17055];
        uint8_t prev_MAX17055_from_slave[size_of_MAX17055];
    #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
        uint8_t curr_MAX17055_to_master[size_of_MAX17055];
    #endif
    
    /***************************************************************************
     * MAX77650 Data Buffers
     **************************************************************************/
    #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
        uint8_t curr_MAX77650_from_slave[size_of_MAX77650];
        uint8_t prev_MAX77650_from_slave[size_of_MAX77650];
    #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
        uint8_t curr_MAX77650_to_master[size_of_MAX77650];
    #endif
    
    /***************************************************************************
     * Finish Setting up LoRa Radios: This passes in pointers to Buffers to send
     **************************************************************************/
    SX1276PingPongSetup(BufferTx, BufferRx);
    
    /***************************************************************************
     * Finish Setting up BLE Radio on the MAX32630FTHR: Only on the Master Device
     **************************************************************************/
    #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
        BLE &ble = BLE::Instance();
        /* Create alias for the BLE data to be saved in function above that 
         * writes data to this buffer when function onDataWritten is called 
         */
        onDataWritten_curr_ble_to_slave = curr_ble_to_slave;
        
        /* Initialize BLE */
        ble.init();
        ble.gap().onDisconnection(disconnectionCallback);
        ble.gattServer().onDataWritten(onDataWritten);
        uart = new UARTService(ble);
        /* setup advertising */
        ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
        ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
        ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
                                         (const uint8_t *)"BLE UART", sizeof("BLE UART") - 1);
        ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
                                         (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
        ble.gap().setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
        ble.gap().startAdvertising();
    #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
        /* There is no BLE device that is integrated into the MAX32620FTHR. Also
         * Also this program is meant to use the BLE gateway on the MAX32630FTHR 
         */
    #endif
    
    /***************************************************************************
     * Finish Setting up the motor controller on the MAX32620FTHR: Only on the Slave Device
     **************************************************************************/
    #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
        // No setup on the master device
    #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
        AFMS.begin();  // create with the default frequency 1.6KHz
        //AFMS.begin(1000);  // OR with a different frequency, say 1KHz
      
        // Set the speed to start, from 0 (off) to 255 (max speed)
        rightMotor->setSpeed(150);
        rightMotor->run(FORWARD);
        // turn on motor
        rightMotor->run(RELEASE);
        
        leftMotor->setSpeed(150);
        leftMotor->run(FORWARD);
        // turn on motor
        leftMotor->run(RELEASE);
    #endif
    
    
    while(1) 
    {        
        /***************************************************************************
         * BLE Radio Data 
         **************************************************************************/
        #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
            #if defined(COMPILE_BLE) 
            ble.waitForEvent();
            #endif
        #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
            memcpy(curr_ble_from_master, prev_ble_from_master, sizeof(curr_ble_from_master));
        #endif
        
        /***************************************************************************
         * Grid Eye Sensor 
         **************************************************************************/
        #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
            // Check to see if the contents of the previous scan are the same. If they are different then continue with converting
            #if defined(RASPBERRY_PI)
                if( memcmp(prev_raw_frame_from_slave, curr_raw_frame_from_slave, sizeof(curr_raw_frame_from_slave)) != 0 )
                {
                    char grid_eye_msg[130];
                    grid_eye_msg[0]=0x55;
                    grid_eye_msg[1]=0xaa;
                    for (int idx = 0; idx < 128; idx++)
                        grid_eye_msg[idx+2] = curr_raw_frame_from_slave[idx];
                    //pc.printf(grid_eye_msg);
                    for (int idx = 0; idx < 130; idx++)
                    pc.putc(grid_eye_msg[idx]);
                    pc.printf("\r\n"); // seperate the lines
                }
            #endif
            #if defined(DEBUG_GRID_EYE)
                if( memcmp(prev_raw_frame_from_slave, curr_raw_frame_from_slave, sizeof(curr_raw_frame_from_slave)) != 0 )
                {
                    // Convert raw data sent from slave to a 16 bit integer array by calling this
                    convRaw8x8Data2Point25degC(curr_raw_frame_from_slave, conv_frame_from_slave);
                    
                    // Next Print off the Converted data
                    pc.printf("\r\nFrame %d data: \r\n", frame_idx);
                    uint8_t idx;
                    float pixel_data;
                    for (int y = 0; y < 8; y++) {
                        for (int x = 0; x < 8; x++) {
                            idx = y*8 + x;
                            pixel_data = ((float)conv_frame_from_slave[idx])/4.0;
                            pc.printf("%.2f  \t", pixel_data);
                        }
                        pc.printf("\r\n\r\n");
                    }
                    pc.printf("\r\n");
                    
                    // Increment frame counter
                    frame_idx = frame_idx +1;
                }
            #endif    
                /* Next copy in data received from current data into buffer used for
                 * comparison next time the memcmp above is called. This prevents the 
                 * program from converting the same raw data aquired by the grid eye
                 * sensor on the slave device to the floating point array with the 
                 * 0.25 degrees celsius precision level when there is not new data. 
                 */

            memcpy(prev_raw_frame_from_slave, curr_raw_frame_from_slave, sizeof(curr_raw_frame_from_slave));
        #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
            // Aquire raw data about 8x8 frame from the grid eye sensor in this function call 
            gridEye.getRaw8x8FrameData(curr_raw_frame_to_master);
//            wait_ms( 10 );
            #if defined(RASPBERRY_PI)
                if( memcmp(prev_raw_frame_to_master, curr_raw_frame_to_master, sizeof(curr_raw_frame_to_master)) != 0 )
                {
                    char grid_eye_msg[130];
                    grid_eye_msg[0]=0x55;
                    grid_eye_msg[1]=0xaa;
                    for (int idx = 0; idx < 128; idx++)
                        grid_eye_msg[idx+2] = curr_raw_frame_to_master[idx];
                    //pc.printf(grid_eye_msg);
                    for (int idx = 0; idx < 130; idx++)
                    pc.putc(grid_eye_msg[idx]);
                    pc.printf("\r\n"); // seperate the lines
                }
            #endif
            #if defined(DEBUG_GRID_EYE)
                if( memcmp(prev_raw_frame_to_master, curr_raw_frame_to_master, sizeof(curr_raw_frame_to_master)) != 0 )
                {
                    // Convert raw data sent from slave to a 16 bit integer array by calling this
                    convRaw8x8Data2Point25degC(curr_raw_frame_to_master, conv_frame_to_master);
                    
                    
                    // Next Print off the Converted data
                    pc.printf("\r\nFrame %d data: \r\n", frame_idx);
                    uint8_t idx;
                    float pixel_data;
                    for (int y = 0; y < 8; y++) {
                        for (int x = 0; x < 8; x++) {
                            idx = y*8 + x;
                            pixel_data = ((float)conv_frame_to_master[idx])/4.0;
                            pc.printf("%.2f  \t", pixel_data);
                        }
                        pc.printf("\r\n\r\n");
                    }
                    pc.printf("\r\n");
                    
                    // Increment frame counter
                    frame_idx = frame_idx +1;
                }
            #endif
//            wait_ms( 10 );
            memcpy(prev_raw_frame_to_master, curr_raw_frame_to_master, sizeof(curr_raw_frame_to_master));
        #endif      
        
        /***************************************************************************
         * GPS Sensor 
         **************************************************************************/
        #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
            //None yet
        #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
            // The testing hasnt been done yet for the GPS Module. Will do this when more parts come in. 
            /*
            if(gps.sample()){
              pc.printf("%f\t%c\t%f\t%c\t%f\t%f\t%f\n\r",gps.longitude, gps.ns,gps.latitude,gps.ew, gps.alt, gps.geoid, gps.time);
              pc.printf("%d:%d:%d",gps.hour,gps.minute,gps.seconed);
            }
            */
        #endif
        
        /***************************************************************************
         * Fill Payload Buffer With Data From Sensors for LoRa Transmition
         **************************************************************************/
        #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
            memcpy(&BufferTx[tx_idx_signature], PingMsg,           size_signature);
            memcpy(&BufferTx[tx_idx_ble],       curr_ble_to_slave, size_of_ble);
        #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
            memcpy(&BufferTx[tx_idx_signature], PongMsg,                  size_signature);
            memcpy(&BufferTx[tx_idx_grid_eye],  curr_raw_frame_to_master, size_of_grid_eye);
            memcpy(&BufferTx[tx_idx_gps],       curr_gps_to_master,       size_of_gps);
            memcpy(&BufferTx[tx_idx_MAX17055],  curr_MAX17055_to_master,  size_of_MAX17055);
            memcpy(&BufferTx[tx_idx_MAX77650],  curr_MAX77650_to_master,  size_of_MAX77650);
        #endif
        
        
        
        
        /***************************************************************************
         * Lora Communications
         **************************************************************************/
//        wait_ms( 10 );  
        SX1276PingPong();
        
        /***************************************************************************
         * Fill Global Buffers With Data From Received Payload Buffer
         **************************************************************************/
        /* The master and slave devices will have different requirements for offloading payload */
        #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
            memcpy(ID_of_slave,               &BufferRx[rx_idx_signature], size_signature);
            memcpy(curr_raw_frame_from_slave, &BufferRx[rx_idx_grid_eye],  size_of_grid_eye);
            memcpy(curr_gps_from_slave,       &BufferRx[rx_idx_gps],       size_of_gps);
            memcpy(curr_MAX17055_from_slave,  &BufferRx[rx_idx_MAX17055],  size_of_MAX17055);
            memcpy(curr_MAX77650_from_slave,  &BufferRx[rx_idx_MAX77650],  size_of_MAX77650);
        #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
            memcpy(ID_of_master,              &BufferRx[rx_idx_signature], size_signature);
            memcpy(curr_ble_from_master,      &BufferRx[rx_idx_ble],       size_of_ble);
        #endif
    
        /***************************************************************************
         * Motor Controller controlls go here
         **************************************************************************/
        #if   defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
            // No motor controller on master
        #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
            // Check to see if the contents of the previous ble sdata received are the same. If they are different then change motor controllers registers
            if( memcmp(prev_ble_from_master, curr_ble_from_master, sizeof(curr_ble_from_master)) != 0 )
            {
                MotorController(curr_ble_from_master);
            }
        #endif
        
        
        
    }
    
}