xsunroboticdesignpaper

1

Gopher Tortoise Scope Redesign

Xiangzhen Sun1

Itiel Agramonte2

Submitted On:

December 11, 2015

EML5930 – Mechatronics II

Dr. Clark

Fall 2015

2

Table of Contents

Title and Group Member Information……………………………………………………1

I. Project Overview………………………………………………………………………3

1. Research Background………………………………………………………………3

2. Design Requirements……………………………………………………………….3

3. Design Objectives…………………………………………………………………..4

4. Design Constraints………………………………………………………………….4

II. Exploration, Analysis, and Design…………………………………………………....5

1. Functional Analysis…………………………………………………………………5

2. Overall Circuitry……………………………………………………………………7

3. Electrical Design…………………………………………………………………....9

3.1 Circuitry of Microprocessor…………………………………………………….9

3.1.1 Gamepad Control…………………………………………………………..9

3.1.2 Video Streaming & Image Capture………………………….....................11

3.2 Circuitry of Microcontroller………………………………………………..…13

3.2.1 Temperature/Humidity Sensor……………………………………………13

3.2.2 Encoder…………………………………………………………………...14

3.2.3 Control Algorithm………………………………………………………..16

3.3 Data Communication and Power plan………………………….......................18

4. Mechanical Design………………………………………………………………...20

4.1 Design Consideration………………………………………………………….20

4.2 Motor Torque Calculation……………………………………………………..21

4.3 Chamber Design……………………………………………………………….21

4.4 Transmission Design…………………………………………………………..22

4.5 Camera Design…………………………………………………… …………..23

4.6 Manufacturing and Assembly …………………………………………………23

III. Challenges………………………………………………………………………….25

IV. Conclusion and Future Work………………………………………………………25

V. Appendix……………………………………………………………………………26.

3

I. Project Overview

Initial background with respect to gopher tortoise along with currently commercial scopes will be

introduced in this section. The necessity as well as major constraints of proposed project design

will be introduced.

1. Research Background

Tortoise has a large effect on its surrounding ecosystem mainly because of its burrow provides

shelter for many species. This fact provides the study of the animal enough impetus, especially for

a research station such as Tall Timbers which specializes in fire ecology studies. Gopher tortoises

however are not the only burrowing animals that require a scope in order to be studied. Other

animals like foxes and small mammals are also burrowing too. To meet this need for research

equipment, Sandpiper Technologies, INC. introduced the Peeper 2000. Its system has the benefits

of being lightweight as well as waterproof. However, given further insights into Sandpiper system,

the major drawback of costing 6,000 dollars apiece becomes a concern. This is generally out of

the budget of research centers such as Tall Timbers, leaving them still without a scope. In order to

meet this need, Tall Timbers built their own scope for a total of about 500 dollars. It is however

outdated and slightly crude in design, consisting of an infrared camera connected via long

detachable wires to a portable DVD player. The wires are protected by thick rubber hosing. This

hosing has proven to be heavy as well as not easily navigated through the burrow, and the DVD

player is not waterproof.

The creation of a scope that is on the technological level of Sandpiper’s Peeper 2000 while also

costing less than 1000 dollars would be pivotal for research centers such as Tall Timbers, and

could do a great deal of good for the advancement of the study of many burrowing keystone

species, not only the gopher tortoise.

Tall Timbers Research Station and Land Conservancy originally tasked the mechanical

engineering senior design Team 21, from the fall of 2014 semester, to develop a system for entering

the burrows of the Gopher Tortoise to monitor the population of the tortoises. Their requirements

include remote control vehicle to drive into the burrows with an IR camera attached to it and they

would also like the ability to read the temperature and humidity data from the burrows. The

dimensions of the robot were to be a max of 4inx6inx10in with a max weight of 40 pounds.

Team 21 developed a robot that would be driven into the burrows with an IR camera and also

monitor the temperature and humidity within the burrows. The initial design, though functional

and water proof, was too large to enter the burrows and move over the angled environment within

the burrows. It met the height, width, and weight constraints but was over a foot in length. The

team, this year, has been tasked with optimizing the body of the robot itself and improving

functionality.

2. Design Requirements

The previous design is cumbersome for several reasons. In order to observe through scope, user

have to control the motion of servo-camera with gamepad, which is redundant. This process can

be replaced by adjusting the position of robot body. Original Design results in longer body length.

Thus, the camera can easily dig into the ground and get blocked by dirt. It is difficult to navigate

the scope, as there is nothing to help it move forward, backwards or navigate turns. Because of

4

this lack of maneuverability, many parts of the burrows are unreachable for observation. The scope

plus rover, which involve many large components, are heavy and bulky. By the end of the day, the

sponsor related that her hands would be covered in blisters from having to physically handle the

heavy equipment for eight or more hours. Besides, there are no video or picture-capturing

capabilities with the current model.

Requirements Summary:

To sum up, there is a need for gopher scopes to have weather and impact durability, greater

mobility, data-acquisition capability, and reduced weight, space especially body length and width.

3. Design Objectives

It is essential for scoping system to be resistant to water as well as dirt, and be able to withstand

temperatures from 0 to 100°F due to the fact that it will be used in the field. It should be resistant

to shock as well in case it is dropped or hits any obstacles. The battery life should last for as long

as it takes to complete full days of field testing and the entire system should fit into a backpack

and weigh less than the current scope at Tall Timbers.

Gopher tortoises begin to burrow as soon as they hatch with some of their burrows being as small

as 4 to 6 inches. Because of this, the scope should be small enough to navigate inside these smaller

burrows. Not disturbing the animals in the burrow is important as well, therefore the camera will

be infrared and the rover will move as quietly and quickly down the burrow as possible. The

camera should be able to record images, capture still photos and take temperature and humidity

readings in the burrow. The main goal is to design a mechanism that has testing sensors, better

durability, and more advanced video capabilities than the current system in order to enhance the

surveying process of gopher tortoises. Most importantly, the dimension of the whole mechanism

should be constrained within a small space as small as a normal burrow.

4. Design Constraints

The following is a list of the constraints of the project. Solving the constraints is crucial to the

success of the project redesign and will allow my team to provide a fully functioning scope to

presentation.

a. The rover must not be more than 7 inches wide, 10 inches long, and 4 inches high

b. The entire system must remain under 50lbs

c. The entire system must be water proof

d. The duration of system operation has to target 8 hours

e. The selected motors need be small enough to be housed in chamber, and provide enough torque.

f. User friendly control panel versus multifunctional operation system.

5

II. Exploration, Analysis, and Design

In this part, we will go through all details in designing this project along with specific

considerations. Moreover, we will also discuss on functions that are being developed but not fully

completed. Possible solutions and our exploration processes will be attached.

1. Function Analysis

While constrained by malfunctions contained in previous design, several primary specifications

affect the overall design of the scope at the same time. The most important of these is cost. The

cost for the entire redesign project cost should be no more than $100 from Dr. Clark and rest costs

have to be covered by us. The weight and size of the final design is also important to consider, as

one person must be able to carry the scope for several miles. For this reason, the full system must

weigh no more than 50 pounds and fit in a backpack.

Mechanical subsystems have additional specifications that must also be considered when creating

the final design. In order to fit into even the smallest of gopher tortoise burrows, both the height

of body and the diameter of rover should be approximately four inches. It will also need to be

maneuverable enough to make tight turns, and have good enough traction to be able to function in

wet and muddy conditions. To handle these requirements, a body is designed that is 4 inches from

one edge of the track to the other. The height of this rover above ground is currently about 1 inch,

since most burrows are oval in shape. It will have a treaded design that allows it traction to

overcome the resistance from the tether, as well as resistance from obstacles within the burrow.

The force that is provided to the treads is about 35 N, while the preliminary estimate for static

tether friction force is only about 5.4 N. This yields enough net force for the rover to overcome

obstacles. Just how competent it is at doing so will be determined in field testing.

Current tread dimensions for the rover allows a contact patch of about 52 cm. This allows it to

