Sea Turtle Sentinel

Luke Scime April 22, 2014

University of Florida EEL 4665C – Intelligent Machines Design Lab (IMDL)

Instructors: Dr. A. Antonio Arroyo, Dr. Eric M. Schwartz Teaching Assistants: Andy Gray, Josh Weaver, Nick Cox, Daniel Frank

v3.1 2

Table of Contents   Abstract .................................................................................................................................... 3 1  Executive Summary ................................................................................................................. 3 2  Introduction ............................................................................................................................. 4 3  Integrated System .................................................................................................................... 5 4  Mobile Platform ....................................................................................................................... 8 5  Actuation ................................................................................................................................. 8 6  Detailed Description of the Special Sensor System ................................................................ 8 7

7.1  Detection (WebCam) ............................................................................................................... 9 7.2  Classification (Non-Contact Thermometer) .......................................................................... 11 7.3  Reporting (GPS Unit) ............................................................................................................ 14   Additional Sensors ................................................................................................................. 15 8  Behaviors ............................................................................................................................... 15 9  Experimental Layout and Results .......................................................................................... 16 10  Conclusion ............................................................................................................................. 17 11  Documentation....................................................................................................................... 17 12

  Main Arduino Software ................................................................................... 18 Appendix A –  Vision Processing Algorithm .......................................................................... 34 Appendix B –

v3.1 3

Abstract 1

Many species of sea turtles are critically endangered; in an attempt to bolster the sea turtle population many volunteer and government organizations work to actively protect sea turtle nests from predators and other humans. Before sea turtle nests can be protected they must first be identified and located. I believe that an autonomous robotic platform could be used to streamline this routine nest-locating process. This paper details Sea Turtle Sentinel, an autonomous robot that serves as an initial prototype for a future sea turtle nest-locating robot.

Executive Summary 2 Sea Turtle Sentinel is an autonomous robot intended as proof of concept for a future robot that will locate sea turtle nests and record their approximate location for use by its human operators. Sentinel is capable of detecting and approaching artificial sea turtle nests that are marked by a two-colored target. Sentinel can also measure the temperature of the artificial nests to determine whether they are “active” or “inactive” nests. If the nests are active Sentinel then uses an on-board GPS unit to geo-tag their approximate locations. Sentinel is primarily controlled by an Arduino Due but all of the vision processing is handled by and an on-board ODROID-U3 running a version of Ubuntu. Locomotion is provided by four independent motors and everything is powered by a 3000 mAh NiCad battery. Sentinel uses a webcam to detect the artificial nests, a non-contact infrared thermometer to measure their temperature, a GPS unit to geo-tag their locations, and ultrasonic sensors to avoid obstacles. Sentinel identifies the targets using color masking and erosion algorithms native to OpenCV in combination with some original algorithms that virtually eliminate the possibility of false positives. This report details the hardware used on Sea Turtle Sentinel and provides a high-level description of Sentinel’s software and behavior architecture. Descriptions of the tests performed

Figure 1 – A beach sign warning of the penalties associated with intentionally

disturbing a sea turtle nest.

v3.1 4

as well as ideas for future work are provided at the end. Finally, full copies of the software can be found in the appendices.

Introduction 3 As discussed in the Abstract, there is a need to locate new sea turtle nests in a timely fashion so that they can be appropriately protected. The detection and localization of sea turtle nests can be accomplished with a mobile, autonomous robot. This report covers the development and construction of Sea Turtle Sentinel. Sentinel is a fully autonomous robot that will serve as a proof of concept for a future robot capable of detecting sea turtle nests in a real-world environment. To keep the scope of the project manageable, the “sea turtle nests” are artificially marked with a two-colored target (Figure 2) and a heat source. After the colored target is detected by an onboard webcam, Sentinel looks for the heat source using a non-contact infrared thermometer to confirm that it is an “active nest,” i.e. “inactive nests,” identified by a colored marker but no heat source, are also present. The artificial nests are heated using a COTS heating pad (similar to the heaters used in reptile terrariums) powered by a wall outlet. If the detected nest is too far away from Sentinel for the non-contact thermometer to be effective, the robot drives toward the nest before checking for a heat source.

Figure 2 – The target on the left represents an inactive nest (no heat source) while the target on the right represents

an active nest.

While any future robot would drive along a beach using GPS waypoints for navigation, due to the inaccuracies inherent in hobby-level GPS units, Sentinel executes a randomized search pattern instead. At regular intervals along its path Sentinel stops moving and scans its surroundings for sea turtle nests. If it detects a potential sea turtle nest it approaches the nest and checks its temperature. If the nest is warm (active) Sentinel geo-tags its approximate location so that human volunteers can then go out and place protective netting and markers over the nest.

v3.1 5

Integrated System 4 Sea Turtle Sentinel uses an ODROID-U3 [1] processor to handle the vision processing algorithms while an Arduino Due [2] is used to make executive decisions, collect data from the remaining sensors, and control the motors and servo. Three bump sensors and three ultrasonic range finders are used for obstacle detection and avoidance. The webcam and non-contact infrared thermometer are used to detect and classify the active sea turtle nests and the GPS unit is used to geo-tag the location of potential sea turtle nests. The toggle switch allows the operator to disable Sentinel’s motors during debugging and transport. The feedback LEDs allow Sentinel to provide the operator with simple status updates, e.g. an LED turns on if Sentinel has just detected a nest. An LCD screen provides the operator with more detailed feedback about Sentinel’s current mode. Finally, the servo is used to pan the webcam and non-contact thermometer when Sentinel is looking for a nest. See Figure 3 below for a summary of the hardware architecture.

Figure 3 – Architecture diagram for Sea Turtle Sentinel. Solid lines indicate data transfer and communications while

the dashed lines indicate electrical power for the components that do not receive their power directly from the Arduino. Blue blocks are processors, green blocks are sensors, yellow blocks are actuators, purple blocks are

operator interface devices, and red blocks are power distribution components.

v3.1 6

Figure 4 shows a SolidWorks CAD model of Sea Turtle Sentinel. Important components of the robot are denoted as follows, ODROID (A), Arduino (B), drive motors (C), all-terrain wheels (D), ultrasonic range finders (E), bump sensors (F), webcam (G), non-contact infrared thermometer (H), pan servo for the conning tower (I), motor controller (J), and the GPS unit (K).

Figure 4 – The current CAD render of the Sea Turtle Sentinel.

Figures 5 and 6 show Sentinel as of April 9, 2014. All of the electrical and mechanical components are visible in these images except for the LCD screen which was added later.

v3.1 7

Figure 5 – An isometric rear view of Sentinel. This view orientation is comparable to that of the CAD render shown

in Figure 4.

Figure 6 – An isometric front view of Sentinel. Note the front ultrasonic sensor and the non-contact thermometer which are not visible in Figure 5.

v3.1 8

Mobile Platform 5 Sentinel’s mobile platform consists of simple chassis constructed primarily from aircraft-grade wood sheets. Because Sentinel was originally intended to operate in an outdoor, sandy environment, a four-wheel drive system powered by motors [3] from Pololu was selected. The drive motors are mounted to the chassis using COTS mounting brackets [4] from Pololu. Additionally, high-traction wheels [5] allow Sentinel to navigate rough terrain effectively. The camera mast and other sensors mounts are also made from the wood sheets. Sentinel is powered by a Tenergy 3000 mAh NiMH battery [6]. Two “antennae” mounted in the front trigger the bump switches when Sentinel impacts an obstacle.

