Team Walter / Mbed OS Low_Power_Long_Distance_IR_Vision_Robot

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@39:df7c8bd2212e, 2018-08-01 (annotated)

Committer:
dev_alexander
Date:
Wed Aug 01 03:41:07 2018 +0000
Revision:
39:df7c8bd2212e
Parent:
36:09e8b51fc91d
Child:
40:6f8744c366c2
This is modifications on a previous revision that show bluetooth connection is stable without a delay. Look into past revisions and fix issue

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Helmut64 0:c43b6919ae15 1 /*
Helmut64 17:98f2528e8399 2 * Copyright (c) 2018 HELIOS Software GmbH
Helmut64 0:c43b6919ae15 3 * 30826 Garbsen (Hannover) Germany
Helmut64 0:c43b6919ae15 4 * Licensed under the Apache License, Version 2.0);
Helmut64 0:c43b6919ae15 5 */
dev_alexander 21:1b92cabe8a3b 6
dev_alexander 21:1b92cabe8a3b 7 /*
dev_alexander 21:1b92cabe8a3b 8 * For Wiring Instructions, please visit the link below:
dev_alexander 21:1b92cabe8a3b 9 * https://www.hackster.io/DevinAlex64/getting-started-with-the-max32620fthr-and-lora-f9d8dd\
dev_alexander 21:1b92cabe8a3b 10 */
dev_alexander 21:1b92cabe8a3b 11
dev_alexander 30:66f9750cc44c 12 #include "main.h"
dev_alexander 30:66f9750cc44c 13 #include "global_buffers.h"
dev_alexander 30:66f9750cc44c 14 #include "GridEye.h"
dev_alexander 32:b108ed6096b0 15
dev_alexander 35:bae9b236070b 16 //#define DEBUG_GRID_EYE
dev_alexander 32:b108ed6096b0 17 #define RASPBERRY_PI
dev_alexander 32:b108ed6096b0 18
dev_alexander 23:f74a50977593 19
dev_alexander 23:f74a50977593 20 /* If the board that is compiled is the master device (BLE enabled MAX32630FTHR),
dev_alexander 23:f74a50977593 21 * then it needs this library and needs to be configed in order for the device to
dev_alexander 23:f74a50977593 22 * work with a 3.3volt output instead of a 1.8 volt output.
dev_alexander 23:f74a50977593 23 *
dev_alexander 23:f74a50977593 24 * This is only needed for the MAX32630FTHR. The MAX325620FTHR is exampt from this
dev_alexander 23:f74a50977593 25 * additional setup in the main program.
dev_alexander 23:f74a50977593 26 */
dev_alexander 31:f15747cffb20 27
dev_alexander 31:f15747cffb20 28 #if defined(TARGET_MAX32620FTHR)
dev_alexander 32:b108ed6096b0 29 // Definitions for running board off of battery
dev_alexander 32:b108ed6096b0 30 #define POWER_HOLD_ON 1
dev_alexander 32:b108ed6096b0 31 #define POWER_HOLD_OFF 0
dev_alexander 32:b108ed6096b0 32
dev_alexander 32:b108ed6096b0 33 // MAX77650
dev_alexander 32:b108ed6096b0 34 DigitalOut pw_Hold(P2_2, POWER_HOLD_ON);
dev_alexander 32:b108ed6096b0 35
dev_alexander 32:b108ed6096b0 36 // Initialize GPS
dev_alexander 32:b108ed6096b0 37 #include "GPS.h"
dev_alexander 31:f15747cffb20 38 GPS gps(P0_1, P0_0, 115200);
dev_alexander 32:b108ed6096b0 39
dev_alexander 32:b108ed6096b0 40 // Motor Driver initialization
dev_alexander 32:b108ed6096b0 41 #include "Adafruit_MotorShield.h"
dev_alexander 32:b108ed6096b0 42 #include "Adafruit_PWMServoDriver.h"
dev_alexander 32:b108ed6096b0 43
dev_alexander 32:b108ed6096b0 44 #define MOTOR_OFF 0
dev_alexander 32:b108ed6096b0 45 #define MOTOR_FORWARD 150
dev_alexander 32:b108ed6096b0 46 #define MOTOR_BACKWARD 150
dev_alexander 32:b108ed6096b0 47 #define MOTOR_TURN 100
dev_alexander 32:b108ed6096b0 48
dev_alexander 32:b108ed6096b0 49
dev_alexander 32:b108ed6096b0 50 // Create the motor shield object with the default I2C address
dev_alexander 32:b108ed6096b0 51 //Adafruit_MotorShield AFMS = Adafruit_MotorShield(p9, p10, 0x60 << 1);
dev_alexander 32:b108ed6096b0 52 Adafruit_MotorShield AFMS = Adafruit_MotorShield(P3_4, P3_5, 0x60 << 1);
dev_alexander 32:b108ed6096b0 53 // Or, create it with a different I2C address (say for stacking)
dev_alexander 32:b108ed6096b0 54 // Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61);
dev_alexander 32:b108ed6096b0 55
dev_alexander 32:b108ed6096b0 56 // Select which 'port' M1, M2, M3 or M4. In this case, M1
dev_alexander 32:b108ed6096b0 57 Adafruit_DCMotor *rightMotor = AFMS.getMotor(1);
dev_alexander 32:b108ed6096b0 58 Adafruit_DCMotor *leftMotor = AFMS.getMotor(4);
dev_alexander 32:b108ed6096b0 59 // You can also make another motor on port M2
dev_alexander 32:b108ed6096b0 60 //Adafruit_DCMotor *myOtherMotor = AFMS.getMotor(2);
dev_alexander 32:b108ed6096b0 61
dev_alexander 32:b108ed6096b0 62 #endif
dev_alexander 32:b108ed6096b0 63
dev_alexander 32:b108ed6096b0 64 #if defined(TARGET_MAX32620FTHR)
dev_alexander 32:b108ed6096b0 65 void MotorController(uint8_t (BLE_data_server)[2])
dev_alexander 32:b108ed6096b0 66 {
dev_alexander 32:b108ed6096b0 67 uint8_t button_state;
dev_alexander 32:b108ed6096b0 68 uint8_t direction;
dev_alexander 32:b108ed6096b0 69 /*Set up constants for direction of motion */
dev_alexander 32:b108ed6096b0 70 const char FRONT = '5';
dev_alexander 32:b108ed6096b0 71 const char BACK = '6';
dev_alexander 32:b108ed6096b0 72 const char LEFT = '7';
dev_alexander 32:b108ed6096b0 73 const char RIGHT = '8';
dev_alexander 32:b108ed6096b0 74
dev_alexander 32:b108ed6096b0 75 /*Set up constants for button state */
dev_alexander 32:b108ed6096b0 76 const char PRESSED = '1';
dev_alexander 32:b108ed6096b0 77 const char RELEASED = '0';
dev_alexander 32:b108ed6096b0 78
dev_alexander 32:b108ed6096b0 79
dev_alexander 32:b108ed6096b0 80 button_state = BLE_data_server[1];
dev_alexander 32:b108ed6096b0 81 direction = BLE_data_server[0];
dev_alexander 32:b108ed6096b0 82
dev_alexander 32:b108ed6096b0 83 switch (button_state)
dev_alexander 32:b108ed6096b0 84 {
dev_alexander 32:b108ed6096b0 85 case PRESSED:
dev_alexander 32:b108ed6096b0 86 //md_left_pwm = static_cast<float>(20/100.0F);
dev_alexander 32:b108ed6096b0 87 //md_right_pwm = static_cast<float>(20/100.0F);
dev_alexander 32:b108ed6096b0 88
dev_alexander 32:b108ed6096b0 89 switch (direction)
dev_alexander 32:b108ed6096b0 90 {
dev_alexander 32:b108ed6096b0 91 case FRONT:
dev_alexander 32:b108ed6096b0 92 rightMotor->run(FORWARD);
dev_alexander 32:b108ed6096b0 93 leftMotor->run(FORWARD);
dev_alexander 32:b108ed6096b0 94 rightMotor->setSpeed(MOTOR_FORWARD);
dev_alexander 32:b108ed6096b0 95 leftMotor->setSpeed(MOTOR_FORWARD);
dev_alexander 32:b108ed6096b0 96 break;
dev_alexander 32:b108ed6096b0 97 case BACK:
dev_alexander 32:b108ed6096b0 98 rightMotor->run(BACKWARD);
dev_alexander 32:b108ed6096b0 99 leftMotor->run(BACKWARD);
dev_alexander 32:b108ed6096b0 100 rightMotor->setSpeed(MOTOR_BACKWARD);
dev_alexander 32:b108ed6096b0 101 leftMotor->setSpeed(MOTOR_BACKWARD);
dev_alexander 32:b108ed6096b0 102 break;
dev_alexander 32:b108ed6096b0 103 case LEFT:
dev_alexander 32:b108ed6096b0 104 rightMotor->run(FORWARD);
dev_alexander 32:b108ed6096b0 105 leftMotor->run(BACKWARD);
dev_alexander 32:b108ed6096b0 106 rightMotor->setSpeed(MOTOR_TURN);
dev_alexander 32:b108ed6096b0 107 leftMotor->setSpeed(MOTOR_TURN);
dev_alexander 32:b108ed6096b0 108 break;
dev_alexander 32:b108ed6096b0 109 case RIGHT:
dev_alexander 32:b108ed6096b0 110 rightMotor->run(BACKWARD);
dev_alexander 32:b108ed6096b0 111 leftMotor->run(FORWARD);
dev_alexander 32:b108ed6096b0 112 rightMotor->setSpeed(MOTOR_TURN);
dev_alexander 32:b108ed6096b0 113 leftMotor->setSpeed(MOTOR_TURN);
dev_alexander 32:b108ed6096b0 114 break;
dev_alexander 32:b108ed6096b0 115 }
dev_alexander 32:b108ed6096b0 116 break;
dev_alexander 32:b108ed6096b0 117 case RELEASED:
dev_alexander 32:b108ed6096b0 118 rightMotor->setSpeed(MOTOR_OFF);
dev_alexander 32:b108ed6096b0 119 leftMotor->setSpeed(MOTOR_OFF);
dev_alexander 32:b108ed6096b0 120 break;
dev_alexander 32:b108ed6096b0 121 }
dev_alexander 32:b108ed6096b0 122 }
dev_alexander 29:f7a0e49b826b 123 #endif
Helmut64 0:c43b6919ae15 124
dev_alexander 31:f15747cffb20 125 #if defined(TARGET_MAX32630FTHR)
dev_alexander 31:f15747cffb20 126 #include "max32630fthr.h"
dev_alexander 31:f15747cffb20 127 MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
dev_alexander 31:f15747cffb20 128
dev_alexander 31:f15747cffb20 129 #define COMPILE_BLE
dev_alexander 31:f15747cffb20 130
dev_alexander 31:f15747cffb20 131 #if defined(COMPILE_BLE)
dev_alexander 31:f15747cffb20 132 /* BLE Related MAX32630FTHR Specific stuff */
dev_alexander 31:f15747cffb20 133 #include "ble/BLE.h"
dev_alexander 31:f15747cffb20 134 //#include "ble/Gap.h"
dev_alexander 31:f15747cffb20 135 #include "UARTService_custom.h"
dev_alexander 31:f15747cffb20 136 #include <events/mbed_events.h>
dev_alexander 31:f15747cffb20 137
dev_alexander 31:f15747cffb20 138 DigitalOut led2(LED2);
dev_alexander 31:f15747cffb20 139 UARTService *uart;
dev_alexander 30:66f9750cc44c 140
dev_alexander 31:f15747cffb20 141 #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
dev_alexander 31:f15747cffb20 142 * it will have an impact on code-size and power consumption. */
dev_alexander 31:f15747cffb20 143
dev_alexander 31:f15747cffb20 144 #if NEED_CONSOLE_OUTPUT
dev_alexander 31:f15747cffb20 145 #define DEBUG(STR) { if (uart) uart->write(STR, strlen(STR)); }
dev_alexander 31:f15747cffb20 146 #define READ(STR) { if (uart) uart->read(STR, strlen(STR)); }
dev_alexander 31:f15747cffb20 147
dev_alexander 31:f15747cffb20 148 #else
dev_alexander 31:f15747cffb20 149 #define DEBUG(...) /* nothing */
dev_alexander 31:f15747cffb20 150 #endif /* #if NEED_CONSOLE_OUTPUT */
dev_alexander 31:f15747cffb20 151
dev_alexander 31:f15747cffb20 152 //Triggered when the robot is disconnected from the mobile app.
dev_alexander 31:f15747cffb20 153 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
dev_alexander 31:f15747cffb20 154 {
dev_alexander 31:f15747cffb20 155 BLE &ble = BLE::Instance();
dev_alexander 31:f15747cffb20 156 DEBUG("Disconnected!\n\r");
dev_alexander 31:f15747cffb20 157 DEBUG("Restarting the advertising process\n\r");
dev_alexander 31:f15747cffb20 158 ble.gap().startAdvertising();
dev_alexander 31:f15747cffb20 159 }
dev_alexander 31:f15747cffb20 160
dev_alexander 31:f15747cffb20 161 /* This pointer is needed to reference data in the main program when filling
dev_alexander 31:f15747cffb20 162 * with data from the BLE
dev_alexander 31:f15747cffb20 163 */
dev_alexander 31:f15747cffb20 164 uint8_t * onDataWritten_curr_ble_to_slave;
dev_alexander 30:66f9750cc44c 165
dev_alexander 31:f15747cffb20 166 // Triggered when there is data written
dev_alexander 31:f15747cffb20 167 void onDataWritten(const GattWriteCallbackParams *params)
dev_alexander 31:f15747cffb20 168 {
dev_alexander 31:f15747cffb20 169 //LED1 will be off when button is pressed, and on when button is released.
dev_alexander 31:f15747cffb20 170 led2 = !led2;
dev_alexander 31:f15747cffb20 171 uint8_t button_state;
dev_alexander 31:f15747cffb20 172 uint8_t direction;
dev_alexander 32:b108ed6096b0 173
dev_alexander 31:f15747cffb20 174 /* If there is a valid data sent by the mobile app */
dev_alexander 31:f15747cffb20 175 if ((uart != NULL) && (params->handle == uart ->getTXCharacteristicHandle())) {
dev_alexander 31:f15747cffb20 176 const uint8_t *packet = params->data;
dev_alexander 31:f15747cffb20 177 button_state = packet[3];
dev_alexander 31:f15747cffb20 178 direction = packet[2];
dev_alexander 31:f15747cffb20 179
dev_alexander 31:f15747cffb20 180 // make parameter to send over Lora
dev_alexander 31:f15747cffb20 181 onDataWritten_curr_ble_to_slave[0] = direction;
dev_alexander 31:f15747cffb20 182 onDataWritten_curr_ble_to_slave[1] = button_state;
dev_alexander 31:f15747cffb20 183
dev_alexander 33:52c898aca207 184 //dprintf("direction: %d\n", direction);
dev_alexander 33:52c898aca207 185 //dprintf("button_state: %d\n", button_state);
dev_alexander 30:66f9750cc44c 186 }
dev_alexander 30:66f9750cc44c 187 }
dev_alexander 31:f15747cffb20 188 #endif
dev_alexander 30:66f9750cc44c 189 #endif
dev_alexander 30:66f9750cc44c 190
Helmut64 17:98f2528e8399 191 DigitalOut myled(LED);
Helmut64 0:c43b6919ae15 192
dev_alexander 28:0ed92c590607 193 Serial pc(USBTX, USBRX);
dev_alexander 25:1a031add188a 194
Helmut64 0:c43b6919ae15 195
dev_alexander 23:f74a50977593 196 #if defined(TARGET_MAX32630FTHR) // Master Device: MAX32630FTHR BLE-to-LoRa Gateway
dev_alexander 23:f74a50977593 197 /* No grid eye object needed. The function the master uses to convert the raw
dev_alexander 23:f74a50977593 198 * data that is sent over from the slave deevice that contains the data from the
dev_alexander 23:f74a50977593 199 * actual grid eye sensor is actually a function that is not part of the grid eye
dev_alexander 23:f74a50977593 200 * class. this is due to the fact that the grid eye class requires n i2c bus to
dev_alexander 23:f74a50977593 201 * be assigned to a phyiscal sensor. In this case, the library for the Grid Eye
dev_alexander 32:b108ed6096b0 202 * sensor has a support function that is used to convert data that is aquired from
dev_alexander 23:f74a50977593 203 * the grid eye sensor. So it is not supposed to be a class specific function.
dev_alexander 23:f74a50977593 204 */
dev_alexander 32:b108ed6096b0 205 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 23:f74a50977593 206 //Init I2C communications in default I2C bus I2C #1
dev_alexander 32:b108ed6096b0 207 // I2C i2cGridEye(SDA,SCL);
dev_alexander 32:b108ed6096b0 208 //I2C i2cGridEye(P3_4,P3_5);
dev_alexander 32:b108ed6096b0 209 I2C i2cGridEye(P1_6,P1_7);
dev_alexander 23:f74a50977593 210 GridEye gridEye(i2cGridEye);
dev_alexander 23:f74a50977593 211 #endif
dev_alexander 19:9f035b9e65ec 212
dev_alexander 23:f74a50977593 213 int main()
dev_alexander 23:f74a50977593 214 {
dev_alexander 32:b108ed6096b0 215 /* Setup begins here: */
dev_alexander 32:b108ed6096b0 216 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 32:b108ed6096b0 217 dprintf("MAX32630FTHR: Master");
dev_alexander 32:b108ed6096b0 218 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 32:b108ed6096b0 219 dprintf("MAX32620FTHR: Slave");
dev_alexander 32:b108ed6096b0 220 #endif
dev_alexander 32:b108ed6096b0 221
dev_alexander 32:b108ed6096b0 222
dev_alexander 32:b108ed6096b0 223 #if defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 32:b108ed6096b0 224 //Set Power Hold High for battery operation MAX77650 on MAX32620FTHR
dev_alexander 32:b108ed6096b0 225 pw_Hold = POWER_HOLD_ON;
dev_alexander 32:b108ed6096b0 226 #endif
dev_alexander 32:b108ed6096b0 227
Helmut64 17:98f2528e8399 228 /*
Helmut64 17:98f2528e8399 229 * inits the Serial or USBSerial when available (230400 baud).
Helmut64 17:98f2528e8399 230 * If the serial uart is not is not connected it swiches to USB Serial
Helmut64 17:98f2528e8399 231 * blinking LED means USBSerial detected, waiting for a connect.
Helmut64 17:98f2528e8399 232 * It waits up to 30 seconds for a USB terminal connections
Helmut64 17:98f2528e8399 233 */
Helmut64 17:98f2528e8399 234 InitSerial(30*1000, &myled);
Helmut64 17:98f2528e8399 235 dprintf("Welcome to the SX1276GenericLib");
Helmut64 18:d5527ce82e6b 236
Helmut64 17:98f2528e8399 237 dprintf("Starting a simple LoRa PingPong");
dev_alexander 23:f74a50977593 238
dev_alexander 32:b108ed6096b0 239
dev_alexander 26:69aba05f010f 240
dev_alexander 23:f74a50977593 241
dev_alexander 23:f74a50977593 242 /***************************************************************************
dev_alexander 23:f74a50977593 243 * Grid Eye Sensor: Non-Buffer Program Variables
dev_alexander 23:f74a50977593 244 **************************************************************************/
dev_alexander 23:f74a50977593 245 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 23:f74a50977593 246 int frame_idx = 0;
dev_alexander 24:e8d03912f303 247 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 32:b108ed6096b0 248 #if defined(DEBUG_GRID_EYE_SLAVE)
dev_alexander 32:b108ed6096b0 249 int frame_idx = 0;
dev_alexander 32:b108ed6096b0 250 #endif
dev_alexander 23:f74a50977593 251 #endif
dev_alexander 23:f74a50977593 252
dev_alexander 23:f74a50977593 253
dev_alexander 28:0ed92c590607 254 /***************************************************************************
dev_alexander 28:0ed92c590607 255 * Combined Payload Buffers for LoRa Communications
dev_alexander 28:0ed92c590607 256 **************************************************************************/
dev_alexander 28:0ed92c590607 257 uint8_t BufferTx[BufferSizeTx];
dev_alexander 28:0ed92c590607 258 uint8_t BufferRx[BufferSizeRx];
dev_alexander 28:0ed92c590607 259
dev_alexander 28:0ed92c590607 260 /***************************************************************************
dev_alexander 32:b108ed6096b0 261 * Identification Buffers
dev_alexander 32:b108ed6096b0 262 **************************************************************************/
dev_alexander 32:b108ed6096b0 263 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 32:b108ed6096b0 264 uint8_t ID_of_slave[size_signature];
dev_alexander 32:b108ed6096b0 265 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 32:b108ed6096b0 266 uint8_t ID_of_master[size_signature];
dev_alexander 32:b108ed6096b0 267 #endif
dev_alexander 32:b108ed6096b0 268
dev_alexander 32:b108ed6096b0 269 /***************************************************************************
dev_alexander 28:0ed92c590607 270 * BLE Data Buffers
dev_alexander 28:0ed92c590607 271 **************************************************************************/
dev_alexander 28:0ed92c590607 272 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 31:f15747cffb20 273 uint8_t curr_ble_to_slave[size_of_ble];
dev_alexander 28:0ed92c590607 274 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 28:0ed92c590607 275 uint8_t curr_ble_from_master[size_of_ble];
dev_alexander 28:0ed92c590607 276 uint8_t prev_ble_from_master[size_of_ble];
dev_alexander 28:0ed92c590607 277 #endif
dev_alexander 28:0ed92c590607 278
dev_alexander 28:0ed92c590607 279 /***************************************************************************
dev_alexander 28:0ed92c590607 280 * Grid Eye Sensor Data Buffers
dev_alexander 28:0ed92c590607 281 **************************************************************************/
dev_alexander 28:0ed92c590607 282 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 28:0ed92c590607 283 char curr_raw_frame_from_slave[size_of_grid_eye];
dev_alexander 28:0ed92c590607 284 char prev_raw_frame_from_slave[size_of_grid_eye];
dev_alexander 28:0ed92c590607 285 int16_t conv_frame_from_slave[64];
dev_alexander 28:0ed92c590607 286 #elif defined(TARGET_MAX32620FTHR) // Client Device: LoRa Controlled Robot
dev_alexander 28:0ed92c590607 287 char curr_raw_frame_to_master[size_of_grid_eye];
dev_alexander 28:0ed92c590607 288 char prev_raw_frame_to_master[size_of_grid_eye];
dev_alexander 28:0ed92c590607 289 int16_t conv_frame_to_master[64];
dev_alexander 28:0ed92c590607 290 #endif
dev_alexander 28:0ed92c590607 291
dev_alexander 28:0ed92c590607 292 /***************************************************************************
dev_alexander 28:0ed92c590607 293 * GPS Data Buffers
dev_alexander 28:0ed92c590607 294 **************************************************************************/
dev_alexander 28:0ed92c590607 295 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 28:0ed92c590607 296 uint8_t curr_gps_from_slave[size_of_gps];
dev_alexander 28:0ed92c590607 297 uint8_t prev_gps_from_slave[size_of_gps];
dev_alexander 28:0ed92c590607 298 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 28:0ed92c590607 299 uint8_t curr_gps_to_master[size_of_gps];
dev_alexander 28:0ed92c590607 300 #endif
dev_alexander 28:0ed92c590607 301
dev_alexander 28:0ed92c590607 302 /***************************************************************************
dev_alexander 28:0ed92c590607 303 * MAX17055 Data Buffers
dev_alexander 28:0ed92c590607 304 **************************************************************************/
dev_alexander 28:0ed92c590607 305 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 28:0ed92c590607 306 uint8_t curr_MAX17055_from_slave[size_of_MAX17055];
dev_alexander 28:0ed92c590607 307 uint8_t prev_MAX17055_from_slave[size_of_MAX17055];
dev_alexander 28:0ed92c590607 308 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 28:0ed92c590607 309 uint8_t curr_MAX17055_to_master[size_of_MAX17055];
dev_alexander 28:0ed92c590607 310 #endif
dev_alexander 28:0ed92c590607 311
dev_alexander 28:0ed92c590607 312 /***************************************************************************
dev_alexander 28:0ed92c590607 313 * MAX77650 Data Buffers
dev_alexander 28:0ed92c590607 314 **************************************************************************/
dev_alexander 28:0ed92c590607 315 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 28:0ed92c590607 316 uint8_t curr_MAX77650_from_slave[size_of_MAX77650];
dev_alexander 28:0ed92c590607 317 uint8_t prev_MAX77650_from_slave[size_of_MAX77650];
dev_alexander 28:0ed92c590607 318 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 28:0ed92c590607 319 uint8_t curr_MAX77650_to_master[size_of_MAX77650];
dev_alexander 28:0ed92c590607 320 #endif
dev_alexander 28:0ed92c590607 321
dev_alexander 28:0ed92c590607 322 /***************************************************************************
dev_alexander 28:0ed92c590607 323 * Finish Setting up LoRa Radios: This passes in pointers to Buffers to send
dev_alexander 28:0ed92c590607 324 **************************************************************************/
dev_alexander 28:0ed92c590607 325 SX1276PingPongSetup(BufferTx, BufferRx);
dev_alexander 28:0ed92c590607 326
dev_alexander 32:b108ed6096b0 327 /***************************************************************************
dev_alexander 32:b108ed6096b0 328 * Finish Setting up BLE Radio on the MAX32630FTHR: Only on the Master Device
dev_alexander 32:b108ed6096b0 329 **************************************************************************/
dev_alexander 32:b108ed6096b0 330 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 32:b108ed6096b0 331 BLE &ble = BLE::Instance();
dev_alexander 32:b108ed6096b0 332 /* Create alias for the BLE data to be saved in function above that
dev_alexander 32:b108ed6096b0 333 * writes data to this buffer when function onDataWritten is called
dev_alexander 32:b108ed6096b0 334 */
dev_alexander 32:b108ed6096b0 335 onDataWritten_curr_ble_to_slave = curr_ble_to_slave;
dev_alexander 32:b108ed6096b0 336
dev_alexander 32:b108ed6096b0 337 /* Initialize BLE */
dev_alexander 32:b108ed6096b0 338 ble.init();
dev_alexander 32:b108ed6096b0 339 ble.gap().onDisconnection(disconnectionCallback);
dev_alexander 32:b108ed6096b0 340 ble.gattServer().onDataWritten(onDataWritten);
dev_alexander 32:b108ed6096b0 341 uart = new UARTService(ble);
dev_alexander 32:b108ed6096b0 342 /* setup advertising */
dev_alexander 32:b108ed6096b0 343 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
dev_alexander 32:b108ed6096b0 344 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
dev_alexander 32:b108ed6096b0 345 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
dev_alexander 32:b108ed6096b0 346 (const uint8_t *)"BLE UART", sizeof("BLE UART") - 1);
dev_alexander 32:b108ed6096b0 347 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
dev_alexander 32:b108ed6096b0 348 (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
dev_alexander 32:b108ed6096b0 349 ble.gap().setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
dev_alexander 32:b108ed6096b0 350 ble.gap().startAdvertising();
dev_alexander 32:b108ed6096b0 351 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 32:b108ed6096b0 352 /* There is no BLE device that is integrated into the MAX32620FTHR. Also
dev_alexander 32:b108ed6096b0 353 * Also this program is meant to use the BLE gateway on the MAX32630FTHR
dev_alexander 32:b108ed6096b0 354 */
dev_alexander 32:b108ed6096b0 355 #endif
dev_alexander 32:b108ed6096b0 356
dev_alexander 32:b108ed6096b0 357 /***************************************************************************
dev_alexander 32:b108ed6096b0 358 * Finish Setting up the motor controller on the MAX32620FTHR: Only on the Slave Device
dev_alexander 32:b108ed6096b0 359 **************************************************************************/
dev_alexander 32:b108ed6096b0 360 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 32:b108ed6096b0 361 // No setup on the master device
dev_alexander 32:b108ed6096b0 362 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 32:b108ed6096b0 363 AFMS.begin(); // create with the default frequency 1.6KHz
dev_alexander 32:b108ed6096b0 364 //AFMS.begin(1000); // OR with a different frequency, say 1KHz
dev_alexander 32:b108ed6096b0 365
dev_alexander 32:b108ed6096b0 366 // Set the speed to start, from 0 (off) to 255 (max speed)
dev_alexander 32:b108ed6096b0 367 rightMotor->setSpeed(150);
dev_alexander 32:b108ed6096b0 368 rightMotor->run(FORWARD);
dev_alexander 32:b108ed6096b0 369 // turn on motor
dev_alexander 32:b108ed6096b0 370 rightMotor->run(RELEASE);
dev_alexander 32:b108ed6096b0 371
dev_alexander 32:b108ed6096b0 372 leftMotor->setSpeed(150);
dev_alexander 32:b108ed6096b0 373 leftMotor->run(FORWARD);
dev_alexander 32:b108ed6096b0 374 // turn on motor
dev_alexander 32:b108ed6096b0 375 leftMotor->run(RELEASE);
dev_alexander 32:b108ed6096b0 376 #endif
dev_alexander 32:b108ed6096b0 377
dev_alexander 32:b108ed6096b0 378
dev_alexander 22:abca9d17d13d 379 while(1)
dev_alexander 28:0ed92c590607 380 {
dev_alexander 23:f74a50977593 381 /***************************************************************************
dev_alexander 30:66f9750cc44c 382 * BLE Radio Data
dev_alexander 30:66f9750cc44c 383 **************************************************************************/
dev_alexander 30:66f9750cc44c 384 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 30:66f9750cc44c 385 #if defined(COMPILE_BLE)
dev_alexander 30:66f9750cc44c 386 ble.waitForEvent();
dev_alexander 30:66f9750cc44c 387 #endif
dev_alexander 30:66f9750cc44c 388 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 32:b108ed6096b0 389 memcpy(curr_ble_from_master, prev_ble_from_master, sizeof(curr_ble_from_master));
dev_alexander 30:66f9750cc44c 390 #endif
dev_alexander 30:66f9750cc44c 391
dev_alexander 39:df7c8bd2212e 392 // wait_ms( 1 ); // This is a tiny delay that enables the Bluetooth connection to be able to detect interrupts
dev_alexander 39:df7c8bd2212e 393
dev_alexander 30:66f9750cc44c 394 /***************************************************************************
dev_alexander 23:f74a50977593 395 * Grid Eye Sensor
dev_alexander 23:f74a50977593 396 **************************************************************************/
dev_alexander 23:f74a50977593 397 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 28:0ed92c590607 398 // Check to see if the contents of the previous scan are the same. If they are different then continue with converting
dev_alexander 32:b108ed6096b0 399 #if defined(RASPBERRY_PI)
dev_alexander 32:b108ed6096b0 400 if( memcmp(prev_raw_frame_from_slave, curr_raw_frame_from_slave, sizeof(curr_raw_frame_from_slave)) != 0 )
dev_alexander 32:b108ed6096b0 401 {
dev_alexander 32:b108ed6096b0 402 char grid_eye_msg[130];
dev_alexander 32:b108ed6096b0 403 grid_eye_msg[0]=0x55;
dev_alexander 32:b108ed6096b0 404 grid_eye_msg[1]=0xaa;
dev_alexander 32:b108ed6096b0 405 for (int idx = 0; idx < 128; idx++)
dev_alexander 32:b108ed6096b0 406 grid_eye_msg[idx+2] = curr_raw_frame_from_slave[idx];
dev_alexander 32:b108ed6096b0 407 //pc.printf(grid_eye_msg);
dev_alexander 32:b108ed6096b0 408 for (int idx = 0; idx < 130; idx++)
dev_alexander 32:b108ed6096b0 409 pc.putc(grid_eye_msg[idx]);
dev_alexander 35:bae9b236070b 410 pc.printf("\r\n"); // seperate the lines
dev_alexander 32:b108ed6096b0 411 }
dev_alexander 35:bae9b236070b 412 #endif
dev_alexander 35:bae9b236070b 413 #if defined(DEBUG_GRID_EYE)
dev_alexander 32:b108ed6096b0 414 if( memcmp(prev_raw_frame_from_slave, curr_raw_frame_from_slave, sizeof(curr_raw_frame_from_slave)) != 0 )
dev_alexander 32:b108ed6096b0 415 {
dev_alexander 32:b108ed6096b0 416 // Convert raw data sent from slave to a 16 bit integer array by calling this
dev_alexander 32:b108ed6096b0 417 convRaw8x8Data2Point25degC(curr_raw_frame_from_slave, conv_frame_from_slave);
dev_alexander 32:b108ed6096b0 418
dev_alexander 32:b108ed6096b0 419 // Next Print off the Converted data
dev_alexander 32:b108ed6096b0 420 pc.printf("\r\nFrame %d data: \r\n", frame_idx);
dev_alexander 32:b108ed6096b0 421 uint8_t idx;
dev_alexander 32:b108ed6096b0 422 float pixel_data;
dev_alexander 32:b108ed6096b0 423 for (int y = 0; y < 8; y++) {
dev_alexander 32:b108ed6096b0 424 for (int x = 0; x < 8; x++) {
dev_alexander 32:b108ed6096b0 425 idx = y*8 + x;
dev_alexander 32:b108ed6096b0 426 pixel_data = ((float)conv_frame_from_slave[idx])/4.0;
dev_alexander 32:b108ed6096b0 427 pc.printf("%.2f \t", pixel_data);
dev_alexander 32:b108ed6096b0 428 }
dev_alexander 32:b108ed6096b0 429 pc.printf("\r\n\r\n");
dev_alexander 23:f74a50977593 430 }
dev_alexander 32:b108ed6096b0 431 pc.printf("\r\n");
dev_alexander 32:b108ed6096b0 432
dev_alexander 32:b108ed6096b0 433 // Increment frame counter
dev_alexander 32:b108ed6096b0 434 frame_idx = frame_idx +1;
dev_alexander 23:f74a50977593 435 }
dev_alexander 32:b108ed6096b0 436 #endif
dev_alexander 32:b108ed6096b0 437 /* Next copy in data received from current data into buffer used for
dev_alexander 32:b108ed6096b0 438 * comparison next time the memcmp above is called. This prevents the
dev_alexander 32:b108ed6096b0 439 * program from converting the same raw data aquired by the grid eye
dev_alexander 32:b108ed6096b0 440 * sensor on the slave device to the floating point array with the
dev_alexander 32:b108ed6096b0 441 * 0.25 degrees celsius precision level when there is not new data.
dev_alexander 32:b108ed6096b0 442 */
dev_alexander 32:b108ed6096b0 443
dev_alexander 28:0ed92c590607 444 memcpy(prev_raw_frame_from_slave, curr_raw_frame_from_slave, sizeof(curr_raw_frame_from_slave));
dev_alexander 24:e8d03912f303 445 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 23:f74a50977593 446 // Aquire raw data about 8x8 frame from the grid eye sensor in this function call
dev_alexander 28:0ed92c590607 447 gridEye.getRaw8x8FrameData(curr_raw_frame_to_master);
dev_alexander 36:09e8b51fc91d 448 // wait_ms( 10 );
dev_alexander 35:bae9b236070b 449 #if defined(RASPBERRY_PI)
dev_alexander 35:bae9b236070b 450 if( memcmp(prev_raw_frame_to_master, curr_raw_frame_to_master, sizeof(curr_raw_frame_to_master)) != 0 )
dev_alexander 35:bae9b236070b 451 {
dev_alexander 35:bae9b236070b 452 char grid_eye_msg[130];
dev_alexander 35:bae9b236070b 453 grid_eye_msg[0]=0x55;
dev_alexander 35:bae9b236070b 454 grid_eye_msg[1]=0xaa;
dev_alexander 35:bae9b236070b 455 for (int idx = 0; idx < 128; idx++)
dev_alexander 35:bae9b236070b 456 grid_eye_msg[idx+2] = curr_raw_frame_to_master[idx];
dev_alexander 35:bae9b236070b 457 //pc.printf(grid_eye_msg);
dev_alexander 35:bae9b236070b 458 for (int idx = 0; idx < 130; idx++)
dev_alexander 35:bae9b236070b 459 pc.putc(grid_eye_msg[idx]);
dev_alexander 35:bae9b236070b 460 pc.printf("\r\n"); // seperate the lines
dev_alexander 35:bae9b236070b 461 }
dev_alexander 35:bae9b236070b 462 #endif
dev_alexander 35:bae9b236070b 463 #if defined(DEBUG_GRID_EYE)
dev_alexander 32:b108ed6096b0 464 if( memcmp(prev_raw_frame_to_master, curr_raw_frame_to_master, sizeof(curr_raw_frame_to_master)) != 0 )
dev_alexander 32:b108ed6096b0 465 {
dev_alexander 32:b108ed6096b0 466 // Convert raw data sent from slave to a 16 bit integer array by calling this
dev_alexander 32:b108ed6096b0 467 convRaw8x8Data2Point25degC(curr_raw_frame_to_master, conv_frame_to_master);
dev_alexander 32:b108ed6096b0 468
dev_alexander 32:b108ed6096b0 469
dev_alexander 32:b108ed6096b0 470 // Next Print off the Converted data
dev_alexander 32:b108ed6096b0 471 pc.printf("\r\nFrame %d data: \r\n", frame_idx);
dev_alexander 32:b108ed6096b0 472 uint8_t idx;
dev_alexander 32:b108ed6096b0 473 float pixel_data;
dev_alexander 32:b108ed6096b0 474 for (int y = 0; y < 8; y++) {
dev_alexander 32:b108ed6096b0 475 for (int x = 0; x < 8; x++) {
dev_alexander 32:b108ed6096b0 476 idx = y*8 + x;
dev_alexander 32:b108ed6096b0 477 pixel_data = ((float)conv_frame_to_master[idx])/4.0;
dev_alexander 32:b108ed6096b0 478 pc.printf("%.2f \t", pixel_data);
dev_alexander 32:b108ed6096b0 479 }
dev_alexander 32:b108ed6096b0 480 pc.printf("\r\n\r\n");
dev_alexander 25:1a031add188a 481 }
dev_alexander 32:b108ed6096b0 482 pc.printf("\r\n");
dev_alexander 32:b108ed6096b0 483
dev_alexander 32:b108ed6096b0 484 // Increment frame counter
dev_alexander 32:b108ed6096b0 485 frame_idx = frame_idx +1;
dev_alexander 25:1a031add188a 486 }
dev_alexander 32:b108ed6096b0 487 #endif
dev_alexander 36:09e8b51fc91d 488 // wait_ms( 10 );
dev_alexander 33:52c898aca207 489 memcpy(prev_raw_frame_to_master, curr_raw_frame_to_master, sizeof(curr_raw_frame_to_master));
dev_alexander 31:f15747cffb20 490 #endif
dev_alexander 31:f15747cffb20 491
dev_alexander 31:f15747cffb20 492 /***************************************************************************
dev_alexander 31:f15747cffb20 493 * GPS Sensor
dev_alexander 31:f15747cffb20 494 **************************************************************************/
dev_alexander 31:f15747cffb20 495 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 31:f15747cffb20 496 //None yet
dev_alexander 31:f15747cffb20 497 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 31:f15747cffb20 498 // The testing hasnt been done yet for the GPS Module. Will do this when more parts come in.
dev_alexander 31:f15747cffb20 499 /*
dev_alexander 31:f15747cffb20 500 if(gps.sample()){
dev_alexander 31:f15747cffb20 501 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);
dev_alexander 31:f15747cffb20 502 pc.printf("%d:%d:%d",gps.hour,gps.minute,gps.seconed);
dev_alexander 31:f15747cffb20 503 }
dev_alexander 31:f15747cffb20 504 */
dev_alexander 31:f15747cffb20 505 #endif
dev_alexander 28:0ed92c590607 506
dev_alexander 23:f74a50977593 507 /***************************************************************************
dev_alexander 28:0ed92c590607 508 * Fill Payload Buffer With Data From Sensors for LoRa Transmition
dev_alexander 28:0ed92c590607 509 **************************************************************************/
dev_alexander 28:0ed92c590607 510 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 28:0ed92c590607 511 memcpy(&BufferTx[tx_idx_signature], PingMsg, size_signature);
dev_alexander 28:0ed92c590607 512 memcpy(&BufferTx[tx_idx_ble], curr_ble_to_slave, size_of_ble);
dev_alexander 28:0ed92c590607 513 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 28:0ed92c590607 514 memcpy(&BufferTx[tx_idx_signature], PongMsg, size_signature);
dev_alexander 28:0ed92c590607 515 memcpy(&BufferTx[tx_idx_grid_eye], curr_raw_frame_to_master, size_of_grid_eye);
dev_alexander 28:0ed92c590607 516 memcpy(&BufferTx[tx_idx_gps], curr_gps_to_master, size_of_gps);
dev_alexander 28:0ed92c590607 517 memcpy(&BufferTx[tx_idx_MAX17055], curr_MAX17055_to_master, size_of_MAX17055);
dev_alexander 28:0ed92c590607 518 memcpy(&BufferTx[tx_idx_MAX77650], curr_MAX77650_to_master, size_of_MAX77650);
dev_alexander 28:0ed92c590607 519 #endif
dev_alexander 28:0ed92c590607 520
dev_alexander 28:0ed92c590607 521
dev_alexander 28:0ed92c590607 522
dev_alexander 32:b108ed6096b0 523
dev_alexander 28:0ed92c590607 524 /***************************************************************************
dev_alexander 28:0ed92c590607 525 * Lora Communications
dev_alexander 28:0ed92c590607 526 **************************************************************************/
dev_alexander 36:09e8b51fc91d 527 // wait_ms( 10 );
dev_alexander 22:abca9d17d13d 528 SX1276PingPong();
dev_alexander 28:0ed92c590607 529
dev_alexander 28:0ed92c590607 530 /***************************************************************************
dev_alexander 28:0ed92c590607 531 * Fill Global Buffers With Data From Received Payload Buffer
dev_alexander 28:0ed92c590607 532 **************************************************************************/
dev_alexander 28:0ed92c590607 533 /* The master and slave devices will have different requirements for offloading payload */
dev_alexander 28:0ed92c590607 534 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 32:b108ed6096b0 535 memcpy(ID_of_slave, &BufferRx[rx_idx_signature], size_signature);
dev_alexander 32:b108ed6096b0 536 memcpy(curr_raw_frame_from_slave, &BufferRx[rx_idx_grid_eye], size_of_grid_eye);
dev_alexander 32:b108ed6096b0 537 memcpy(curr_gps_from_slave, &BufferRx[rx_idx_gps], size_of_gps);
dev_alexander 32:b108ed6096b0 538 memcpy(curr_MAX17055_from_slave, &BufferRx[rx_idx_MAX17055], size_of_MAX17055);
dev_alexander 32:b108ed6096b0 539 memcpy(curr_MAX77650_from_slave, &BufferRx[rx_idx_MAX77650], size_of_MAX77650);
dev_alexander 28:0ed92c590607 540 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 32:b108ed6096b0 541 memcpy(ID_of_master, &BufferRx[rx_idx_signature], size_signature);
dev_alexander 32:b108ed6096b0 542 memcpy(curr_ble_from_master, &BufferRx[rx_idx_ble], size_of_ble);
dev_alexander 28:0ed92c590607 543 #endif
dev_alexander 23:f74a50977593 544
dev_alexander 32:b108ed6096b0 545 /***************************************************************************
dev_alexander 32:b108ed6096b0 546 * Motor Controller controlls go here
dev_alexander 32:b108ed6096b0 547 **************************************************************************/
dev_alexander 32:b108ed6096b0 548 #if defined(TARGET_MAX32630FTHR) // Master Device: BLE-to-LoRa Gateway
dev_alexander 32:b108ed6096b0 549 // No motor controller on master
dev_alexander 32:b108ed6096b0 550 #elif defined(TARGET_MAX32620FTHR) // Slave Device: LoRa Controlled Robot
dev_alexander 32:b108ed6096b0 551 // Check to see if the contents of the previous ble sdata received are the same. If they are different then change motor controllers registers
dev_alexander 32:b108ed6096b0 552 if( memcmp(prev_ble_from_master, curr_ble_from_master, sizeof(curr_ble_from_master)) != 0 )
dev_alexander 32:b108ed6096b0 553 {
dev_alexander 32:b108ed6096b0 554 MotorController(curr_ble_from_master);
dev_alexander 32:b108ed6096b0 555 }
dev_alexander 32:b108ed6096b0 556 #endif
dev_alexander 23:f74a50977593 557
dev_alexander 23:f74a50977593 558
dev_alexander 23:f74a50977593 559
dev_alexander 22:abca9d17d13d 560 }
dev_alexander 22:abca9d17d13d 561
Helmut64 0:c43b6919ae15 562 }