have greater traction. The material for the treads must be durable and withstand tension and shock.

Sectioned rough patches can be attached to the tread to give it additional tractive force. A challenge

arises in finding the proper length belt for the track. It is possible that a desired track product has

a length determined by the manufacturer. Hence, the rover wheelbase and body may have to adjust

to fit the treads.

The body itself is going to be enclosed in ABS. This material is easy to acquire, relatively

inexpensive to cut, and has unique capabilities. The front camera that will be mounted to the dome

chassis needs to be infrared capable. In order to mitigate glare, the orientation of the dome cover

in front of the camera will be modified.

In addition to the casing material, the body also needs to be weatherproof. Namely, it needs to be

water and dirt resistant in order to survive in burrows that are dug into the earth. Hence, special

attention must be given to the type of adhesive used to seal out any potential debris, as well as the

type of sealing for each wheel axle (so that dirt and water do not seep in through the axles that

protrude out of the body).

The body must be strong enough to withstand the weight of the internal components as well as

shock from external forces. The high strength of the ABS (110MPa yield strength, 115MPa

6

flexural strength) ensures that it can withstand the impact of colliding with an object at the rover’s

top speed, as well as not fatigue under the load of all of the components being fastened to it.

In previous design, a pan-and-tilt system is a major desire by the sponsor. Yet it turns out to be

redundant now. Even with a small camera, the need for the camera rotation, as well as the need for

a mounting frame that the camera sits in, means that small space will be an issue for the pan-tilt

system. The rover will be dependent on a tether. Gopher tortoise burrows can reach up to 15 m in

length, and almost 5 m deep. Hence, sending a wireless rover increases the risk of losing the rover

within the burrow. With a tether, even if power is lost to the rover, it can still be physically pulled

out of the rover. A tether also allows for the power source (battery) to be above ground,

transmitting power to the rover through the tether. Hence, the tether will have wires to control the

rover, as well as having a tension guide wire (perhaps a steel cable) to relieve tension from the

electronics cables. The tether is to have a tough yet flexible exterior. It is desired that a Kevlar

material sheath can be used to protect the wires, while not being too rigid or large.

Logitech F310 gamepad was chosen because it has

drivers that work with the Raspberry Pi series.

Established library with member functions

handling gamepad events is provided. Having an

intuitive user input is a great way to make the

learning curve very low. The two joysticks each

control two sets of motors: rover left tread and

right tread The USB inputs from the gamepad can

be translated to keyboard inputs which will be read

by Linux based scripts. Maximum current draw

from the gamepad is 400mA at 5V.

Raspberry Pi B+ has a 700 MHz CPU and a very capable GPU to handle the data input/output.

The four USB ports are also useful in receiving input from the RCA/USB adaptor for video feed,

the gamepad for user input, power and data to the Arduino micro. The raspberry pi b+ has a current

draw range of 600mA to 1.8A at 5V.

The lithium ion battery is 12V and has a capacity of 14AH. If the current drawn from the system

is an estimated 6A, then the system can operate for 2.5 hours continuously. The battery weighs

only 0.75kg so if the energy capacity is not enough for a full 8 hour day of operation then a second

battery can be purchased and easily replace the

drained battery when needed. The rover system is

not expected to be operating longer than a

continuous 3 hours out of the 8 hour work day so

the battery may be sufficient. There is a power

converter from 12V to 5V 3A USB port that will

power the screen and raspberry pi b+ with a USB

splitter. A DC-DC converter needs to be

purchased to deliver continuous 12V output to the

camera and motor drivers.

7

Arduino Micro is small in size although not

the smallest of its type. But it has ample

performance in the CPU to read the USB

input from the raspberry pi b+ and output

PWM signals through the GPIO pins. The

GPIOs will also be replaced by USB cable

connection for temperature and humidity

sensor input. The maximum current draw of

the Arduino micro is 50mA at 5V.

Motor drivers use a L298 chip to read the

PWM signal from the Arduino to change the

power outputted to the motors. Each motor

driver has signal input and output for two

motors. Each motor driver has an input of

12V and the current draw changes based on the load of the motor.

Easy Cap DC60 RCA to USB adaptor converters the RCA video feed from the camera, and

converts it to USB signal. This adaptor was chosen because there are drivers and other projects

that use this device on the raspberry pi b+. There is a 0.4 second delay because of the analog to

digital data conversion. The maximum power draw of the adaptor is 250mA at 5V.

The size of the 7-inch screen has been proven by mass produced products as a great size for mobile

monitors. The screen is the same resolution of 800x480 pixels as the camera feed. This does not

allow for distorted image quality. Having a USB for power will make for easy integration into the

system by adding a USB hub to the 5V 3A adaptor source from the battery. The maximum current

draw is 500mA at 5V.

2. Overall Circuitry

It is helpful to refer to electrical schematic in this part for user to operate even debug every

subsystems. The user interface electrical components are connected as follow:

The Raspberry Pi B+ has an HDMI male/male coupler connected to the screen driver. The micro

USB connected on the pi is connected to the USB splitter on the 5 V buck/boost converter. The

USB hub is connected to the Raspberry Pi B+ through a type-A and type-B USB connection for

data.

8

The screen driver connects to the screen through two multi-pin connectors for power and data. The

screen driver has a barrel connection that is powered by 12 V buck/boost converter on the terminal

block.

The USB hub is powered by a USB to barrel connector wire from the USB splitter on the 5 V buck

converter. The gamepad, RCA to USB Adapter, and tether active USB cable are connected to the

USB hub.

The RCA to USB adapter has the tether RCA connected to the yellow male connection. The two

buck/boost converters are connected to the battery through a screw terminal. The battery is then

connected with the ring terminal wires to the interface casing.

The rover components are connected as follow:

The tether has power from the battery screw terminal to the rover’s 12 V buck/boost converter.

The 12 V output is connected to a screw terminal where the camera and motor driver receive

power. The screw terminal’s ground is connected to the camera, motor driver, DHT22, and the

Arduino Micro.

The motor driver is connected to the two rover motors on pins AB and CD. The 5 V output of the

motor driver is connected to a screw terminal where the DHT22 receive power. The motor driver’s

two 3-pin signal inputs are connected through a ribbon cable to the Arduino Micro’s pin # 3 – 8.

9

The Arduino Micro’s pin #9 is connected to the DHT22. The micro USB connection is from the

tether active USB.

The camera is connected to the tether RCA cable to output video signal.

3. Electrical Design

This section helps user understand why and how each components being selected and connected

is specific ways.

3.1 Circuitry of Microprocessor

3.1.1 Gamepad Control

A majority of the I/O

and data processing

will be handled by the

Raspberry Pi B+

located in the User

Interface. For this

reason, a majority of

the code written will

also be for the

Raspberry Pi. The most

critical code will be that

which reads in user input from the gamepad joysticks and then makes the rover motors, and pan

and tilt motors spin accordingly. When a user moves one of the joysticks on the user interface, the

gamepad sends two integer values to the Raspberry Pi; a horizontal coordinate ranging from -

32,767 to 32,768 and a vertical coordinate also ranging from -32,767 to 32,768. In order to turn

these horizontal and vertical coordinates into useful directions for the motors, they are converted

into polar coordinates and the charts below are used.

Button Press Command

Y Read Sensor

Mode Pause drive

Select Shutdown Raspberry Pi

Joystick Drive pattern

Full UP Left, Full UP Right Full speed forward

Half UP Left, Half UP Right Half speed forward

Full Down Left, Full Down Right Full speed backward

Half Down Left, Half Down Right Half speed backward

Full UP Left, Full Down Right Full speed CW

Half UP Left, Half Down Right Half speed CW

Full Down Left, Full UP Right Full speed CCW

Half Down Left, Half UP Right Half speed CCW

No direction, No direction Break

Full UP Left, Half UP Right Left forward

Half UP Left, Full UP Right Right Forward

Full Down Left, Half Down Right Left backward

Half Down Left, Full Down Right Right backward

10

For the Rover motors, the chart above is used. The domain of possible joystick inputs is represented