Actuation 6 To allow Sentinel to scan its surroundings for sea turtle nests without needing to move the drive base, the webcam and non-contact thermometer are mounted atop a mast. The mast is rotated by a Hi-Tec, 90° range servo [7]. The angle of the servo is controlled by a Pulse Width Modulated (PWM) signal generated by the Arduino. The four drive motors are controlled by Pololu PWM speed controllers [8].

Detailed Description of the Special Sensor System 7 Sentinel’s special sensor suite allows it to perform its mission of locating artificial sea turtle nests. While Sentinel’s other sensors allow it to drive around and avoid obstacles, it is the special sensor suite that allows it to collect mission-critical data about its environment. The webcam mounted to the rotating mast assembly allows Sentinel to detect potentially active nests from a distance. The non-contact thermometer enables Sentinel to determine if the sea turtle nests are active or long abandoned (inactive). Finally, the GPS unit gives Sentinel the ability to record the location of potential nests for its human partners. These sensors and actuators must work in unison in order for Sentinel to make intelligent decisions and complete its extremely important mission.

v3.1 9

7.1 Detection (WebCam) One of the most effective, low-cost methods for detecting an object at a distance is machine vision. Sentinel uses a Creative Live USB Cam Chat HD [11] webcam (see Figure 7) to detect artificial sea turtle nests (marked by a two-colored target) from up to 10 feet away. The vision processing is handled by an OROID-U3 running an algorithm developed using the OpenCV library. The ODROID then sends information about the location of the target to an Arduino Due which handles all of Sentinel’s other functions. The webcam sits atop a camera mast that can rotate 45° in either direction by means of a servo.

Figure 7 – The webcam used on Sea Turtle Sentinel

v3.1 10

The vision algorithm (running on the ODROID) first masks the each video frame for the two colors, neon pink and neon green (see Figure 2). The masked frames are then processed by OpenCV’s erosion algorithm, using default settings. The (x,y) centroid coordinates of the masked and processed images are then calculated. In order for Sentinel to recognize a target in its field of view the x coordinates of the pink and green centroids must be closely aligned. This greatly reduces the likelihood of false positives. By calculating the number of pixels between the y coordinates of the pink and green centroids, Sentinel can obtain an accurate and stable distance measurement to the target. Figure 8 shows the vision processing algorithm running live on the ODROID.

Figure 8 – The window on the top right shows the raw camera feed, the two windows on the left show the pink and green masks, and the bottom right window shows position and centroid information about the target.

While searching for sea turtle nests Sentinel stops at regular intervals and pans the camera mast back and forth. If a target is detected the servo angle and the centroids of the two masked images can be used to determine the position and distance of the target with respect to Sentinel. The current vision processing algorithm (C++) is included in Appendix B.

v3.1 11

7.2 Classification (Non-Contact Thermometer) Once Sentinel has identified a possible sea turtle nest it must determine if it is an “active” or an “inactive” nest. This is accomplished using a GE Measurement and Control non-contact infrared thermometer [12] (see Figure 9). The non-contact thermometer communicates with the Arduino using an analog signal.

Figure 9 – The non-contact infrared thermometer used on Sea Turtle Sentinel.

Non-contact infrared thermometers operate by measuring the thermal radiation emitted by an object. Converting that measurement to a temperature estimate depends greatly upon knowing the emissivity of the object being measured. Because every object has a slightly different (or even significantly different) emissivity, this is one of the most common sources of error for non-contact thermometers. It should also be noted that there is no guarantee that all of the thermal radiation reaching the sensor from the direction of the target object actually originated at that object (i.e. some of it may have come from another source and simply been reflected by the target). As a result this is also a common source of error. Most non-contact thermometers, including the one used by Sentinel, are equipped with a filter that blocks out most non-infrared wavelengths in order to reduce noise and prevent saturation of the sensor in certain environments. All non-contact thermometers have a conical field of view; that is, the infrared radiation entering the sensor comes from a larger and larger area the further the target moves away from the sensor. Because the temperature reading is based on essentially the average thermal radiation seen by the sensor, the measured temperature of a target tends to approach the ambient temperature as the target is moved away from the sensor.

v3.1 12

The heat for the artificial, active nests is provided by a COTS heating pad, similar to those used in reptile terrariums, powered by an electrical outlet. Because the non-contact thermometer has an approximately 10° conical field of view, its effective range is approximately 6 inches (see Figure 10). For this reason Sentinel must drive up close to the artificial sea turtle nests, using the distance information proved by the webcam, before it can classify the nests.

Figure 10 – This plot shows the temperature of a heat gun muzzle as measured by the non-contact thermometer. At separation distances of less than approximately 6 inches the thermometer registers the heat gun at a temperature at least twice the ambient temperature. This is considered a temperature difference sufficient to avoid false positives.

The current algorithm for reading the non-contact thermometer is relatively simple and is shown in Appendix A, lines 175 – 176. Essentially it maps the output of the ADC to the function shown below in Figure 11. This function was developed based on the thermometer’s documentation as well as empirical corrections.

v3.1 13

Figure 11 – This figure shows the function that relates the ADC output to the measured temperature in °F. The

function can be expressed as: / . .

It should be noted that the current algorithm does not provide entirely correct temperature readings; it does however distinguish between hot, cold, and room-temperature objects which is all that is required for Sentinel to perform its mission. This issue is most likely being caused by the fact that the sensor is designed to operate using 5 V but is being supplied with only 3.3 V to protect the Arduino Due’s analog inputs from possible damage. Table I shows the measured temperatures of several objects at a distance of 3 inches.

Table I – This table presents the temperature (as measured by the non-contact thermometer) of several objects. All of the objects (except of course “Ambient”) were placed approximately 3 inches away from the thermometer.

Target Measured Temperature (°F) Ambient 48.4 Human 61.9 Ice Pack 16.5

Soldering Iron 194 Heat Gun 194

Initially, Sentinel was going to use a Melexis non-contact thermometer [13] that utilizes an I2C communications protocol. Unfortunately, due to a widely-known but currently unresolved issue with the Arduino Due’s two-wire (I2C) library this sensor could not be integrated and was replaced by the GE sensor on March 1, 2014.

v3.1 14

7.3 Reporting (GPS Unit) If Sentinel determines that an artificial sea turtle nest is indeed “active” it must then record the location of the nest so that its human partners can protect the vulnerable baby sea turtles. Sentinel accomplishes this using a LOCOSYS GPS unit [10] (see Figure 12) which communicates with the Arduino over a serial connection.

Figure 12 – The GPS unit used on the Sea Turtle Sentinel.

The software handling the GPS unit communicates using a 57600 baud rate in part because the unit provides a large amount of information during each read cycle. In addition to mission-critical information such as the current longitude, latitude, and UTC time from the satellites (used to confirm a good connection) the unit also outputs information such as the number of satellites being used, etc. All of the data are stored in a buffer during each read cycle and then the mission-critical information is extracted using several string-parsing operations. Because the GPS unit sends garbled data every ten to twenty serial read cycles a robust, error-tolerant algorithm is necessary. This robustness is currently accomplished by checking for specific tags and identifiers in the raw data and confirming the length of data following those tags and identifiers before allowing the new data to overwrite Sentinel’s currently saved location. The current software is shown in Appendix A, lines 709 – 769. One of the major limitations of this GPS unit has turned out to be how long it takes to acquire a solid location lock. It generally takes five or more minutes after booting for the GPS to report that is has a stable fix from a sufficient number of satellites. While this is not expected to be an issue during normal operations it is a concern for certain time-limited demonstrations. Additionally, the GPS has particular difficulty acquiring a satellite lock when inside of a building.

