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