as a circle. Radially, the circle is divided into 8 sections, corresponding to the direction that the

rover will move for a given angle of the joystick. The domain of possible inputs is further broken

up into three concentric circles. If the magnitude of the joysticks position falls into the red circle,

then the rover will stop for brake. The yellow circle corresponds to slow movement, and the green

circle fast movement. Overall, the domain of possible user inputs is dived into 24 sections.

Once the Raspberry Pi reads in which of the 24 possible cases has been selected by the user, a five

bit code is sent to the Arduino. Using this and the information in the following table, the Arduino

is able to send the proper signals to the motor drivers to move the rover as the user instructed. In

order to limit the amount of data being sent between the Raspberry Pi B+ and Arduino, the Arduino

will continue to drive the motors in this way until a change in the user input is detected by the

Raspberry Pi B+.

11

3.1.2 Video Streaming & Image Capture

The video streaming requirement decides selection of microprocessor as Raspberry Pi B+. Before

talking about the method we employed streaming video lively, it is necessary to give reason upon

processor and graphic driver selection. While power consumption is an important consideration

for the overall design, the difference in power usage between most microprocessors is small

enough that it does not have a large impact on the design. Similarly, while having a community of

consumers that use the microprocessors can aid in programming and make spare parts easier to

obtain, it is not thought to be important enough to heavily influence which microprocessor we use.

The selection criteria were determined to be the graphics, memory, expandability and overall cost.

Graphics are a necessity because the end-user must have a live feed of the camera input. It is also

one of the major performance differences between the two microprocessors being considered and

it is important that this difference be accounted for in the decision matrix. Memory is weighted as

average importance because it is needed to record video and images along with keeping the

12

operating system of the microprocessor onboard. Expandability refers to the number of add-on

devices that have been designed specifically for the microprocessors by manufactures such as

Adafruit and SparkFun. These add on devices can give the microprocessors LCD screen or

temperature sensor capabilities without the team having to design custom options. However, since

this is not a necessity to complete the design, it was given a lower weight. Cost was given a slightly

higher weight since a major part of the design is to make the rover cost effective. The cost category

does not just account for the cost of the microprocessor, but also the cost of the necessary

peripherals such as microSD cards or USB cables.

The microprocessor will be the hub for the majority of the data being input and output to our

system. The processor will need to have a high enough clock speed in order to make the device

function with as little lag as possible and ensure a good end-user experience. What connectivity

options are available on the microprocessor will also determine how well the final design works.

Available input and output ports can include but are not limited to USB, HDMI, RCA, GPIO, I2C

and CAN. The more options available, the more likely a product that will be compatible and meet

the criteria will be found.

Before editing video stream feature, my group first ran the method employed by previous design

group. It turns out that their method does not work but somehow they did not realized reason.

Through research, we found that the microprocessor that has been selected does not support video

streaming through the player (m-player) that’s preinstalled in Raspbian OS. Later version of Pi

series may support this function. However, under the consideration of great cost on replacing

microprocessor, we decided to keep using the current microprocessor, while streaming video lively

through VGA cable. Anyway, if it is not streaming video on web, there is no necessity to pomp up

preinstalled player software in Raspberry pi. This is the first adaption we made on streaming video.

By doing that, however, we lose the functionality of taking picture while streaming video. Based

on that requirement, we tried to put a tiny web camera aligned with IR camera. In this way, two

camera work simultaneously to achieve streaming video and taking snapshot without losing any

functionality. To take snapshot, we created a new folder for storing pictures in jpeg format, and

create shortcut executable file on desktop using following command (SDL lib supposed to be

installed first):