v3.1 15

Additional Sensors 8 Three ultrasonic sensors [9] from Maxbotix allow Sentinel to detect potential obstacles while they are still far enough away for evasive maneuvers to be effective. The ultrasonic sensors communicate with the Arduino via an analog signal. Ultrasonic sensors were chosen over cheaper infrared range finders due to concerns about sunlight interfering with their operation in an outdoor environment. Three bump sensors function as a fail-safe obstacle detection system should the ultrasonic sensors fail to register an obstacle from a distance. The bump sensors communicate with the Arduino via a digital signal tied to an interrupt-enabled digital pin.

Behaviors 9 Sentinel has seven distinct behaviors which are selected by a master arbitrator (see Figure 13). In order of decreasing priority they are Calibrate, Impact Response, Analyze Nest, Approach Nest, Scan for Nests, Avoid Obstacles, and Default.

1. Calibrate – This is the mode that Sentinel boots into. It allows the operator wait for the GPS to connect to a sufficient number of satellites, calibrate the forward ultrasonic sensor, and set the stopping distance to the target (based on the camera data).

2. Default – Is the mode that Sentinel enters after Calibrate. As its name suggests Sentinel defaults to this mode unless another, higher-priority behavior is selected by the arbitrator. Sentinel drives straight forward when executing the default behavior.

3. Impact Response – If at any point Sentinel triggers one of its three bump sensors it will stop moving, reverse, and then execute a turn before returning to its previous behavior.

4. Avoid Obstacles – If Sentinel is driving in its Default mode and it detects an obstacle in its path (using the forward ultrasonic sensor) it will stop, and then execute a turn until it detects that its path is clear in front of it.

5. Scan for Nests – After every ten seconds of driving forward or avoiding obstacles, Sentinel will stop and scan for nests using its camera. The camera mast will pan right, left, and end facing forward. If a nest was detected during the scanning process, Sentinel will then enter its Approach Nest mode.

6. Approach Nest – When entering this mode Sentinel will first turn in the direction of a detected nest until it is roughly pointed toward the nest. Sentinel will then drive toward the nest, constantly adjusting wheel speed to keep it centered in its field of view, until it is approximately six inches away from the nest.

7. Analyze Nest – Once Sentinel has reached a nest it will rotate the camera mast to ensure that the camera and non-contact infrared thermometer are pointed directly at the center of the target. Sentinel will then check the temperature of the nest. If the nest is warm Sentinel will display the current GPS coordinates on the LCD screen, if the nest is close to the ambient temperature Sentinel will indicate that it is an inactive nest on the LCD screen. After displaying the appropriate message on the LCD screen, Sentinel reenters its Default mode.

v3.1 16

Figure 13 – A high level software flowchart. The purple blocks in the top right hand corner run on the ODROID

while all of the other segments of code run on the Arduino. The highest priority behavior is Calibrate and the lowest priority is Default.

Experimental Layout and Results 10 A number of Sea Turtle Sentinel’s systems went through multiple iterations; this is particularly true of Sentinel’s special sensor system. Originally Sentinel was going to use a non-contact thermometer that communicated with the Arduino over an I2C connection, unfortunately the software bugs in the Arduino Due’s I2C (two-wire) communication library made this option untenable. As a result, Sentinel now uses a non-contact thermometer that communicates over a simple analog connection. The GPS unit also required a good deal of trial-and-error to get working. Because the GPS transmits at a high baud rate and is susceptible to regularly sending garbled data a lot of work was required to create robust serial communication and parsing software. Integrating the camera and the ODROID with Sentinel was by far the task that required the most testing and iteration. Just installing OpenCV on the ODROID took over a week as OpenCV is not compatible with most Ubuntu images for the ODROID. Sentinel is currently running Ubuntu

v3.1 17

13.04 but Ubuntu 13.10, Ubuntu 12.11, and a version of Linaro were also tried. Once OpenCV was successfully installed the testing of the actual machine vision algorithm proceeded apace. The first step in testing the machine vision algorithm was tuning the RGB thresholds for both the pink and the green mask. Then the acceptance thresholds for how close the pink and green centroids must vertically align to be considered a target were tuned. Finally, the optimal distance for the thermometer to read the temperature of the nest was experimentally determined and then correlated with the distance measurements calculated by the machine vision algorithm.

Conclusion 11 As the semester comes to a close, I can say with confidence that Sentinel was largely successful at meeting the goals that I originally set in January; Sentinel reliable locates targets and classifies them as either inactive or active while simultaneously avoiding obstacles. There is however a large amount of room for improvement, particularly in the area of transitioning Sentinel into an outdoor-capable mobile platform as originally intended. With more time to test I believe that it is possible to have Sentinel locate targets in an outdoor environment. If possible, I would also like to implement navigation between GPS waypoints. This will require adding a magnetic compass as well as a substantial amount of additional software development. All major deliverables and weekly blog posts for this course can be found on Sea Turtle Sentinel’s website here: https://sites.google.com/site/seaturtlesentinel/home.

Documentation 12 [1] ODROID-U3 – http://www.hardkernel.com/main/main.php [2] Arduino Due – https://www.sparkfun.com/products/11589 [3] Drive motors – http://www.pololu.com/product/2275/specs [4] Motor mounting brackets – http://www.pololu.com/product/1569 [5] All-terrain wheels – http://www.pololu.com/product/1555/specs [6] Battery – http://www.amazon.com/Tenergy-3000mAh-Battery-Tamiya-Connectors/dp/B0037U35SO/ref=sr_1_7?ie=UTF8&qid=1391050907&sr=8-7&keywords=nicad+batteries+rc+car [7] Motor controllers – http://www.pololu.com/product/1451 [8] 90° rotation servo – http://www.andymark.com/product-p/am-2543.htm [9] Ultrasonic range finders – https://www.sparkfun.com/products/8503 [10] GPS unit – https://www.sparkfun.com/products/8975 [11] Webcam – http://www.amazon.com/Creative-Live-5-7MP-Webcam-Black/dp/B004431UBM [12] GE non-contact infrared thermometer – http://www.mouser.com/ProductDetail/GE-MC-NovaSensor/ZTP-115M/?qs=gBjjYPXQNsQsQj8R%252bB9kkA== [13] Melexis non-contact infrared thermometer – https://www.sparkfun.com/products/9570

v3.1 18

