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:
- 32:b108ed6096b0
- Parent:
- 31:f15747cffb20
- Child:
- 33:52c898aca207
--- a/main.cpp Tue Jul 24 18:30:26 2018 +0000 +++ b/main.cpp Mon Jul 30 05:53:55 2018 +0000 @@ -12,7 +12,12 @@ #include "main.h" #include "global_buffers.h" #include "GridEye.h" -#include "GPS.h" +#include "mbed.h" +#include <string> + +//#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 @@ -23,7 +28,100 @@ */ #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 #if defined(TARGET_MAX32630FTHR) @@ -74,16 +172,7 @@ led2 = !led2; 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'; - + /* If there is a valid data sent by the mobile app */ if ((uart != NULL) && (params->handle == uart ->getTXCharacteristicHandle())) { const uint8_t *packet = params->data; @@ -96,49 +185,6 @@ dprintf("direction: %d\n", direction); dprintf("button_state: %d\n", button_state); - - /* - 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: - md_left_dir = 1; - md_left_pwm = 0.7; - md_right_dir = 1; - md_right_pwm = 0.7; - break; - case BACK: - md_left_dir = 0; - md_left_pwm = 0.7; - md_right_dir = 0; - md_right_pwm = 0.7; - break; - case LEFT: - md_left_dir = 0; - md_left_pwm = 0.5; - md_right_dir = 1; - md_right_pwm = 0.5; - break; - case RIGHT: - md_left_dir = 1; - md_left_pwm = 0.5; - md_right_dir = 0; - md_right_pwm = 0.5; - break; - } - break; - case RELEASED: - md_left_pwm = 0; - md_right_pwm = 0; - break; - } - */ - } } #endif @@ -155,17 +201,32 @@ * 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 + * 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: Robot +#elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot //Init I2C communications in default I2C bus I2C #1 - I2C i2cGridEye(P3_4,P3_5); + // 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 @@ -177,12 +238,7 @@ 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 + /*************************************************************************** @@ -191,8 +247,9 @@ #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway int frame_idx = 0; #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot - // none yet - int frame_idx = 0; + #if defined(DEBUG_GRID_EYE_SLAVE) + int frame_idx = 0; + #endif #endif @@ -203,32 +260,19 @@ 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]; @@ -282,6 +326,58 @@ **************************************************************************/ 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) { /*************************************************************************** @@ -292,7 +388,7 @@ ble.waitForEvent(); #endif #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot - // NOTHING YET + memcpy(curr_ble_from_master, prev_ble_from_master, sizeof(curr_ble_from_master)); #endif /*************************************************************************** @@ -300,66 +396,95 @@ **************************************************************************/ #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( 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); + #if defined(RASPBERRY_PI) + if( memcmp(prev_raw_frame_from_slave, curr_raw_frame_from_slave, sizeof(curr_raw_frame_from_slave)) != 0 ) + { + /* + //for (int idx = 0; idx < 8; idx++) + // pc.printf("%c", ID_of_slave[idx]); + string grid_eye_msg; + grid_eye_msg[0]=0x55; + grid_eye_msg[1]=0xaa; + for (int idx = 0; idx < 128; idx++) + grid_eye_msg[idx+2] = (unsigned char)curr_raw_frame_from_slave[idx]; + pc.printf(grid_eye_msg); + */ + 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]); + } + #else + 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\r\n"); + pc.printf("\r\n"); + + // Increment frame counter + frame_idx = frame_idx +1; } - pc.printf("\r\n"); - - // Increment frame counter - frame_idx = frame_idx +1; - } - - /* 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. - */ + #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 ( 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 = conv_frame_to_master[idx]/4.0; - pc.printf("%.2f \t", pixel_data); + wait_ms( 20 ); + + #if defined(DEBUG_GRID_EYE_SLAVE) + if( memcmp(prev_raw_frame_to_master, curr_raw_frame_to_master, sizeof(curr_raw_frame_to_master)) != 0 ) + { + wait_ms( 5 ); + // 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); + + wait_ms( 5 ); + + // 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\r\n"); + pc.printf("\r\n"); + + // Increment frame counter + frame_idx = frame_idx +1; } - pc.printf("\r\n"); - - // Increment frame counter - frame_idx = frame_idx +1; - //} - */ - memcpy(prev_raw_frame_to_master, curr_raw_frame_to_master, sizeof(curr_raw_frame_to_master)); + wait_ms( 20 ); + memcpy(prev_raw_frame_to_master, curr_raw_frame_to_master, sizeof(curr_raw_frame_to_master)); + + #endif #endif /*************************************************************************** @@ -393,6 +518,7 @@ + /*************************************************************************** * Lora Communications **************************************************************************/ @@ -404,15 +530,28 @@ **************************************************************************/ /* The master and slave devices will have different requirements for offloading payload */ #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway - 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(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(curr_ble_from_master, &BufferRx[rx_idx_ble], size_of_ble); + 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