system (device=/dev/video0:fps=1:noaudio:outfmt=uyvy:fps=10 -vo jpeg -frames 3 -fs -vf

format=y8,scale -hardframedrop &");

13

We successfully took a picture by double clicking mouse on desktop icon. However, we failed to

initialize the snapshot.exe file on startup. In other word, user has to type long command lines in

Xterminal to reinitialize snapshot.exe, which is not desirable. So we gave up this functionality.

3.2 Circuitry of Microcontroller

In this section, all parts directly connected to

microcontroller (Arduino Micro) will be introduced.

Meanwhile, functions realization of each

components correlated will also be specified.

3.2.1 Temperature/Humidity Sensor

The DHT22 sensor is manufactured by USPRO.

Temperature and humidity is measured by the sensor

every two seconds. Temperature precision is 0.5

degrees Celsius. Humidity procession is 0.05%

relative humidity. The USPRO model is used

because of a built-in series resistor and capacitor on

an integrated circuit board.

In Align

Web Camera

+5 V

14

The microcontroller will have to send and receive data from the microprocessor as well as control

four motors and read inputs from two sensors.

Arduino Micro is small in size and has ample performance in the CPU to read the USB input from

the Raspberry Pi b+ and output PWM signals through the GPIO pins. The GPIOs will also be used

for a 4 pin temperature and humidity sensor input. The maximum current draw of the Arduino

micro is 50mA at 5V.

Since there is only one USB connecting the Raspberry Pi b+ to the Arduino, it is important the two

devices coordinate with one another so that all messages are sent and received properly. In order

to achieve this, the default states of the devices will be the Raspberry Pi b+ transmitting data and

the Arduino receiving data. This will only change when the Raspberry Pi b+ requests that the

Arduino send it data. This request will only be sent once every ten seconds. When it is sent, the

Arduino will poll the temperature and humidity sensors and then send the collected data back to

the Raspberry Pi b+ to be displayed on the User Interface. The Arduino will then go back to

listening for motor control data from the Raspberry Pi b+.

3.2.2 Encoder

It is important to verify how many counts per revolution of selected DC motor with encoder. By

running the following program, we verified the encoder along with motor encoder precision:

int val;

int encoder0PinA = 3;

int encoder0PinB = 4;

int encoder0Pos = 0;

int encoder0PinALast = LOW;

int n = LOW;

void setup() {

pinMode (encoder0PinA,INPUT);

pinMode (encoder0PinB,INPUT);

Serial.begin (9600);

}

void loop() {

n = digitalRead(encoder0PinA);

if ((encoder0PinALast == LOW) && (n == HIGH)) {

if (digitalRead(encoder0PinB) == LOW) {

encoder0Pos--;

} else {

encoder0Pos++;

}

Serial.print (encoder0Pos);

Serial.print ("/");

}

encoder0PinALast = n;

15

}

The motor we selected has encoder with six pins. The functionality of each pin is defined as follow:

Red Pin Motor+

Black Pin Motor-

Green Pin GND

Blue Pin Vcc

Yellow Pin Vout-A

White Pin Vout-B

Since we control two motors separately (see control algorithm part), it is necessary to bring two

sets of encoder pins into Arduino PWM pin connections. We connected encoder 1 Vout-A to pin#

2, encoder 1 Vout-B to pin# 10, encoder 2 Vout-A to pin # 3, encoder 2 Vout-B to pin # 11. To

detect any pulse indicating encoder rotates by one step, we defined counter + 1 while voltage at

interrupter pin (pin 2 and pin 3) falling from high to low. Actually, you can also define a raising

voltage as counter – 1 on the other hand.

To make use of default interrupter predefined in Arduino Micro, we can simply attach interrupt

mode by calling the following phrase:

attachInterrupt(digitalPinToInterrupt(pin2), counter1++, Falling);

attachInterrupt(digitalPinToInterrupt(pin3), counter2++, Falling);

In Arduino setup() section, we also need to define Interrupt Service Routines so that counter

understands the behavior of encoder. Generally, an ISR should be as short and fast as possible. If

your sketch uses multiple ISRs, only one can run at a time, other interrupts will be executed after

the current one finishes in an order that depends on the priority they have. millis() relies on

interrupts to count, so it will never increment inside an ISR. Since delay() requires interrupts to

16

work, it will not work if called inside an ISR. micros() works initially, but will start behaving

erratically after 1-2 ms.delayMicroseconds() does not use any counter, so it will work as normal.

Typically global variables are used to pass data between an ISR and the main program. To make

sure variables shared between an ISR and the main program are updated correctly, declare them

as volatile.

3.2.3 Control Algorithm

In this section, we will specify how we add controller to control the speed of motor spinning. What

comes first is the concern about motor speed difference. If two motors have speed difference, it is

difficult to adjust this expanding error by controlling joysticks, even if we have defined CW, CCW

rotation. Below is a control diagram of proposed control algorithm

Next, we will use pseudo codes to demonstrate and give reason to proposed control algorithm.

Here’s the code for the equation we used. The gain constants are set in the code, but could be made

programmable through some other interface.

void CalculatePD(void)

{

// Set constants here

PTerm = 10;

DTerm = 0.7;

Divider = 10;

// Calculate the PD

Accumulator += Error[0]; // accumulator is sum of errors

PD = Error[0]*PTerm; // start with proportional gain

PD += DTerm*(Error[0]-Error[9]); // differential gain comes next

Here are the major components of our PD controller in detail.

Error Signal: At the heart of PD control is a need to measure an error signal. In this case it is the

desired speed (voltage from the pot) minus the actual speed (voltage from the encoder). The error

value is signed.

17

void GetError(void)

{

byte i = 0;

// read analogs

word ActualSpeed = analogRead(ActSpd);

word DesiredSpeed = analogRead(DesSpd);

// shift error values

for(i=9;i>0;i--)

Error[i] = Error[i-1];

// load new error into top array spot

Error[0] = (long)DesiredSpeed-(long)ActualSpeed;

}

PTerm: The error signal is multiplied the P term and this provides most of the “umph” behind the

motor’s movement. A negative error signal causes the P term create a negative motor movement.

Likewise, if my error signal is 0 then the P term has no impact on the motor.

PD = Error[0]*PTerm; // start with proportional gain

In some systems it is important to prevent “windup” of the accumulator (also called saturation).

Windup occurs when the small errors build so high that when movement finally occurs it takes a

long time for the accumulator to reduce to an insignificant amount. We didn’t add integration term

for this reason.

DTerm: While we have the D term in this equation as 0.7, it is not always the same value before

tuned in the end. The D term is multiplied by the change in the error signal (error – last error).

Sometimes it is useful to store your error measurements in an array and use as last error something

a little further back in time. For fast changing systems, like a motor controller, the derivative

portion of the PD has little impact unless you make it very large, or compare error signals with

adequate time between their sampling. In this code the derivative error is the latest error signal

minus the 10th previous error.

PD += DTerm*(Error[0]-Error[9]); // differential gain comes next

Divider: When we put the PD together you get a pretty big value. This value needs to be scaled to

a value that matched the pulse-width modulation range for the controller. The Divider does that.

You’ll notice that the division of the PD is accomplished by right-rotates as opposed to division.

This is just a faster way of accomplishing the same thing. And the faster our PD loop runs the

more responsive it can be to commanded changes.

PD = PD>>Divider; // scale PD down with divider

18

Converting the PD to PWM: Once your PD is calculated you need to change it to a motor drive

signal. The sign of the PD output determines the direction the motor should be driven and the

divider previously discussed should get you in the right neighborhood for a final number. Now

we make sure the PD register contains a value that’s neither too large nor too small.

If (PD>=127)

PD = 127;

If (PD<=-126)

PD = -126;

//PWM output should be between 1 and 254 so we add to the PD

PWMOutput = PD + 127;

The PWM pin accepts a range of 1 to 254 (1 = full reverse, 127 = stopped, 254 = full forward). So

we want our PD to be in the area of –126 to +127, and we’ll add 127 to it to get our 1-254 range.

Tuning the PD: There are a number of methods of tuning PD algorithms. For a DC motor speed

controller using analog signals we start by adjusting your proportional settings until you get rough

control. If your proportional term is too high the movement will be sharp and choppy. If too low,

it will be slow. This is also when you dial in the divider value to make sure your PD output falls

within your PWM requirements.

The settings we used for this design got me to within +/-10 ADC counts within a couple of seconds.

That is right at about 1% accuracy or 3.5 degrees for a single rotation.

3.3 Data Communication and Power plan

19

Above is the top level design for how the projects power and data flow which has been split into

above ground and below ground modules. The battery, at full charge, provides 13.4v and will

continue to provide power until it is down to 9 volts, and then needs to be charged, with an average

run time of 2.5 hours with a 5 hour charge time, with a life time of 800 cycles. The power out from

the battery is driven through a DCDC step-up / step-down converter, which allows a constant 12.0v

to be outputted from the converter. Through testing with an alternating voltage, it was determined

that the buck boost was stable without any ripple showing in the output of the boost. That output

is used for all the 12v systems - the video driver, the motors, and the IR RCA camera. The 12v

power is also then input into a 5v step down converter, which powers the raspberry pi, the Arduino

micro, the DHT22 temperature and humidity sensor, and the encoder for both motors.

The user interface system can be seen above. This portion is above ground and held by the user.

The box that the controller is plugged into contains the raspberry pi, and video driver, the step

down 5v converter and 12v step-up/step-down converter, with the video display connected to the

top of the box. The user may press a button on the remote attached to the box to switch between

the RCA input from the IR camera, and the HDMI input from the raspberry pi, which is where the

sensor data will display. The tether connection can be seen below. This is how power and data

transfer.

20

Tether Connections

Circuit Diagram for rover

The rovers systems can be broken into those components powered by the 12v input from the tether,

while the other systems are powered by the motor drivers 5v output. One of the 12v components

are the IR camera which uses RCA video with no audio. The others are the motors which are

controlled by a motor driver which uses a 12v input and PWM signal to drive the motor speeds,

full controlled by the Arduino Micro. The Arduino Micro is controlled by the 5v source, which

receives commands from the raspberry pi, and the micro tells all subsystems what to do. The

encoders and sensor are also powered by the 5v source.

4. Mechanical Design

In this section, mechanical design process is mainly talked about, with an emphasis on dimension

concerns and corresponding solutions to them. As stated, to rescale previous design to almost ¾

of it, calculation and components selection need to be taken particular care of.

Encoder

21

4.1 Design Consideration

One of the most important consideration is size/weight. First of all, it is impossible to purchase

any commercially available chassis for rover: none of those chassis has the appropriate dimension

as required. The triangular tread design however, would be the largest in the vertical direction due

to the shape of its treads. Previous design separate rover body into several chassis. By doing so,

their design actually increased a lot redundant space even if it guaranteed that each components

do not interfere with others. Power consumption was also considered and the wheeled chassis out

scored both the linear and triangular treaded chassis due to the fact that the wheeled chassis

requires less torque. Treads are specially designed to maneuver over obstacles. Since the linear

treads do not have any open spaces that rocks or dirt can become trapped in as compared to the

wheeled and triangular tread design, it has the most maneuverability. Portability was the next

concern; the wheeled design scored higher than both treaded designs due to the fact that the

wheeled design is easier to clean and place in a backpack. With the treads larger surface area, there

is more dirt and cleaning required.

4.2 Motor Torque Calculation

We calculated the required torque each DC motor need to generate using the method recommended

by previous designers:

clear all

H = input('Enter Motor Power in W ');

d = input('Enter wheel diameter in mm ');

w = input('Enter rotational speed in rpm ');

Wt = 60000*H/(pi*w*10);

T = (d/2)*Wt;

disp(' ')

X = ['Torque ' , '= ', num2str(T)];

disp(X)

disp(' ')

disp(' ')

end

Enter wheel diameter in mm 10

Enter rotational speed in rpm 300

Torque 2 = 4.527

So designed torque is around 4.527N.mm.

4.3 Chamber Design

Based on sponsor’s request, we need to constrain our design to a space within 4inx6inx10in with

a max weight of 40 pounds. So our main concern becomes the width of the body. The motor we

selected need to be placed shoulder to shoulder inside of the chassis, while their width add up to

4.3 inches. Taking the width of two treads and wheels together, the minimum width of rover has

to be 6.5 inches, which is only 0.5 inches longer then required width. The other dimensions are all

within the range. Below is a picture specifying dimensional information of our chassis design:

22

The positions of the holes on base plate (to your left) are left for motor mount seats. Camera

extrude from the bridge shape front (to your right). It turns out that our design (after assembly) is

3.7 inches high, 6.5 inches wide, and 8.3 inches long.

4.4 Transmission Design

In order to combat the vibration brought by two motors simultaneously, each motor is not only

mounted on base plate, but also onto the side wall nearest to them. In this way, the weight of body

can effectively decrease vibration to minimum, protecting circuits inside from being damaged.

Below is a 3D simulation drawing that demonstrate the positional relation between components.

23

4.5 Camera Design

The IR camera needs protective cover in front of it. This cover should be transparent to infrared as

well as water proof when connected with surrounding parts. The way we machine this cover is to

cut a surveillance camera cover into two pieces symmetrically, and then put the half cover against

front wall of rover chassis with hot glue sealing edges. In this way, this design satisfied proposed

requests as it is shown in the following pictures.

4.6 Manufacturing and Assembly

Unless assembling in the correct sequence, it is not possible to put every components of circuit

with a limited space in chassis while not interfering the motion of motor shaft and wheel shaft.

Below is the assembly view of designed rover. Referring the explosive view, it is recommended

to install wheels onto the wheel shafts using M3 screws. Next, leaving wheel assembly apart, put

side walls against base plate. After that, you should be able to install two motors into the chassis,

using M4 screws. Arrange circuits components correctly and maybe compress extruding

wires/cables a little so that every parts sit inside of chassis. Then install the IR camera in front.

Connect wheel shafts to motor shafts and install the other two passive wheels by putting shafts

inside of drilled holes. Now safely put screen cover and top plate of chassis against each other and

use hot glue gun to seal all slots. Finally, install two treads. Treads are detachable, so it is easier

24

as well as safer to detach a screw from one tread joint, spread tread out, and align tread with wheels

before screw down detached joints.

25

III. Challenge

As with all projects there are certain risks and challenges that will need to be overcome during the

course of this year. One of the major requirements for the rover is for it to be waterproof. The rover

body will be encased in ABS, but there will be a point where the wires from the tether will have

to run into the casing. The interior of the case must also be accessible, so not all the joints will be

permanently sealed. These two areas of the rover will be extra susceptible to water leakage. The

tether and the connections at the rover and the user interface will have to be waterproofed. With

the tether it is important that the wires themselves are not stressed when the rover is being pulled

out. To mitigate this risk a steel guide wire may be placed in the tether sheath along with the wires.

This guide wire will take the brunt of the force when the rover is being pulled out of the burrow.

IV. Conclusion and Future Work

We did not meet the physical width, we were over by half an inch. We also weren’t able to machine

the half cylindrical body we envisioned due to a lack of experience with the machinery involved.

Another step to making this system full functional would be to waterproof the system. Another

system we would like to implement is an image capture system, to take pictures of the screen to

store sensor data and images of the current residents of the burrows. We had tried to implement

this system using an Easy Cap USB capture devices but it proved to only be functional for previous

operating systems of the raspberry pi.

26

V. Appendix

1.Arduino Code

/*Xiangzhen Sun, Program Description:

This program will be loaded to Arduino Uno for motor motion control. Two PD controllers separately

control motor on each sides.

This entail using two interrupter at the same time.

//Description: This program reads in a character from the serial port. This character is then

// stored into the variable "cmd". Based of the value of this variable, this code

// can perform a variety of tasks. These tasks are outlined in the table below.

// Note that the decimal value of ascii characters is used rather than the actual

// characters.

//

// ====================================================================

// | COMMAND ACTION | COMMAND ACTION |

// |--------------------------------|---------------------------------|

// | 96 read temp/humid | 107 slow backward |

// | 97 | 108 slow backward, right |

// | 98 | 110 fast right |

// | 99 | 111 fast forward right |

// | 100 breaks | 112 fast forward |

// | 101 slow, right | 113 fast forward left |

// | 102 slow forward, right | 114 fast left |

// | 103 slow forward | 115 fast backward left |

// | 104 slow forward, left | 116 fast backward |

// | 105 slow left | 117 fast backward, right |

// | 106 slow backward left | |

// ====================================================================

the COMMAND ACTIONs in () in above chart are not avaialb now.*/

//------------------------------------start---------------------------------------------------

//library and setup for the DHT22 temp and humidity sensor

#include <DHT.h>

#define DHTTYPE DHT22

const float pi = 3.14159;

unsigned long prev_cmd_time = 0; //time last command was received

const unsigned long dog_time = 3000; //set watch dog timer to 3 seconds

const unsigned int looptime = 100; //PID loop time

unsigned long curr_time; //this variable stores the current time instant

unsigned long prev_time = 0; //this variable stores the previous time instant

unsigned long delta_time = 0; //this variable stores the time difference

//--------------------------------------------------------------------------------------------

volatile long count[2] = {0, 0};// rev counter2, and rev counter3 are stored

//--------------------------------------------------------------------------------------------

//PD control parameters for left, and right motor

float Kp = 10; //proportional constant;

float Kd = 0.7; //derivative constant;

//for tuning PD parameters, we are going to keep Kd = 0 at frist, try Kp, and then keep Kp and try

different Kd;

//--------------------------------------------------------------------------------------------

//pin set up for motor controller

27

//Left motor (green block on motor driver)

int pinI1 = 5; //define I1 port, blue wire on ribbon cable

int pinI2 = 4; //define I2 port, green wire

int pinEA = 6; //define EA(PWM speed regulation)port, yellow wire

int encodPinA1 = 10; //define hall effect sensor A Vout;

int encodPinB1 = 3; //define hall effect sensor B Vout; digitalPinToInterrupt(pin 2) ;

//Right motor (red block on motor driver)

int pinI3 = 7;//define I3 port, orange wire

int pinI4 = 8;//define I4 port, red wire

int pinEB = 9;//define EB(PWM spped regulation) port, brown wire

int encodPinA2 = 11; //define hall effect sensor A Vout;

int encodPinB2 = 2; //define hall effect sensor B Vout; digitalPinToInterrupt(pin 3) ;

//--------------------------------------------------------------------------------------------

//speed constants for motor control

float act_speed[2] = {0, 0}; //actual speed;

int PWM_speed[2] = {0 ,0}; //speed converted to PWM value;

int last_error1=0;

int last_error2=0;

long prev_count[2] = {0, 0};

//--------------------------------------------------------------------------------------------

//rpm*2*pi(degree per rev)/60(seconds) = rad/s

//const float zero_speed = 5 * (2 * pi / 60);

//const float quarter_speed = 90 * (2 * pi / 60);

const float half_speed = 180 * (2 *pi / 60);

//const float three_quarter_speed = 270 * (2 *pi / 60);

const float full_speed = 360 * (2 * pi / 60);

const int CPR = 8; //counts per revolution

const int pause=100; //standize length of delays

//--------------------------------------------------------------------------------------------

//variables used for recieving and storing serial data

int cmd=110;

int prev_cmd=0;

//sensor pin

int dhtpin = 12;

//Initialize DHT sensor for normal 16mhz Arduino

DHT dht(dhtpin, DHTTYPE);

//--------------------------------------------------------------------------------------------

void setup()

{

Serial.begin (9600);//define baud rate at 9600 bps for serial comms

dht.begin(); //intialize temp and humidity sensor

//set motor controller pins as outputs

pinMode(pinI1,OUTPUT);




pinMode(pinEA,OUTPUT);

28

pinMode(pinEB,OUTPUT);

//enable encoder inputs.

pinMode(encodPinA1, INPUT);//sensor A1 input;

pinMode(encodPinB1, INPUT);//sensor B1 input;

pinMode(encodPinA2, INPUT);//sensor A2 input;

pinMode(encodPinB2, INPUT);//sensor B2 input;

//enable pullup resistor:

digitalWrite(encodPinA1, HIGH);

digitalWrite(encodPinB1, HIGH);

digitalWrite(encodPinA2, HIGH);

digitalWrite(encodPinB2, HIGH);

//--------------------------------------------------------------------------------------------

//two interrupters work together, each one is responsible for an encoder

attachInterrupt(digitalPinToInterrupt(3), rencoder1, FALLING); //system interrupter2

attachInterrupt(digitalPinToInterrupt(2), rencoder2, FALLING); //system interrupter3

//print start-up message

//Serial.println("Arduino Ready!");

}//end setup

//--------------------------------------------------------------------------------------------

//This is the main loop

void loop()

{

//Read and store input command

if (Serial.available()>0) {//if data is being received

prev_cmd=cmd; //store previous command in an int

cmd= Serial.read();//read in new byte and stores it as an int

prev_cmd_time = millis(); //store time command was received

}

//if time since last command is less than 3 seconds

if ((millis() - prev_cmd_time) < dog_time){

prev_time = curr_time; //previous time is equal to the time at the start of previous loop

curr_time = millis(); //current time is equal to the time at the start of current loop

delta_time = curr_time - prev_time; //delta time is the time difference of two connecting

loops

getSpeed(); //Obtain the speed of each motor shaft

switch (cmd){

//cmd 96 to 99 are trivial, because we do not have servo_control and temp sensor yet

case 96:

temp();

break;

case 100:

breaks();

break;

case 114:

PWM_speed[0] = int((act_speed[0] + control_effort1(half_speed, act_speed[0])) / full_speed

* 255);


* 255);//converts a float speed to a PWM value of 0~255

slow_CW(PWM_speed[0], PWM_speed[1]);

29

break;

case 101:

PWM_speed[0] = int((act_speed[0] + control_effort1(full_speed, act_speed[0])) / full_speed

* 255);



forward_right(PWM_speed[0], PWM_speed[1]);

break;

case 111:


* 255);



slow_forward(PWM_speed[0], PWM_speed[1]);

break;

case 110:


* 255);



forward_left(PWM_speed[0], PWM_speed[1]);

break;

case 105:


* 255);



slow_CCW(PWM_speed[0], PWM_speed[1]);

break;

case 112:


* 255);



