1. 1
Gopher Tortoise Scope Redesign
Xiangzhen Sun1
Itiel Agramonte2
Submitted On:
December 11, 2015
EML5930 – Mechatronics II
Dr. Clark
Fall 2015
2. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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(pinI2,OUTPUT);
pinMode(pinI3,OUTPUT);
pinMode(pinI4,OUTPUT);
pinMode(pinEA,OUTPUT);
28. 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);
PWM_speed[1] = int((act_speed[1] + control_effort2(half_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~255
slow_CW(PWM_speed[0], PWM_speed[1]);
29. 29
break;
case 101:
PWM_speed[0] = int((act_speed[0] + control_effort1(full_speed, act_speed[0])) / full_speed
* 255);
PWM_speed[1] = int((act_speed[1] + control_effort2(half_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~255
forward_right(PWM_speed[0], PWM_speed[1]);
break;
case 111:
PWM_speed[0] = int((act_speed[0] + control_effort1(half_speed, act_speed[0])) / full_speed
* 255);
PWM_speed[1] = int((act_speed[1] + control_effort2(half_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~255
slow_forward(PWM_speed[0], PWM_speed[1]);
break;
case 110:
PWM_speed[0] = int((act_speed[0] + control_effort1(half_speed, act_speed[0])) / full_speed
* 255);
PWM_speed[1] = int((act_speed[1] + control_effort2(full_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~255
forward_left(PWM_speed[0], PWM_speed[1]);
break;
case 105:
PWM_speed[0] = int((act_speed[0] + control_effort1(half_speed, act_speed[0])) / full_speed
* 255);
PWM_speed[1] = int((act_speed[1] + control_effort2(half_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~255
slow_CCW(PWM_speed[0], PWM_speed[1]);
break;
case 112:
PWM_speed[0] = int((act_speed[0] + control_effort1(half_speed, act_speed[0])) / full_speed
* 255);
PWM_speed[1] = int((act_speed[1] + control_effort2(full_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~255
backward_left(PWM_speed[0], PWM_speed[1]);
break;
case 113:
PWM_speed[0] = int((act_speed[0] + control_effort1(half_speed, act_speed[0])) / full_speed
* 255);
PWM_speed[1] = int((act_speed[1] + control_effort2(half_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~255
slow_forward(PWM_speed[0], PWM_speed[1]);
break;
case 103:
PWM_speed[0] = int((act_speed[0] + control_effort1(full_speed, act_speed[0])) / full_speed
* 255);
PWM_speed[1] = int((act_speed[1] + control_effort2(half_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~255
backward_right(PWM_speed[0], PWM_speed[1]);
break;
case 115:
30. 30
PWM_speed[0] = int((act_speed[0] + control_effort1(full_speed, act_speed[0])) / full_speed
* 255);
PWM_speed[1] = int((act_speed[1] + control_effort2(full_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~255
fast_CW(PWM_speed[0], PWM_speed[1]);
break;
case 102:
PWM_speed[0] = int((act_speed[0] + control_effort1(full_speed, act_speed[0])) / full_speed
* 255);
PWM_speed[1] = int((act_speed[1] + control_effort2(full_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~255
fast_forward(PWM_speed[0], PWM_speed[1]);
break;
case 106:
PWM_speed[0] = int((act_speed[0] + control_effort1(full_speed, act_speed[0])) / full_speed
* 255);
PWM_speed[1] = int((act_speed[1] + control_effort2(full_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~255
fast_CCW(PWM_speed[0], PWM_speed[1]);
break;
case 104:
PWM_speed[0] = int((act_speed[0] + control_effort1(full_speed, act_speed[0])) / full_speed
* 255);
PWM_speed[1] = int((act_speed[1] + control_effort2(full_speed, act_speed[1])) / full_speed
* 255);//converts a float speed to a PWM value of 0~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. 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. 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
digitalWrite(pinI1, LOW); //CW rotation
digitalWrite(pinI2, HIGH);
analogWrite(pinEA, Aspeed);
//set motor B
33. 33
digitalWrite(pinI3, LOW); //CW rotation
digitalWrite(pinI4, HIGH);
analogWrite(pinEB, Bspeed);
}//end of function "slow_forward_right"
//--------------------------------------------------------------------------------------------
//This function moves the rover slowly forward
void slow_forward(int Aspeed, int Bspeed){
//set motor A
digitalWrite(pinI1, LOW); //CW rotation
digitalWrite(pinI2, HIGH);
analogWrite(pinEA, Aspeed);
//set motor B
digitalWrite(pinI3, LOW); //CW rotation
digitalWrite(pinI4, HIGH);
analogWrite(pinEB, Bspeed);
}//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
digitalWrite(pinI1, LOW); //CW rotation
digitalWrite(pinI2, HIGH);
analogWrite(pinEA, Aspeed);
//set motor B
digitalWrite(pinI3, LOW); //CW rotation
digitalWrite(pinI4, HIGH);
analogWrite(pinEB, Bspeed);
}//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
digitalWrite(pinI1, HIGH); //CCW rotation
digitalWrite(pinI2, LOW);
analogWrite(pinEA, Aspeed);
//set motor B
digitalWrite(pinI3, LOW); //CW rotation
digitalWrite(pinI4, HIGH);
analogWrite(pinEB, Bspeed);
}//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
digitalWrite(pinI1, HIGH); //CCW rotation
digitalWrite(pinI2, LOW);
analogWrite(pinEA, Aspeed);
//set motor B
digitalWrite(pinI3, HIGH); //CCW rotation
digitalWrite(pinI4, LOW);
analogWrite(pinEB, Bspeed);
}//end of function "slow_backward_left"
34. 34
//--------------------------------------------------------------------------------------------
//This function moves the rover slowly backward
void slow_backward(int Aspeed, int Bspeed){
//set motor A
digitalWrite(pinI1, HIGH); //CCW rotation
digitalWrite(pinI2, LOW);
analogWrite(pinEA, Aspeed);
//set motor B
digitalWrite(pinI3, HIGH); //CCW rotation
digitalWrite(pinI4, LOW);
analogWrite(pinEB, Bspeed);
}//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
digitalWrite(pinI1, HIGH); //CCW rotation
digitalWrite(pinI2, LOW);
analogWrite(pinEA, Aspeed);
//set motor B
digitalWrite(pinI3, HIGH); //CCW rotation
digitalWrite(pinI4, LOW);
analogWrite(pinEB, Bspeed);
}//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
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 "fast_right"
//--------------------------------------------------------------------------------------------
//This function moves the rover quickly forward
void fast_forward(int Aspeed, int Bspeed){
//set motor A
digitalWrite(pinI1, LOW); //CW rotation
digitalWrite(pinI2, HIGH);
analogWrite(pinEA, Aspeed);
//set motor B
digitalWrite(pinI3, LOW); //CW rotation
digitalWrite(pinI4, HIGH);
analogWrite(pinEB, Bspeed);
}//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. 35
digitalWrite(pinI1, HIGH); //CCW rotation
digitalWrite(pinI2, LOW);
analogWrite(pinEA, Aspeed);
//set motor B
digitalWrite(pinI3, LOW); //CW rotation
digitalWrite(pinI4, HIGH);
analogWrite(pinEB, Bspeed);
}//end of function "fast_left"
//--------------------------------------------------------------------------------------------
//This function moves the rover quickly backward
void fast_backward(int Aspeed, int Bspeed){
//set motor A
digitalWrite(pinI1, HIGH); //CCW rotation
digitalWrite(pinI2, LOW);
analogWrite(pinEA, Aspeed);
//set motor B
digitalWrite(pinI3, HIGH); //CCW rotation
digitalWrite(pinI4, LOW);
analogWrite(pinEB, Bspeed);
}//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. 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: %sn", 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: %sn",
SDL_GetError() );
success = false;
}
}
37. 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 devicen");
success = false;
}
}
//setup serial comms
if (wiringPiSetup () == -1)
{
printf ("Unable to start wiringPin");
success = false;
}
//Determine number of available joysticks
//printf("%i joysticks were found.nn", 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. 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. 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: %dn",cmd);
40. 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 )
{
42. 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. 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. 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("%cn", 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)))