Main Arduino Software Appendix A – /* 1 Program: Arduino Main 2 Description: The master program running on Sentinel's Arduino Due 3 Author: Luke Scime 4 Last Modified: 4/21/2014 5 Comments: 6 */ 7  8 //Additional libraries‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 9  10 #include <Servo.h> 11 #include <LiquidCrystal.h> 12 LiquidCrystal lcd(28, 32, 34, 36, 38, 40); //Set up the LCD screen 13  14 //Global variable declarations‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 15  16 //Digital pins 17 //Pin 0 is reserved for Serial #0 Rx 18 //Pin 1 is reserved for Serial #0 Tx 19 int Drive_RB = 2; //Right‐Back drive motor PWM 20 int Drive_RF = 3; //Right‐Front drive motor PWM 21 int Drive_LB = 4; //Left‐Back drive motor PWM 22 int Drive_LF = 5; //Left‐Front drive motor PWM 23 Servo Mast; //Pin 6, the camera mast servo PWM 24 int safetyswitch = 12; //Safety interlock switch 25 int heartbeat = 13; //Heartbeat LED 26 //Pin 14 is reserved for Serial #3 Tx 27 //Pin 15 is reserved for Serial #3 Rx 28 int led_mark = 22; //LED used to indicate that a nest is close enough to read the temperature 29 int led_active = 24; //LED used to indicate that a nest is active 30 int led_nest = 26; //LED used to indicate that a nest has been detected 31 //Pin 28 is reserved for LCD ‐ RS 32 //Pin 32 is reserved for LCD ‐ E 33 //Pin 34 is reserver for LCD ‐ D4 34 //Pin 36 is reserved for LCD ‐ D5 35 //Pin 38 is reserved for LCD ‐ D6 36 //Pin 40 is reserved for LCD ‐ D7 37 int DIR2_LF = 44; //Left‐Front drive motor direction 2 38 int DIR1_LF = 45; //Left‐Front drive motor direction 1 39 int DIR1_LB = 46; //Left‐Back drive motor direction 1 40 int DIR2_LB = 47; //Left‐Back drive motor direction 2 41 int DIR1_RF = 50; //Right‐Front drive motor direction 1 42 int DIR2_RF = 51; //Right‐Front drive motor direction 2 43 int DIR1_RB = 52; //Right‐Back drive motor direction 1 44 int DIR2_RB = 53; //Right‐Back drive motor direction 2 45  46 //Analog pins 47 int Sonar_F = A0; //Front sonar sensor 48 int Sonar_L = A1; //Left sonar sensor 49 int Sonar_R = A2; //Right sonar sensor 50

v3.1 19

int Therm = A3; //Non‐contact infrared temperature sensor 51  52 //System maintainance variables 53 int i = 0; //Current loop cycle 54 int safety = 0; //Safety interlock switch status 55 bool beat = true; //Heartbeat status 56 String masterMode = "Calibrate"; //Label of current master mode 57 String masterModeOld = "N/A"; //Label of previous master mode 58 int tMark = 0; //Marked time at beginning of an operation 59 int tPause = 0; //Allows the timer to be paused 60  61 //Sonar sensor‐related variables 62 int filterSize = 500; 63 int Range_F_filter[500] = {0}; //Filter buffer for front sonar sensor 64 int Range_L_filter[500] = {0}; //Filter buffer for left sonar sensor 65 int Range_R_filter[500] = {0}; //Filter buffer for right sonar sensor 66 int Range_F = 0; //Range reading from front sonar sensor 67 int Range_L = 0; //Range reading from left sonar sensor 68 int Range_R = 0; //Range reading from right sonar sensor 69 int stopdist = 40; //Minimum allowed distance from Sentinel to an obstacle 70  71 //Bump switch‐related variables 72 int Bump_L = 31; //Left bump sensor 73 int Bump_C = 33; //Center bump sensor 74 int Bump_R = 35; //Right bump sensor 75 int impact = 0; //Goes true if any of the bump switches are triggered 76  77 //Drive state‐related variables 78 int man_sub = 0; //Sub maneuver state 79 int lor = 3; //Contains the random value for either a left or right turn 80 int avoidLock = 0; //Lock into avoidance maneuver or higher priority mode 81 double drive[] = {0, 0}; //Left and right motor power 82  83 //Thermometer‐related variables 84 float temperature = 0; //Temperature reading from temperature sensor 85 int thermMaxRange = 200; //Maximum functional range for the thermometer 86 float thresholdTemp = 56; //Temperature threshold for an active nest 87  88 //GPS‐related variables 89 String UTC_sr = "000000.000"; //Time stamp (string) 90 double lat = 0; //Latitude, decimal degrees 91 double lon = 0; //Longitude, decimal degrees 92 int gpsSpeed = 1; //How often the GPS is queried, in loop cycles 93  94 //Camera mast and scanning‐related variables 95 int nextScan = 10000; //The time at which the next scan will occur 96 int scanWait = 10000; //Waiting time before next scan 97 int scan_sub = 0; //Scanning submode 98 int nestPosRough = 0; //The rough position of the nest as found during the scanning process 99 int nestDist = 0; //Current distance to the nest 100 bool nestFound = false; //True if a nest has been located during a scan 101 bool nestSeen = false; //True if a nest is seen at any time 102 bool nestClose = false; //True if the nest is close 103

v3.1 20

int nestPos = 1000; //The (x,y) nest position in the camera's field of view 104 int app_sub = 0; //Approach submode 105 bool readyToTest = false; //True if the target has ben approached and is close 106 int ana_sub = 0; //Analyze submode 107 int currentAngle = 80; //Reports the current angle of the camera mast 108 int centerAngle = 80; //Angle that returns the servo to pointed straight ahead 109  110 //User interface‐related variables 111 int calib = 1; //Sentinel boots to 1 to trigger calibration 112 int calib_sub = 0; //Calibrate submode 113  114 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 115  116 void setup()  117 { 118    119   //Set up speed controller direction pins  120   pinMode(DIR1_RB, OUTPUT); 121   pinMode(DIR2_RB, OUTPUT); 122   pinMode(DIR1_RF, OUTPUT); 123   pinMode(DIR2_RF, OUTPUT); 124   pinMode(DIR1_LB, OUTPUT); 125   pinMode(DIR2_LB, OUTPUT); 126   pinMode(DIR1_LF, OUTPUT); 127   pinMode(DIR2_LF, OUTPUT); 128    129   Mast.attach(6); //Set up the camera mast servo 130    131   pinMode(safetyswitch, INPUT); //Set up the safety switch 132   pinMode(heartbeat, OUTPUT); //Set up the heartbeat LED 133    134   //Set up the status LEDs 135   pinMode(led_mark, OUTPUT); 136   pinMode(led_active, OUTPUT); 137   pinMode(led_nest, OUTPUT); 138    139   //Attach the external interruprs for the bump switches 140   attachInterrupt(Bump_L,INT_Impact,FALLING); 141   attachInterrupt(Bump_C,INT_Impact,FALLING); 142   attachInterrupt(Bump_R,INT_Impact,FALLING); 143    144   //Set up the LCD screen 145   lcd.begin(16, 2);  146   lcd.setCursor(0, 0); 147   lcd.print("Sea Turtle"); 148   lcd.setCursor(0, 1); 149   lcd.print("Sentinel ‐ LUKE"); 150      151   Serial.begin(9600); //Setup serial communication back to computer for debug purposes and communication to 152 ODROID 153   Serial3.begin(57600); //Setup serial communication with GPS 154  155 } 156