backward_left(PWM_speed[0], PWM_speed[1]);

break;

case 113:


* 255);



slow_forward(PWM_speed[0], PWM_speed[1]);

break;

case 103:


* 255);



backward_right(PWM_speed[0], PWM_speed[1]);

break;

case 115:

30


* 255);



fast_CW(PWM_speed[0], PWM_speed[1]);

break;

case 102:


* 255);



fast_forward(PWM_speed[0], PWM_speed[1]);

break;

case 106:


* 255);



fast_CCW(PWM_speed[0], PWM_speed[1]);

break;

case 104:


* 255);



fast_backward(PWM_speed[0], PWM_speed[1]);

break;

default:

breaks();

}//end swich statement

}//end if statement

else{ //if no command has been received in the last 3 seconds

breaks(); //the rover will apply the breaks

}

}//end of main loop

//--------------------------------------------------------------------------------------------

// The interrupt routine - runs each time a falling edge of a pulse is detected

void rencoder1() { // pulse and direction, direct port reading

to save cycles

if (digitalRead(encodPinB1)==HIGH){

(count[0])++;} // if(digitalRead(encodPinA1)==HIGH) count

++;

else{

(count[0])--;} // if (digitalRead(encodPinA1)==LOW) count

--;

}

//--------------------------------------------------------------------------------------------

