Diff: main.cpp

Revision:

34:f31a6b53d4ea

Parent:

33:52c898aca207

--- 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
+