![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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
Diff: main.cpp
- Revision:
- 34:f31a6b53d4ea
- Parent:
- 33:52c898aca207
diff -r 52c898aca207 -r f31a6b53d4ea main.cpp --- a/main.cpp Tue Jul 31 02:00:57 2018 +0000 +++ b/main.cpp Tue Jul 31 15:51:43 2018 +0000 @@ -12,13 +12,10 @@ #include "main.h" #include "global_buffers.h" #include "GridEye.h" -#include "mbed.h" -#include <string> +#include "GPS.h" -//#define DEBUG_GRID_EYE_SLAVE #define RASPBERRY_PI - /* 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. @@ -27,206 +24,108 @@ * additional setup in the main program. */ -#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); -// Initialize GPS - #include "GPS.h" - GPS gps(P0_1, P0_0, 115200); - -// Motor Driver initialization - #include "Adafruit_MotorShield.h" - #include "Adafruit_PWMServoDriver.h" - - #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(p9, p10, 0x60 << 1); - Adafruit_MotorShield AFMS = Adafruit_MotorShield(P3_4, P3_5, 0x60 << 1); - // 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); - Adafruit_DCMotor *leftMotor = AFMS.getMotor(4); - // You can also make another motor on port M2 - //Adafruit_DCMotor *myOtherMotor = AFMS.getMotor(2); - -#endif - -#if defined(TARGET_MAX32620FTHR) -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: - //md_left_pwm = static_cast<float>(20/100.0F); - //md_right_pwm = static_cast<float>(20/100.0F); - - 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 - +/*************************************************************************** + * Create an instance of the MAX32630FTHR related Items. + **************************************************************************/ #if defined(TARGET_MAX32630FTHR) #include "max32630fthr.h" MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3); - #define COMPILE_BLE + + + /* BLE Related MAX32630FTHR Specific stuff */ + #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 defined(COMPILE_BLE) - /* BLE Related MAX32630FTHR Specific stuff */ - #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(); + /* 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); + } - - /* 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; + } +#endif + - /* 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); - } - } - #endif +/*************************************************************************** + * Create an instance of the Panasonic Grid Eye Sensor (AMG88xx) + **************************************************************************/ +#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 functin 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: Robot + //Init I2C communications in default I2C bus I2C #1 + I2C i2cGridEye(P3_4,P3_5); + GridEye gridEye(i2cGridEye); +#endif + +/*************************************************************************** + * Create an instance of the Adafruit Ultimate GPS v3 + + **************************************************************************/ +#if defined(TARGET_MAX32620FTHR) + GPS gps(P0_1, P0_0, 115200); #endif DigitalOut myled(LED); Serial pc(USBTX, USBRX); - -#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 - //Init I2C communications in default I2C bus I2C #1 - // I2C i2cGridEye(SDA,SCL); - //I2C i2cGridEye(P3_4,P3_5); - I2C i2cGridEye(P1_6,P1_7); - GridEye gridEye(i2cGridEye); -#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 @@ -238,7 +137,12 @@ dprintf("Starting a simple LoRa PingPong"); - + /* 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 /*************************************************************************** @@ -247,9 +151,8 @@ #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 + // none yet + int frame_idx = 0; #endif @@ -260,19 +163,32 @@ 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]; + #if defined(COMPILE_BLE) + 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(); + #endif #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]; @@ -326,58 +242,6 @@ **************************************************************************/ 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) { /*************************************************************************** @@ -388,7 +252,7 @@ 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)); + // NOTHING YET #endif /*************************************************************************** @@ -499,6 +363,7 @@ memcpy(prev_raw_frame_to_master, curr_raw_frame_to_master, sizeof(curr_raw_frame_to_master)); #endif + /*************************************************************************** * GPS Sensor **************************************************************************/ @@ -530,7 +395,6 @@ - /*************************************************************************** * Lora Communications **************************************************************************/ @@ -542,28 +406,15 @@ **************************************************************************/ /* 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); + 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); + 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 +