// The interrupt routine - runs each time a falling edge of a pulse is detected

void rencoder2() { // pulse and direction, direct port reading

to save cycles

if (digitalRead(encodPinB2)==HIGH){

31

(count[1])++;} // if(digitalRead(encodPinA2)==HIGH) count

++;

else{

(count[1])--;} // if (digitalRead(encodPinA2)==LOW) count

--;

}

//-----------------------------------------------------------------------------

//this function is actually a PD controller, it outputs control effort as a float type

float control_effort1(const int targetSpeed, int currentSpeed) {

float ctrl_effort1 = 0; // PID correction

int error1=0;

error1 = abs(targetSpeed) - abs(currentSpeed);

ctrl_effort1 = (Kp * error1) + (Kd * (error1 - last_error1));

last_error1 = error1;

return ctrl_effort1;

}

//-----------------------------------------------------------------------------

//this function is actually a PD controller, it outputs control effort as a float type

float control_effort2(const int targetSpeed, int currentSpeed) {

float ctrl_effort2 = 0; // PID correction

int error2=0;

error2 = abs(targetSpeed) - abs(currentSpeed);

ctrl_effort2 = (Kp * error2) + (Kd * (error2 - last_error2));

last_error2 = error2;

return ctrl_effort2;

}

//------------------------------------------------------------------

//this function calculates the

void getSpeed() { // calculate speed

// last counts

for (int i = 0; i < 2; i++){

act_speed[i] = (count[i] - prev_count[i]) / CPR * 2 * pi / delta_time; // delt counts / CPR *

2pi / delta time = rad/s

if(act_speed[i] > full_speed){ //speed saturation

act_speed[i] = full_speed;

}

//act_PWM[i] = constrain(act_spped[i], 0, 255); //converts actual speed to

prev_count[i] = count[i];

}

}

//--------------------------------------------------------------------------------------------

//This function reads in temperature and humidity data from a DHT22 sensor. It then prints

//the humidity, temperature in celcius and temperature in ferenheight.

void temp (){

// Reading temperature or humidity takes about 250 milliseconds!

// Sensor readings may also be up to 2 seconds 'old' (its a very slow sensor)

float h = dht.readHumidity();

// Read temperature as Celsius

float t = dht.readTemperature();

// Read temperature as Fahrenheit

32

float f = dht.readTemperature(true);

// Check if any reads failed and exit early (to try again).

if (isnan(h) || isnan(t) || isnan(f)) {

Serial.println("Failed to read from DHT sensor!");

return;

}

//print humidity

Serial.print(h);

Serial.print("%,");

//print temperature

Serial.print(t);

Serial.print("C,");

Serial.print(f);

Serial.println("F");

cmd=prev_cmd;//reset input command to previous command so motor control isn't interupted

}//end of function "temp"

//--------------------------------------------------------------------------------------------

//This function applies the breaks

void breaks (){

analogWrite(pinEA,0);//set motor A speed to zero

analogWrite(pinEB,0);//set motor B speed to zero

//set motor A

digitalWrite(pinI1,HIGH);// DC motor stop rotating

digitalWrite(pinI2,HIGH);

//set motor B

digitalWrite(pinI3,HIGH);// DC motor stop rotating

digitalWrite(pinI4,HIGH);

}//end of function "breaks"

//--------------------------------------------------------------------------------------------

//This function moves the rover slowly to the right

void slow_CW(int Aspeed, int Bspeed){

//set motor A

digitalWrite(pinI1, LOW); //CW rotation

digitalWrite(pinI2, HIGH);

analogWrite(pinEA, Aspeed);

//set motor B

digitalWrite(pinI3, HIGH); //CCW rotation

digitalWrite(pinI4, LOW);

analogWrite(pinEB, Bspeed);

}//end of function "slow_right"

//--------------------------------------------------------------------------------------------

//This function moves the rover slowly to the forward, right

void forward_right(int Aspeed, int Bspeed){

//set motor A




//set motor B

33




}//end of function "slow_forward_right"

//--------------------------------------------------------------------------------------------

//This function moves the rover slowly forward

void slow_forward(int Aspeed, int Bspeed){

//set motor A




//set motor B




}//end of function "slow_forward"

//--------------------------------------------------------------------------------------------

//This function moves the rover slowly to the forward, left

void forward_left(int Aspeed, int Bspeed){

//set motor A




//set motor B




}//end of function "slow_forward_left"

//--------------------------------------------------------------------------------------------

//This function moves the rover slowly to the left

void slow_CCW(int Aspeed, int Bspeed){

//set motor A




//set motor B




}//end of function "slow_left"

