This is example code that can get you started with building your own IR vision robot that communicates over LoRa
Dependencies: Adafruit-MotorShield Adafruit-PWM-Servo-Driver Adafruit_GFX BufferedSerial MAX17055_EZconfig NEO-6m-GPS SX1276GenericLib USBDeviceHT max32630fthr max77650_charger_sample
Fork of MAX326xxFTHR_LoRa_Example_test by
Diff: main.cpp
- Revision:
- 37:f0fcc0341654
- Parent:
- 36:09e8b51fc91d
- Child:
- 38:c10d2a04a0c2
diff -r 09e8b51fc91d -r f0fcc0341654 main.cpp --- a/main.cpp Tue Jul 31 22:39:49 2018 +0000 +++ b/main.cpp Tue Jul 31 23:21:01 2018 +0000 @@ -8,190 +8,50 @@ * 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" -#include "GridEye.h" - //#define DEBUG_GRID_EYE #define RASPBERRY_PI +DigitalOut myled(LED); +Serial pc(USBTX, USBRX); -/* 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. - */ - +/*************************************************************************** + * 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 +// 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 - -#if defined(TARGET_MAX32630FTHR) - #include "max32630fthr.h" - MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3); - - #define COMPILE_BLE - - #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(); - } - - /* 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); - } - } - #endif -#endif - -DigitalOut myled(LED); - -Serial pc(USBTX, USBRX); - +/*************************************************************************** + * 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 @@ -201,15 +61,188 @@ * 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 +/*************************************************************************** + * 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: */