v3.1 21

 157 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 158  159 void loop()  160 { 161  162   //Manage Sentinel's heartbeat 163   digitalWrite(heartbeat, beat); 164   if((i%1001) == 0) beat = !beat; 165      166   //Read most recent sensor values 167   safety = digitalRead(safetyswitch); //Read the safety switch 168   Range_F_filter[i%filterSize] = analogRead(Sonar_F); //Apply Cycling average filter to forward sonar sensor 169   Range_F = Avg(Range_F_filter, filterSize); 170   Range_L_filter[i%filterSize] = analogRead(Sonar_L); //Apply Cycling average filter to left sonar sensor 171   Range_L = Avg(Range_L_filter, filterSize); 172   Range_R_filter[i%filterSize] = analogRead(Sonar_R); //Apply Cycling average filter to right sonar sensor 173   Range_R = Avg(Range_R_filter, filterSize); 174   temperature = analogRead(Therm); //Read the thermometer 175   temperature = ‐0.0002*(temperature*temperature) + 0.4628*temperature ‐ 102.0127; //Convert from ADC to 176 Celsius 177   temperature = (9/5)*temperature + 32; //Convert from Celsius to Fahrenheit 178   if((i%gpsSpeed) == 0) GPS(); //Only read the GPS unit only on particular loop cycles 179   if((i%200) == 0) Camera(); //Only read the ODROID's camera data only on particular loop cycles 180    181   //Mangage status LEDs 182   if(temperature>thresholdTemp) digitalWrite(led_active,HIGH); //Active nest LED 183   else digitalWrite(led_active,LOW); 184   if(nestSeen) digitalWrite(led_nest,HIGH); //Nest detected LED 185   else digitalWrite(led_nest,LOW); 186   if (nestClose) digitalWrite(led_mark,HIGH); //Nest within thermometer range LED 187   else digitalWrite(led_mark,LOW); 188    189   //Master arbitrator 190   if(calib) 191   { 192     //Boot to calibration mode 193     Calibrate();  194   } 195   else if(impact) 196   { 197     //Immediately jump to a maneuver if the bump switches are hit 198     masterMode = "Impact"; 199     nextScan = millis() + scanWait; //Pause the scan timer 200     ObstacleHit(); 201   } 202   else if(readyToTest) //Greater than because the nestDist is inversely proportional to distance 203   { 204     //Analyze the nest and geo‐tag if active 205     masterMode = "Analyze"; 206     nextScan = millis() + scanWait; //Pause the scan timer 207     Analyze(); 208   } 209

v3.1 22

  else if(nestFound) 210   { 211     //Approach a nest 212     masterMode = "Approach"; 213     nextScan = millis() + scanWait; //Pause the scan timer 214     Approach(); 215   } 216   else if((millis() > nextScan) && (avoidLock == 0)) 217   { 218     //Scan the surroundings for nests  219     masterMode = "Scan"; 220     Scan(); 221   }  222   else if((Range_F < stopdist) || (avoidLock == 1)) 223   { 224     //Avoid an object seen by the front sonar 225     masterMode = "Avoid"; 226     ObstacleSeen(); 227   } 228   else 229   { 230     //Default to drive forward 231     masterMode = "Default"; 232     Default(); 233   } 234    235   DriveCmds(drive[0], drive[1]); //Manage drive commands 236    237   //Display debug data to the COM terminal for debugging 238   if(((i%2000) == 0) && 0) 239   { 240     Serial.println("‐‐‐‐‐‐‐‐‐‐"); 241     Serial.println(masterMode); //Display the current master mode 242     Serial.print("Forward Range: "); //Display the current front range in the COM window 243     Serial.println(Range_F);  244     Serial.print("Left Range: "); //Display the current left range in the COM window 245     Serial.println(Range_L); 246     Serial.print("Right Range: "); //Display the current right range in the COM window 247     Serial.println(Range_R); 248     Serial.print("Drive Motors: "); //Display Sentinel's drivestate in the COM window 249     Serial.print(drive[0]); 250     Serial.print("   "); 251     Serial.println(drive[1]); 252     Serial.print("Temperature (F): "); //Display Sentinel's drivestate in the COM window 253     Serial.println(temperature); 254     Serial.print("UTC (hhmmss.sss): "); //Display the timestamp from the GPS satellites 255     Serial.println(UTC_sr); 256     Serial.print("Latitude (deg): "); 257     Serial.println(lat); 258     Serial.print("Longitude (deg): "); 259     Serial.println(lon); 260     Serial.println("‐‐‐‐‐‐‐‐‐‐"); 261    } 262

v3.1 23

    263   //Update LCD screen 264   if(masterMode != masterModeOld) 265   { 266     lcd.setCursor(0, 0); 267     lcd.print("                "); 268     lcd.setCursor(0, 0); 269     lcd.print(masterMode); 270   } 271     272   //System maintenance  273   i++; //Increment the loop counter 274   masterModeOld = masterMode; //Reset the label of the previous master mode 275  276 } 277  278 //End Main‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 279  280 //Functions‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 281  282 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Waits without slowing down the loop cycle‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 283  284 int Wait(int action, int counter, int tWait) 285 { 286   switch(action) 287   { 288     case 0: 289       //Reset the wait timer 290       tMark = millis(); 291       tPause = 0; 292       counter++; //So this command must always be executed with this in mind 293       break; 294     case 1: 295       //Pause the wait timer 296       tPause = millis(); 297       break; 298     case 2: 299       //Check the wait timer 300       if(millis() >= (tMark+tPause+tWait)) counter++; 301       break; 302   } 303   return counter; 304 } 305  306 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Finds the average of an array‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 307  308 int Avg (int array[], int elm) 309 { 310   int average = 0; 311   int sum = 0; 312    313   for(int k = 0; k < elm; k++) 314   { 315

v3.1 24

    sum += array[k]; 316   } 317    318   average = sum/elm; 319  320   return average; 321 } 322  323 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Decides which direction to turn‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 324  325 int LOR(int L, int R, int limit) 326 { 327   int dir = 0; 328   if((R <= limit) && (L > limit)) dir = 1; //Turn left 329   else if((L <= limit) && (R > limit)) dir = 0; //Turn right 330   else dir = random(0,2); //Random turn direction 331   return dir; 332 } 333  334 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Default robot state‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 335  336 void Default() 337 { 338   drive = {0.25,0.25}; //Drive forward slowly 339   ScanCmds(centerAngle); //Keep the camera mast pointed forward 340 } 341  342 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Avoid obstacle seen by front sonar‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 343  344 void ObstacleSeen() 345 { 346   avoidLock = 1; //Lock the master arbitrator into this mode or a higher priority mode 347   switch(man_sub) 348   { 349     case 0: 350     //Set up for manuever 351     drive = {0,0}; 352     lor = LOR(Range_L, Range_R, stopdist); //Decide on turn direction 353     man_sub = Wait(0, man_sub, 0); 354     break; 355   case 1: 356     //Wait 357     man_sub = Wait(2, man_sub, 1000); 358     break; 359   case 2: 360     //Execute turn until forward range sensor is clear 361     if(lor == 1) drive = {‐0.5,0.5}; 362     else drive = {0.5,‐0.5}; 363     if(Range_F >= stopdist) man_sub = Wait(0, man_sub, 0); 364     break; 365   case 3: 366     //Keep executing the turn for a little bit longer 367     man_sub = Wait(2, man_sub, 500); 368

v3.1 25

    break; 369   case 4: 370     //Stop executing the turn 371     drive = {0,0}; 372     man_sub = 0; 373     avoidLock = 0; 374     break; 375   } 376 } 377    378 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Avoid obstacle after impact‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐  379    380 void ObstacleHit() 381 { 382   avoidLock = 1; //Lock the master arbitrator into this mode or a higher priority mode 383   switch(man_sub) 384   { 385   case 0: 386     //Set up for manuever 387     drive = {0,0}; 388     man_sub = Wait(0, man_sub, 0); 389     break; 390   case 1: 391     //Wait 392     man_sub = Wait(2, man_sub, 1000); 393     break; 394   case 2: 395     //Reset the timer 396     man_sub = Wait(0, man_sub, 0); 397     break; 398   case 3: 399     //Back up for a little bit 400     drive = {‐0.25,‐0.25}; 401     man_sub = Wait(2, man_sub, 1000); 402     break; 403   case 4: 404     //Decide on which direction to turn 405     lor = LOR(Range_L, Range_R, stopdist); //Decide on turn direction 406     man_sub = Wait(0, man_sub, 0); 407     break; 408   case 5: 409     //Execute turn 410     if(lor == 1) drive = {‐0.5,0.5}; 411     else drive = {0.5,‐0.5}; 412     man_sub = Wait(2, man_sub, 1000); 413     break; 414   case 6: 415     //Reset and leave maneuver 416     drive = {0,0}; 417     man_sub = 0; 418     impact = 0; 419     break; 420   } 421

v3.1 26

} 422  423 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Manages drive commands‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 424  425 void DriveCmds (float left, float right) 426 { 427   if(safety == 1) 428   { 429     //Command motors to stop if safety is engaged 430     right = 0; 431     left = 0; 432   } 433    434   //Set right‐side direction 435   if (right > 0) 436   { 437     digitalWrite(DIR1_RB, HIGH); 438     digitalWrite(DIR2_RB, LOW); 439     digitalWrite(DIR1_RF, HIGH); 440     digitalWrite(DIR2_RF, LOW); 441   } 442   else 443   { 444     digitalWrite(DIR1_RB, LOW); 445     digitalWrite(DIR2_RB, HIGH); 446     digitalWrite(DIR1_RF, LOW); 447     digitalWrite(DIR2_RF, HIGH); 448   } 449    450   //Set left‐side direction 451   if (left > 0) 452   { 453     digitalWrite(DIR1_LB, HIGH); 454     digitalWrite(DIR2_LB, LOW); 455     digitalWrite(DIR1_LF, HIGH); 456     digitalWrite(DIR2_LF, LOW); 457   } 458   else 459   { 460     digitalWrite(DIR1_LB, LOW); 461     digitalWrite(DIR2_LB, HIGH); 462     digitalWrite(DIR1_LF, LOW); 463     digitalWrite(DIR2_LF, HIGH); 464   } 465      466   //Convert speeds from percentages: ‐1 to 1 ‐‐> 0 to 255 467   right = abs(right*255); 468   left = abs(left*255); 469      470   //Set the PWM outputs 471   analogWrite(Drive_RB, right); 472   analogWrite(Drive_RF, right); 473   analogWrite(Drive_LB, left); 474

v3.1 27

  analogWrite(Drive_LF, left);   475    476 } 477  478 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Handle the scanning procedure‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 479  480 void Scan() 481 { 482   drive = {0, 0}; //Stop driving during the scanning process 483   switch(scan_sub) 484   { 485     case 0: 486       scan_sub = Wait(0, scan_sub, 0); //Reset timer 487       nestPosRough = 0; //Reset the rough position of the target 488       break; 489     case 1: 490       //Look right 491       ScanCmds(170); 492       scan_sub = Wait(2, scan_sub, 3000); 493       break; 494     case 2: 495       if(nestSeen) nestPosRough = 2; //Set the nest position as to the right 496       scan_sub = Wait(0, scan_sub, 0); //Reset timer 497       break; 498     case 3: 499       //Look left 500       ScanCmds(10); 501       scan_sub = Wait(2, scan_sub, 3000); 502       break; 503     case 4: 504       if(nestSeen) nestPosRough = 3; //Set the nest position as to the left 505       scan_sub = Wait(0, scan_sub, 0); //Reset timer 506       break; 507     case 5: 508       //Look forward 509       ScanCmds(centerAngle); 510       scan_sub = Wait(2, scan_sub, 3000); 511       break;   512     case 6: 513       if(nestSeen) nestPosRough = 1; //Set the nest position as to the front 514       scan_sub = 0; 515       nextScan = millis() + scanWait; 516       if(nestPosRough != 0) nestFound = true; 517       break; 518   } 519 } 520  521  522 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Manages servo commands‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 523  524 void ScanCmds(int angle) 525 { 526   if(safety == 0) 527

v3.1 28

  { 528     //Only change the mast angle if the safety is disengaged 529     Mast.write(angle); 530     currentAngle = angle; //Update the current camera mast angle indicator 531   } 532 } 533  534 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Perform calibration steps‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 535  536 void Calibrate() 537 { 538   switch(calib_sub) 539   { 540     case 0: 541       masterMode = "Calibrate‐GPS"; 542       gpsSpeed = 5001; //Speed up the GPS communication 543       calib_sub++; 544       break; 545     case 1: //Allow GPS to connect 546       if(impact) 547       { 548         gpsSpeed = 5001; //Slow the GPS communication back down 549         calib_sub = Wait(0, calib_sub, 0); //Reset the timer 550       } 551       break; 552     case 2: 553       masterMode = "Calibrate‐Sonar"; 554       impact = false; 555       calib_sub = Wait(2, calib_sub, 500); 556       break; 557     case 3: //Calibrate the sonar distance 558       if(impact) 559       { 560         stopdist = Range_F; 561         calib_sub = Wait(0, calib_sub, 0); //Reset the timer 562       }  563       break; 564     case 4: 565       masterMode = "Calibrate‐Camera"; 566       impact = false; 567       calib_sub = calib_sub = Wait(2, calib_sub, 500); 568       break; 569     case 5: //Calibrate the camera distance 570       if(impact) 571       {  572         thermMaxRange = nestDist; 573         calib_sub = Wait(0, calib_sub, 0); //Reset the timer 574       }  575       break; 576     case 6: 577       impact = false; //Reset the impact indicator  578       calib = false; //Jump out of the calibration mode 579       calib_sub = 0; 580

v3.1 29

      break; 581   } 582 } 583  584 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Approach a nest‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 585  586 void Approach() 587 { 588   int lScale = 1; //Bang‐bang adjustments for the left side 589   int rScale = 1; //Bang‐band adjustments for the right side 590   switch(app_sub) 591   { 592     case 0: 593       app_sub = Wait(0, app_sub, 0); //Reset timer 594       break; 595     case 1: 596       if(nestPosRough == 1) drive = {0,0}; //If the target is directly in front 597       else if(nestPosRough == 2) drive = {0.30,‐0.30}; //If the target is to the right 598       else if(nestPosRough == 3) drive = {‐0.30,0.30}; //If the target is to the left 599       app_sub ++; 600       break; 601     case 2: 602       if(abs(nestPos)<75) //Stop turning when (roughly) lined up with the target 603       {  604         drive = {0,0}; //Stop driving 605         app_sub++; 606       } 607       if(nestPosRough == 1) app_sub++; 608       app_sub = Wait(2, app_sub, 10000); //Trigger time‐out override 609       break; 610     case 3: 611       drive = {0,0}; //Stop driving  612       app_sub = Wait(0, app_sub, 0); //Reset timer 613       break; 614     case 4: 615       if(nestPos<‐25) //If the target is slightly to the left 616       { 617         lScale = 0.9; 618         rScale = 1; 619       } 620       else if(nestPos>25) //If the target is slightly to the right 621       { 622         lScale = 1; 623         rScale = 0.9; 624       } 625       else //If the target is directly ahead 626       { 627         lScale = 1; 628         rScale = 1; 629       } 630       if(!nestClose) drive = {lScale*0.25,rScale*0.25}; //Drive forward until the nest is close 631       else  632       { 633

v3.1 30

        drive = {0,0}; //Stop driving when close to the nest 634         app_sub++; 635       } 636       app_sub = Wait(2, app_sub, 10000); //Time‐out override 637       break; 638     case 5: 639       //Reset after reaching the target, trigger scanning 640       app_sub = 0; 641       nestFound = false; 642       readyToTest = true; 643       drive = {0,0}; 644     break; 645   } 646 } 647  648 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Analyze a nest‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 649  650 void Analyze() 651 { 652   switch(ana_sub) 653   { 654     case 0: 655       ana_sub = Wait(0, ana_sub, 0); //Reset timer 656       break; 657     case 1: 658       if((i%500)==0) 659       { 660         if(nestPos>10) ScanCmds(currentAngle+1); //Turn camera mast to the right 661         else if(nestPos<‐10) ScanCmds(currentAngle‐1); //Turn camera mast to the left 662         else ana_sub++; //Proceed if within dead band 663       } 664       ana_sub = Wait(2, ana_sub, 10000); //Time‐out override 665       break; 666     case 2: 667       ana_sub = Wait(0, ana_sub, 0); //Reset timer 668       break; 669    case 3: 670       ana_sub = Wait(2, ana_sub, 2000); //Wait for the temperature reading to stabilize 671       break; 672    case 4: 673      //Update the LCD screen 674      if(temperature>thresholdTemp) 675      { 676        //Active nest 677        lcd.setCursor(0, 0); 678        lcd.print("                "); 679        lcd.setCursor(0, 0); 680        lcd.print("Nest "); 681        lcd.print(lat); 682        lcd.print(","); 683        lcd.print(lon); 684      } 685      else 686

v3.1 31

     { 687        //Inactive nest 688        lcd.setCursor(0, 0); 689        lcd.print("                "); 690        lcd.setCursor(0, 0); 691        lcd.print("Inactive Nest"); 692      } 693      ana_sub = Wait(0, ana_sub, 0); //Reset timer 694      break; 695    case 5: 696      ana_sub = Wait(2, ana_sub, 5000); //Wait to display the message 697      break; 698    case 6: 699      //Reset after analyzing the nest 700      ScanCmds(centerAngle); 701      readyToTest = false; 702      ana_sub = 0; 703      break; 704   } 705   706 } 707  708 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Handle GPS communication‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 709  710 void GPS() 711 { 712   //Variable Declarations 713   char InBytes[130]; //Serial read buffer (character array) 714   String InString = "blank"; //Serial read buffer (string) 715   int TagIndex = 0; //Location of the "$GPGGA" tag 716   String latd_sr = "00"; //Latitude, degrees portion (string) 717   char latd_ch[3]; //Latitude, degrees portion (character array) 718   String latm_sr = "00.0000"; //Latitude, minutes portion (string) 719   char latm_ch[8]; //Latitude, minutes portion (character array) 720   String lat_dir = "X"; //Latitude compass rose direction (N or S) 721   double latd = 0; //Latitude, degrees portion (double) 722   double latm = 0; //Latitude, minutes portion (double) 723   String lond_sr = "000"; //Longitude, degrees portion (string) 724   char lond_ch[4]; //Longitude, degrees portion (character array) 725   String lonm_sr = "00.0000"; //Longitude, minutes portion (string) 726   char lonm_ch[8]; //Longitude, minutes portion (character array) 727   String lon_dir = "X"; //Longitude compass rose direction (W or E) 728   double lond = 0; //Longitude, degrees portion (double) 729   double lonm = 0; //Longitude, minutes portion (double) 730    731   if(Serial3.available()) 732     { 733        734       Serial3.readBytes(InBytes,130); //Read serial communications buffer from the GPS 735        736       InString = InBytes; //Transfer the buffer to a string 737       TagIndex = InString.indexOf("GPGGA");  //Search for "$GPGGA" tag 738        739

v3.1 32

      if(TagIndex != ‐1) //Only update the GPS coordinates if the read was successful 740       {   741         UTC_sr = InString.substring(TagIndex+6,TagIndex+16); //Parse the timestamp 742         latd_sr = InString.substring(TagIndex+17,TagIndex+19); //Parse the degree portion of the latitude 743         latm_sr = InString.substring(TagIndex+19,TagIndex+26); //Parse the minutes portion of the latitude 744         lat_dir = InString.substring(TagIndex+27,TagIndex+28); //Parse the latitude compass‐rose direction 745         lond_sr = InString.substring(TagIndex+29,TagIndex+32); //Parse the degree portion of the longitude 746         lonm_sr = InString.substring(TagIndex+32,TagIndex+39); //Parse the minutes portion of the longitude 747         lon_dir = InString.substring(TagIndex+40,TagIndex+41); //Parse the longitude compass‐rose direction 748          749         latd_sr.toCharArray(latd_ch,3); //Convert the string to a character array 750         latd = atof(latd_ch); //Convert the character array to a float 751         latm_sr.toCharArray(latm_ch,8); //Convert the string to a character array 752         latm = atof(latm_ch); //Convert the character array to a float 753         if(lat_dir == "S") lat = ‐(latd + latm/60); //Calculate the decimal degree latitude 754         else lat = latd + latm/60; 755          756         lond_sr.toCharArray(lond_ch,4); //Convert the string to a character array 757         lond = atof(lond_ch); //Convert the character array to a float 758         lonm_sr.toCharArray(lonm_ch,8); //Convert the string to a character array 759         lonm = atof(lonm_ch); //Convert the character array to a float 760         if(lon_dir == "W") lon = ‐(lond + lonm/60); //Calculate the decimal degree longitude 761         else lon = lond + lonm/60;    762       } 763     } 764     else 765     { 766       Serial.print("No GPS Connection \n"); 767     }    768 } 769  770 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Handle ODROID and camera communication‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 771  772 void Camera() 773 { 774   //Variable declarations 775   String inString = "#0,0,0"; 776   char inBytes[12]; 777   String nestString = "0"; 778   char nestChar[1]; 779   int nest = 0; 780   String distString = "0"; 781   char distChar[3]; 782   int dist = 0; 783   String posString = "0"; 784   char posChar[3]; 785   int tagIndex = 0;   786    787   if (Serial.available() > 0) 788   { 789     Serial.readBytes(inBytes,12); //Read the serial port 790     inString = inBytes; 791     tagIndex = inString.indexOf("#"); //Search for "#" tag 792

v3.1 33

    if(tagIndex != ‐1) 793     { 794       nestString = inString.substring(tagIndex+1,tagIndex+2); 795       nestString.toCharArray(nestChar,2); 796       nest = atoi(nestChar); 797       tagIndex = inString.indexOf(","); //Search for the first "," 798       inString = inString.substring(tagIndex+1); //Remove the first "," 799       tagIndex = inString.indexOf(","); //Search for the second "," 800       distString = inString.substring(0,tagIndex); //Extract the distance nibble 801       distString.toCharArray(distChar,inString.length()); 802       dist = atoi(distChar); 803       inString = inString.substring(tagIndex+1); //Remove the second "," 804       tagIndex = inString.indexOf(","); //Search for the third "," 805       posString = inString.substring(0,tagIndex); //Extract the left‐right position nibble 806       posString.toCharArray(posChar,inString.length()); 807       nestPos = atoi(posChar);  808       809       if(nest == 1) nestSeen = true; //If a nest is currently in the field of view 810       else nestSeen = false; 811       if((dist > thermMaxRange) && (nest == 1)) nestClose = true; //If a nest is close 812       else nestClose = false; 813       if(nest != 1) nestPos = 1000; //Set nest position as out‐of‐bounds if not currently in field of view 814       nestDist = dist; //Need to send this out for the calibration step 815        816       //Display the ODROID communications on the LCD screen 817       inString = inBytes; 818       lcd.setCursor(0, 1); 819       lcd.print("                "); 820       lcd.setCursor(0, 1); 821       lcd.print(inString); 822     } 823   } 824 } 825  826 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐Interrupt for the bump sensors‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 827  828 void INT_Impact() 829 { 830   if(i>10) impact = 1; 831 } 832  833 //‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐END OF ARDUINO MAIN‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐‐ 834

Vision Processing Algorithm Appendix B – /* 1 Program: Target Detection 2 Description: The visual target detection algorithm running natively on Sentinel’s ODROID‐U3 3 Author: Luke Scime 4 Last Modified: 4/16/2014 5 Comments: 6  7 g++ ‐ggdb `pkg‐config ‐‐cflags opencv` ‐o `basename TargetDetect.cpp .cpp` TargetDetect_v9.cpp `pkg‐config ‐‐libs 8 opencv` 9  10 */ 11  12 #include "opencv2/imgproc/imgproc.hpp" 13 #include "opencv2/highgui/highgui.hpp" 14 #include "opencv2/imgproc/imgproc_c.h"  15 #include <stdlib.h> 16 #include <stdio.h> 17 #include <iostream> 18 #include <string.h> 19 #include <unistd.h> 20 #include <stdint.h> 21 #include <fcntl.h> 22 #include <termios.h> 23 #include <errno.h> 24 #include <sys/ioctl.h> 25  26 using namespace cv; 27 using namespace std; 28  29 int main()  30 { 31  32   int size_pink = 0; //Size of the pink mask 33   int size_green = 0; //Size of the green mask 34   Moments p; //Centroid of the pink mask 35   float x_pink = 0; //x coordinate of the pink centroid 36   float y_pink = 0; //y coordinate of the pink centroid 37   Moments g; //Centroid of the green mask 38   float x_green = 0; //x coordinate of the green centroid 39   float y_green = 0; //y coordinate of the pink centroid 40   bool nest = false; //Is a nest currently in the field of view? 41   float distance = 0; //Distance to the target, note that this measurement gets LARGER as the target gets closer 42   float location = 0; //Left‐right location of the target 43   char buffer[12]; //Character buffer for data string sent from the ODROID to the Arduino 44   string send = "0,0.00,0.00"; //Data string sent from the ODROID to the Arduino 45   int fd; //The device label for the Arduino 46   struct termios toptions; //Store the serial port settings 47  48   //Open the serial port 49

v3.1 35

  fd = open("/dev/ttyACM0", O_RDWR | O_NOCTTY); 50   printf("fd opened as %i\n", fd); 51        52   usleep(3500000);  // Wait for the Arduino to reboot  53        54   tcgetattr(fd, &toptions);  //Get current serial port settings  55    56

//Set 9600 baud both ways  57   cfsetispeed(&toptions, B9600); 58   cfsetospeed(&toptions, B9600); 59  60   // Serial settings: 8 bits, no parity, no stop bits  61   toptions.c_cflag &= ~PARENB; 62   toptions.c_cflag &= ~CSTOPB; 63   toptions.c_cflag &= ~CSIZE; 64   toptions.c_cflag |= CS8; 65  66   // Canonical mode  67   toptions.c_lflag |= ICANON; 68  69   //Commit the serial port settings  70   tcsetattr(fd, TCSANOW, &toptions); 71  72   VideoCapture stream1(0); //0 is Sentinel's webcam 73   74   if (!stream1.isOpened()) 75   {  76     //Check if the webcam has been initialized 77     cout << "Cannot open camera."; 78   } 79   80   while (true)  81   { 82     Mat cameraFrame; 83     Mat maskedFramePink; 84     Mat maskedFrameGreen; 85     Mat erodedFramePink; 86     Mat erodedFrameGreen; 87  88     stream1.read(cameraFrame); //Read the camera stream 89     // Perform the threshholding for neon pink 90     inRange(cameraFrame,Scalar(100,0,100),Scalar(255,100,255),maskedFramePink);    91       //Perform the threshholding  for neon green 92     inRange(cameraFrame,Scalar(0,135,0),Scalar(100,255,150),maskedFrameGreen);  93      94     //Perform and erosion using default settings 95     erode(maskedFrameGreen,erodedFrameGreen,Mat()); 96     erode(maskedFramePink,erodedFramePink,Mat()); 97  98     size_pink = countNonZero(erodedFramePink); 99     size_green = countNonZero(erodedFrameGreen); 100     p = moments(erodedFramePink, false); //Calculate the centroid of the pink masked image 101     g = moments(erodedFrameGreen, false); //Calculate the centroid of the green masked image 102

v3.1 36

     103     x_pink = p.m10/p.m00; //x coordinate of the pink mask 104     y_pink = p.m01/p.m00; //y coordinate of the pink mask 105     x_green = g.m10/g.m00; //x coordinate of the green mask 106     y_green = g.m01/g.m00; //y coordinate of the green mask 107      108     //Print the centroid and size information   109     cout << x_pink << ", " << y_pink << endl; 110     cout << x_green << ", " << y_green << endl; 111     cout << size_pink << endl; 112      113

//Nest is confirmed if pink and green centroids are vertically aligned and the masks are large 114     if((fabs(x_pink‐x_green)<40) && (size_pink>200) && (size_green>200)) nest = true; 115     else nest = false; 116        117     if(nest) 118     { 119       cout << "I found a nest! Range: " << distance << " Location: " << location << endl; 120       distance = fabs(y_pink ‐ y_green); //Distance to the nest 121       location = ((x_pink + x_green)/2) ‐ 330; //Left‐right location of the nest  122     } 123     else 124     { 125       cout << "I'll keep looking..." << endl; 126       distance = 0; 127       location = 0; 128     } 129      130     sprintf(buffer,"#%d,%d,%d,",nest,(int)distance,(int)location);  //Construct data string 131     cout << buffer << endl; //Display the data string for debug purposes 132     write(fd, buffer, 12); //Sent the data string to the Arduino 133     usleep(500); //Slow data transmission rate to accommodate the Arduino 134    135     imshow("Raw", cameraFrame); //Display the raw image 136     imshow("Pink", erodedFramePink); //Display the pink masked image 137     imshow("Green", erodedFrameGreen); //Display the green masked image 138     if (waitKey(30) >= 0) //Allow for terminating the program 139     break; 140   } 141   return 0; 142 } 143