//--------------------------------------------------------------------------------------------

//This function moves the rover slowly to the backward, left

void backward_left(int Aspeed, int Bspeed){

//set motor A




//set motor B




}//end of function "slow_backward_left"

34

//--------------------------------------------------------------------------------------------

//This function moves the rover slowly backward

void slow_backward(int Aspeed, int Bspeed){

//set motor A




//set motor B




}//end of function "slow_backward"

//--------------------------------------------------------------------------------------------

//This function moves the rover slowly to the backward, right

void backward_right(int Aspeed, int Bspeed){

//set motor A




//set motor B




}//end of function "slow_backward_right"

//--------------------------------------------------------------------------------------------

//This function moves the rover quickly to the right

void fast_CW(int Aspeed, int Bspeed){

//set motor A




//set motor B




}//end of function "fast_right"

//--------------------------------------------------------------------------------------------

//This function moves the rover quickly forward

void fast_forward(int Aspeed, int Bspeed){

//set motor A




//set motor B




}//end of function "fast_forward"

//--------------------------------------------------------------------------------------------

//This function moves the rover quickly to the left

void fast_CCW(int Aspeed, int Bspeed){

//set motor A

35




//set motor B




}//end of function "fast_left"

//--------------------------------------------------------------------------------------------

//This function moves the rover quickly backward

void fast_backward(int Aspeed, int Bspeed){

//set motor A




//set motor B




}//end of function "fast_backward"

//--------------------------------------------------------------------------------------------

2. Raspberry Pi Code

//--------------------------------------------------------------------------------------- This program is designed to read inputs form a gamepad and send commands to the rover. While it has the ability to read all gamepad commands, currently only the left joystick, directional pad, and a few buttons are used. This program was designed to be run on a Raspberry Pi running the Raspbian OS and read commands off of a Logitech Gamepad F310. */ //-------------------------------------------------------------------------------------------- //Include neccesary libraries #include <SDL2/SDL.h> //window creation and gamepad input reading features #include <SDL2/SDL_image.h> //image loading and rendering features #include <stdio.h> //print text to console #include <string> //to hadle strings #include <cmath> //basic math function like inverse tangent #include <iostream> //print to console #include <stdlib.h> //sending system commands in Linux #include "wiringPi.h" //send commands over USB on raspberry pi #include "wiringSerial.h" //-------------------------------------------------------------------------------------------- //Function Declarations

36

bool init(); //Initialization routine run on start up void close(); //Close routine run on shut down char eventHandler(SDL_Event input, char cmd, bool &quit); //handles events, ya' dummy char motorControl(char cmd); //convert joystick coordinates into motor commands void sendcmd(char cmd); //send commands over usb or prints commands to console //-------------------------------------------------------------------------------------------- //Global variables SDL_GameController* GameController = NULL; //Game Controller handler //SDL_GameController* GameController1 = NULL; //Game Controller hand$ //These global strings can be helpful when dubugging, but are kept commented out for production. //std::string command = ""; //std::string prev_command = ""; //These variables are needed to set up serial communication over USB //Look up wireing pi library for more detials char device [] = "/dev/ttyACM0"; //Name or Arduino's usb port char device1 [] = "/dev/ttyACM1"; int fd; unsigned long baud = 9600; //-------------------------------------------------------------------------------------------- //This function runs once at start up. It initializes the gamepad and serial communications //with the Arduino. It returns success = true if everything initialzes correctly. It prints //error messages and returns success = false if something fails to initialize. Success = false //will cause the program to quit in main loop. bool init() { //Initialization flag bool success = true; //Initialize SDL if( SDL_Init( SDL_INIT_GAMECONTROLLER | SDL_INIT_TIMER ) < 0 ) { printf( "SDL could not initialize! SDL Error: %s\n", SDL_GetError() ); success = false; } else //if SDL initialized succesfully {// SDL_JoystickEventState(SDL_ENABLE); //Load gamepad GameController = SDL_GameControllerOpen( 0 ); //GameController1 = SDL_GameControllerOpen( 1 ); if(( GameController == NULL)/* || (GameController1 == NULL )*/ ) { printf( "Warning: Unable to open game controller! SDL Error: %s\n", SDL_GetError() ); success = false; } }

37

std::cout<<"Initialized!\n"; //print initialization message to console fflush(stdout); //clear serial data lines (just in case!) //open serial device (Arduino) if ((fd = serialOpen (device, baud)) < 0) { if((fd = serialOpen (device1, baud)) < 0) { printf ("Unable to open serail device\n"); success = false; } } //setup serial comms if (wiringPiSetup () == -1) { printf ("Unable to start wiringPi\n"); success = false; } //Determine number of available joysticks //printf("%i joysticks were found.\n\n", SDL_NumJoysticks() ); //printf("Names: %s \n",SDL_JoystickName(0) , SDL_JoystickName(1)); return success; } //End function "Init" //-------------------------------------------------------------------------------------------- //This function runs once at shut down. It deallocates memory and shuts down the program // properly. void close() { //Close game controller SDL_GameControllerClose( GameController ); GameController = NULL; //close serial comms serialClose(fd); //Quit SDL subsystems SDL_Quit(); } //End function "close" //-------------------------------------------------------------------------------------------- //This funciton determines the event type and then sends the proper command over usb to the //Arduino Micro onboard the rover. //For more info, look up the SDL_Event's "caxis" and "cbutton". /* Note: It is helpful to uncomment the "command = ..." lines when debugging. These commands can then be printed in the sendcmd function to verify code. "cmd = ..." lines should not be uncommented unless the Arduino code is updated to include these added commands in it decision logic. */

38

char eventHandler(SDL_Event input, char cmd, bool &quit) { //If user requests quit if( input.type == SDL_QUIT ) { quit = true; } //If input is of Controller axis motion type //This includes the two joysticks and the two triggers else if( input.type == SDL_CONTROLLERAXISMOTION ) { if (input.caxis.axis == SDL_CONTROLLER_AXIS_TRIGGERLEFT) { //command = "Left Trigger"; cmd = 83; } else if (input.caxis.axis == SDL_CONTROLLER_AXIS_TRIGGERRIGHT) { //command = "Right Trigger"; cmd = 84; } else { cmd = motorControl(cmd); //this function will deal with joystick motion and return cmd value } } //If a controller button has been pushed down else if( input.type == SDL_CONTROLLERBUTTONDOWN ) { switch(input.cbutton.button){ case SDL_CONTROLLER_BUTTON_A: //play live video-feed on UI screen using mplayer //command = "Button A"; cmd = 93; system("mplayer tv: -tv driver=v4l2:norm=PAL-M:device=/dev/video0:fps=1:noaudio:outfmt=uyvy:fps=10 -vo sdl -fs -vf format=y8,scale -hardframedrop &"); system("/home/pi/mediaplayer/run.sh"); break; case SDL_CONTROLLER_BUTTON_B: //close video-feed //command = "Button B"; cmd = 94; system("pkill mplayer"); break; case SDL_CONTROLLER_BUTTON_X: //command = "Button X"; cmd = 95; system("mplayer tv: -tv driver=v4l2:norm=PAL-M:device=/dev/video0:fps=1:noaudio:outfmt=uyvy:fps=10 -vo jpeg -frames 3 -fs -vf format=y8,scale -hardframedrop &"); break; case SDL_CONTROLLER_BUTTON_Y: //request temp and humidity data from Arduino

39

//command = "Button Y"; cmd = 96; break; case SDL_CONTROLLER_BUTTON_BACK: //shutdown raspberry pi //command = "Back"; cmd = 88; system("sudo shutdown -h now"); break; case SDL_CONTROLLER_BUTTON_GUIDE: //quit program //command = "Guide"; quit = true; system("sudo shutdown -h now"); break; case SDL_CONTROLLER_BUTTON_START: //command = "Start"; cmd = 89; break; case SDL_CONTROLLER_BUTTON_LEFTSTICK: //command = "Left Stick"; cmd = 90; break; case SDL_CONTROLLER_BUTTON_RIGHTSTICK: //command = "Right Stick"; cmd = 91; break; case SDL_CONTROLLER_BUTTON_LEFTSHOULDER: //command = "Left Shoulder"; cmd = 85; break; case SDL_CONTROLLER_BUTTON_RIGHTSHOULDER: //command = "Right Shoulder"; //cmd = 86; break; case SDL_CONTROLLER_BUTTON_DPAD_UP: //center camera //command = "Dpad Up"; cmd = 99; break; case SDL_CONTROLLER_BUTTON_DPAD_DOWN: //command = "Dpad Down"; cmd = 87; break; case SDL_CONTROLLER_BUTTON_DPAD_LEFT: //pan camera left //command = "Dpad Left"; cmd = 98; break; case SDL_CONTROLLER_BUTTON_DPAD_RIGHT: //pan camera right //command = "Dpad Right"; cmd = 97; break; } } printf ("CMD: %d\n",cmd);

40

return (cmd); } //End of function "eventHandler" //-------------------------------------------------------------------------------------------- //This function converts joystick coordinates into motor commands and sends them over usb. char motorControl(char cmd) { SDL_JoystickEventState(SDL_ENABLE); const double min = 2500; //deadzone const double mid = 20700; //line between fast a slow speed //read in x and y coordinates. Flip y coordinate to make more sense //Left Stick double yCorL = -1*SDL_GameControllerGetAxis(GameController, SDL_CONTROLLER_AXIS_LEFTY); //double xCorL = SDL_GameControllerGetAxis(GameController, SDL_CONTROLLER_AXIS_LEFTX); //Right stick double yCorR = -1*SDL_GameControllerGetAxis(GameController, SDL_CONTROLLER_AXIS_RIGHTY); //double xCorR = SDL_GameControllerGetAxis(GameController, SDL_CONTROLLER_AXIS_RIGHTX); //Variable for joystick position, up or down //Left Stick double joystickPosL = 0; //Right stick double joystickPosR = 0; //determine joystick up or down //this logic converts joystick position to be 1 for up -1 for down //Left stick //Up Position if (yCorL > min ) { joystickPosL = 1; } //Down Position else if (yCorL < -min ) { joystickPosL = -1; } else{ joystickPosL=0; } //Right stick //Up Position if (yCorR > min ) { joystickPosR = 1; } //Down Position else if (yCorR < -min ) {

41

joystickPosR = -1; } else { joystickPosR = 0; } //calculate joystick magnitude double joystickMagL = sqrt(/* (xCorL * xCorL) + (*/yCorL * yCorL)/*)*/; double joystickMagR = sqrt(/* (xCorR * xCorR) + (*/yCorR * yCorR)/*)*/; //For debugging printf ("PosL: %f\n", joystickPosL); printf ("MagL: %f\n", joystickMagL); printf ("PosR: %f\n", joystickPosR); printf ("MagR: %f\n", joystickMagR); //Finally, joystick angle and magnitude is used to determine the direction //and speed the rover should move in. if ( (joystickPosL || joystickPosR) == 0 ) { //command = "Break"; cmd = 100; } //Fwd commands else if ( (joystickPosL > 0) && (joystickPosR > 0)) { if (( joystickMagL < mid )&& ( joystickMagR < mid )) { //command = "Slow Fwd"; cmd = 111; } else if (( joystickMagL < mid )&& ( joystickMagR > mid )) { //command = "Left Fwd"; cmd = 110; } else if (( joystickMagL > mid )&& ( joystickMagR < mid )) { //command = "Right Fwd"; cmd = 101; } else { //command = "Fast Fwd"; cmd = 102; } } //Bwd commands else if ( (joystickPosL < 0) && (joystickPosR < 0)) { if (( joystickMagL < mid )&& ( joystickMagR < mid )) {

42

//command = "Slow Bwd"; cmd = 113; } else if (( joystickMagL < mid )&& ( joystickMagR > mid )) { //command = "Left Bwd"; cmd = 112; } else if (( joystickMagL > mid )&& ( joystickMagR < mid )) { //command = "Right Bwd"; cmd = 103; } else { //command = "Fast Bwd"; cmd = 104; } } else if ( (joystickPosL < 0) && (joystickPosR > 0)) { if (( joystickMagL > mid )&& ( joystickMagR > mid )) { //command = "Fast Counter Clockwise Rotation"; cmd = 106; } else { //command = "Slow Counter Clockwise Rotation "; cmd = 105; } } else if ( (joystickPosL > 0) && (joystickPosR < 0)) { if (( joystickMagL > mid )&& ( joystickMagR > mid )) { //command = "Fast Clockwise Rotation"; cmd = 115; } else { //command = "Slow Clockwise Rotation "; cmd = 114; } } return cmd; } //End of funciton "motorControl" //-------------------------------------------------------------------------------------------- //This function sends characters over usb using the wireing pi libraries. It also reads in //temp and humidty data. void sendcmd(char cmd) { if (cmd == 96) //request temp and humidty data from rover { char humidity [10];

43

char celsius [10]; char fahrenheit [10]; char receivedChar = 0; int i = 0; int j = 0; int k = 0; serialFlush(fd); //clear serail lines serialPutchar (fd, cmd); //send command while ( serialDataAvail(fd) < 1 ) //delay until arduino sends data { SDL_Delay(1); } receivedChar = serialGetchar(fd); //read in first chracter do { humidity[i] = receivedChar; //store character as humidity data i++; receivedChar = serialGetchar(fd); } while ( receivedChar != ','); //store characters until , is reached receivedChar = serialGetchar(fd); //begin reading temperature data do { celsius[j] = receivedChar; j++; receivedChar = serialGetchar(fd); } while ( receivedChar != ','); receivedChar = serialGetchar(fd); do { fahrenheit[k] = receivedChar; k++; receivedChar = serialGetchar(fd); } while ( receivedChar != ','); //print data to the console std::cout<<"Humidity: "; for (int lcv = 0; lcv < i; lcv++) { std::cout<<humidity[lcv]; } std::cout<<"\n"<<"Celsius: "; for (int lcv = 0; lcv < j; lcv++) { std::cout<<celsius[lcv]; }

44

std::cout<<"\n"<<"Fahrenheit: "; for (int lcv = 0; lcv < k; lcv++) { std::cout<<fahrenheit[lcv]; } std::cout<<"\n"; } else //send command to rover and move on, if we are not expecting data in reply { serialPutchar (fd, cmd); } //Useful for debugging //std::cout<<command<<"\n"; //printf("%c\n", cmd); } //End of function "sendcmd" //-------------------------------------------------------------------------------------------- //main control logic loop int main( int argc, char* args[] ) { //Run initialization function and check for success if( !init() ) { printf( "Failed to initialize!\n" ); } else //if initialization is successful { //displaying camera feed //system("mplayer tv:// -tv driver=v4l2:norm=PAL-M:device=/dev/video0 -vo sdl -framedrop"); bool quit = false; //flag to exit main loop SDL_Event input; //store input commands char cmd = 0; //commands to be sent to arduino char prev_cmd = 0; //store old commands int WatchDog = SDL_GetTicks() + 300; //watchdog timer //While user hasn't quit while( !quit ) { while( SDL_PollEvent( &input ) != 0 ) //poll gamepad, store events in input variable { cmd = eventHandler( input, cmd, quit); //determine event type and corresponding cmd to send if (cmd != 0) { //limit number of commands we send for efficiency and power savings if ( (cmd != prev_cmd) || (SDL_TICKS_PASSED(SDL_GetTicks(), WatchDog)))

45

{ sendcmd(cmd); WatchDog = SDL_GetTicks() + 300; //reset watchdog timer //prev_command = command; prev_cmd = cmd; } } //command = ""; cmd = 0; }//end while input event present }//end while not quite } //For Debugging //SDL_Delay(4000); //Free resources and close SDL close(); //system("sudo shutdown -h now"); return 0; } //End "main" //--------------------------------------------------------------------------------------------

xsunroboticdesignpaper

Documents