Showing preview only (640K chars total). Download the full file or copy to clipboard to get everything.
Repository: alexbaucom17/DominoRobot
Branch: master
Commit: 4f7273faa068
Files: 159
Total size: 596.1 KB
Directory structure:
gitextract_eu8r6t07/
├── .gitignore
├── LICENSE
├── README.md
├── doc/
│ ├── DominoRobotControllerFlowchart.drawio
│ ├── DominoRobotSoftwareArchitecture.drawio
│ ├── KukaPsuedoCode.txt
│ ├── MassAndPowerCalcs.xlsx
│ ├── NetworkPrototcols.txt
│ ├── SpeedConversions.ods
│ ├── SubsystemDiagram.drawio
│ ├── SwClassHeirarchy.txt
│ └── clearcore_api.txt
├── experimental_testing/
│ ├── ImageParser.py
│ ├── RobotArduinoTesting/
│ │ ├── StepperDriverMotion_4Motor_Accel/
│ │ │ └── StepperDriverMotion_4Motor_Accel.ino
│ │ ├── TestMotor/
│ │ │ └── TestMotor.ino
│ │ ├── WifiServerWithESP/
│ │ │ ├── RobotServer.cpp
│ │ │ ├── RobotServer.h
│ │ │ └── WifiServerWithESP.ino
│ │ ├── WifiTesting/
│ │ │ └── WifiTesting.ino
│ │ ├── all_motors/
│ │ │ └── all_motors.ino
│ │ ├── all_motors_speed_control/
│ │ │ ├── Motor.cpp
│ │ │ ├── Motor.h
│ │ │ └── all_motors_speed_control.ino
│ │ ├── kalman_test/
│ │ │ ├── KalmanFilter.cpp
│ │ │ ├── KalmanFilter.h
│ │ │ └── kalman_test.ino
│ │ ├── lifter_nano_test/
│ │ │ └── lifter_nano_test.ino
│ │ ├── motor_driver_test_script/
│ │ │ └── motor_driver_test_script.ino
│ │ ├── new_dc_motor_tuning/
│ │ │ ├── Motor.cpp
│ │ │ ├── Motor.h
│ │ │ ├── constants.h
│ │ │ └── new_dc_motor_tuning.ino
│ │ ├── single_motor_speed_control/
│ │ │ ├── Motor.cpp
│ │ │ ├── Motor.h
│ │ │ └── single_motor_speed_control.ino
│ │ ├── stepper_nano_test/
│ │ │ └── stepper_nano_test.ino
│ │ └── test_encoders/
│ │ └── test_encoders.ino
│ ├── TrajGen.py
│ └── TrajGenv2.py
└── src/
├── master/
│ ├── DominoDesign-Questions.psd
│ ├── DominoDesign.psd
│ ├── FieldPlanner.py
│ ├── MarvelMindHandler.py
│ ├── MasterMain.py
│ ├── README.md
│ ├── RobotClient.py
│ ├── Runtime.py
│ ├── Utils.py
│ ├── __init__.py
│ ├── camera_utils.py
│ ├── config.py
│ ├── plans/
│ │ ├── AccuracyTesting_1x3_withdistance.p
│ │ ├── AccuracyTesting_1x3_withdistance086.p
│ │ ├── AccuracyTesting_2x2.p
│ │ ├── AccuracyTesting_3x3_withdistance_2axis.p
│ │ ├── DockingTest1.p
│ │ ├── DockingTest2.p
│ │ ├── FullPlan_DominoBros.p
│ │ ├── FullTest1.p
│ │ ├── HackyTestPlan.p
│ │ ├── LargeScale1_5x5.p
│ │ ├── LargeScale2_5x5.p
│ │ ├── VisionAccuracyTesting_2x2_SmallTestArea.p
│ │ ├── autosaved.p
│ │ ├── plan.p
│ │ ├── plan2.p
│ │ ├── vision_offsets small_testing_area.csv
│ │ └── vision_offsets_full_plan.csv
│ ├── plot_logs2.py
│ └── requirements.txt
├── robot/
│ ├── Makefile
│ ├── README.md
│ ├── src/
│ │ ├── KalmanFilter.cpp
│ │ ├── KalmanFilter.h
│ │ ├── Localization.cpp
│ │ ├── Localization.h
│ │ ├── MarvelmindWrapper.cpp
│ │ ├── MarvelmindWrapper.h
│ │ ├── RobotController.cpp
│ │ ├── RobotController.h
│ │ ├── RobotServer.cpp
│ │ ├── RobotServer.h
│ │ ├── SmoothTrajectoryGenerator.cpp
│ │ ├── SmoothTrajectoryGenerator.h
│ │ ├── StatusUpdater.cpp
│ │ ├── StatusUpdater.h
│ │ ├── TrayController.cpp
│ │ ├── TrayController.h
│ │ ├── camera_tracker/
│ │ │ ├── CameraPipeline.cpp
│ │ │ ├── CameraPipeline.h
│ │ │ ├── CameraTracker.cpp
│ │ │ ├── CameraTracker.h
│ │ │ ├── CameraTrackerBase.h
│ │ │ ├── CameraTrackerFactory.cpp
│ │ │ ├── CameraTrackerFactory.h
│ │ │ └── CameraTrackerMock.h
│ │ ├── constants.cfg
│ │ ├── constants.h
│ │ ├── main.cpp
│ │ ├── robot.cpp
│ │ ├── robot.h
│ │ ├── robot_controller_modes/
│ │ │ ├── RobotControllerModeBase.cpp
│ │ │ ├── RobotControllerModeBase.h
│ │ │ ├── RobotControllerModePosition.cpp
│ │ │ ├── RobotControllerModePosition.h
│ │ │ ├── RobotControllerModeStopFast.cpp
│ │ │ ├── RobotControllerModeStopFast.h
│ │ │ ├── RobotControllerModeVision.cpp
│ │ │ └── RobotControllerModeVision.h
│ │ ├── serial/
│ │ │ ├── MockSerialComms.cpp
│ │ │ ├── MockSerialComms.h
│ │ │ ├── SerialComms.cpp
│ │ │ ├── SerialComms.h
│ │ │ ├── SerialCommsBase.cpp
│ │ │ ├── SerialCommsBase.h
│ │ │ ├── SerialCommsFactory.cpp
│ │ │ └── SerialCommsFactory.h
│ │ ├── sockets/
│ │ │ ├── ClientSocket.cpp
│ │ │ ├── ClientSocket.h
│ │ │ ├── MockSocketMultiThreadWrapper.cpp
│ │ │ ├── MockSocketMultiThreadWrapper.h
│ │ │ ├── README.md
│ │ │ ├── ServerSocket.cpp
│ │ │ ├── ServerSocket.h
│ │ │ ├── Socket.cpp
│ │ │ ├── Socket.h
│ │ │ ├── SocketException.h
│ │ │ ├── SocketMultiThreadWrapper.cpp
│ │ │ ├── SocketMultiThreadWrapper.h
│ │ │ ├── SocketMultiThreadWrapperBase.h
│ │ │ ├── SocketMultiThreadWrapperFactory.cpp
│ │ │ ├── SocketMultiThreadWrapperFactory.h
│ │ │ └── SocketTimeoutException.h
│ │ ├── utils.cpp
│ │ └── utils.h
│ └── test/
│ ├── CameraPipeline_Test.cpp
│ ├── CameraTracker_Test.cpp
│ ├── Localization_Test.cpp
│ ├── RobotController_Test.cpp
│ ├── RobotServer_Test.cpp
│ ├── Robot_Test.cpp
│ ├── SanityCheckTest.cpp
│ ├── SmoothTrajectoryGenerator_Test.cpp
│ ├── SocketWrapper_Test.cpp
│ ├── StatusUpdater_Test.cpp
│ ├── TrayController_Test.cpp
│ ├── test-main.cpp
│ ├── test-utils.h
│ ├── test_constants.cfg
│ └── utils_Test.cpp
├── robot_motor_driver/
│ ├── SerialComms.cpp
│ ├── SerialComms.h
│ └── robot_motor_driver.ino
└── tools/
├── 80-domino-bot.rules
├── IR_calibration_1.yml
├── IR_calibration_2.yml
├── camera_calibration.py
├── motor_test_script.py
└── plot_logs.py
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
.VSCodeCounter/
beacon_state.txt
build/
lib/
.vscode/
scratch
log/
__pycache__
*.so
marvelmind_SW*
*previous_plan_state.json
venv/
================================================
FILE: LICENSE
================================================
GNU GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The GNU General Public License is a free, copyleft license for
software and other kinds of works.
The licenses for most software and other practical works are designed
to take away your freedom to share and change the works. By contrast,
the GNU General Public License is intended to guarantee your freedom to
share and change all versions of a program--to make sure it remains free
software for all its users. We, the Free Software Foundation, use the
GNU General Public License for most of our software; it applies also to
any other work released this way by its authors. You can apply it to
your programs, too.
When we speak of free software, we are referring to freedom, not
price. Our General Public Licenses are designed to make sure that you
have the freedom to distribute copies of free software (and charge for
them if you wish), that you receive source code or can get it if you
want it, that you can change the software or use pieces of it in new
free programs, and that you know you can do these things.
To protect your rights, we need to prevent others from denying you
these rights or asking you to surrender the rights. Therefore, you have
certain responsibilities if you distribute copies of the software, or if
you modify it: responsibilities to respect the freedom of others.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must pass on to the recipients the same
freedoms that you received. You must make sure that they, too, receive
or can get the source code. And you must show them these terms so they
know their rights.
Developers that use the GNU GPL protect your rights with two steps:
(1) assert copyright on the software, and (2) offer you this License
giving you legal permission to copy, distribute and/or modify it.
For the developers' and authors' protection, the GPL clearly explains
that there is no warranty for this free software. For both users' and
authors' sake, the GPL requires that modified versions be marked as
changed, so that their problems will not be attributed erroneously to
authors of previous versions.
Some devices are designed to deny users access to install or run
modified versions of the software inside them, although the manufacturer
can do so. This is fundamentally incompatible with the aim of
protecting users' freedom to change the software. The systematic
pattern of such abuse occurs in the area of products for individuals to
use, which is precisely where it is most unacceptable. Therefore, we
have designed this version of the GPL to prohibit the practice for those
products. If such problems arise substantially in other domains, we
stand ready to extend this provision to those domains in future versions
of the GPL, as needed to protect the freedom of users.
Finally, every program is threatened constantly by software patents.
States should not allow patents to restrict development and use of
software on general-purpose computers, but in those that do, we wish to
avoid the special danger that patents applied to a free program could
make it effectively proprietary. To prevent this, the GPL assures that
patents cannot be used to render the program non-free.
The precise terms and conditions for copying, distribution and
modification follow.
TERMS AND CONDITIONS
0. Definitions.
"This License" refers to version 3 of the GNU General Public License.
"Copyright" also means copyright-like laws that apply to other kinds of
works, such as semiconductor masks.
"The Program" refers to any copyrightable work licensed under this
License. Each licensee is addressed as "you". "Licensees" and
"recipients" may be individuals or organizations.
To "modify" a work means to copy from or adapt all or part of the work
in a fashion requiring copyright permission, other than the making of an
exact copy. The resulting work is called a "modified version" of the
earlier work or a work "based on" the earlier work.
A "covered work" means either the unmodified Program or a work based
on the Program.
To "propagate" a work means to do anything with it that, without
permission, would make you directly or secondarily liable for
infringement under applicable copyright law, except executing it on a
computer or modifying a private copy. Propagation includes copying,
distribution (with or without modification), making available to the
public, and in some countries other activities as well.
To "convey" a work means any kind of propagation that enables other
parties to make or receive copies. Mere interaction with a user through
a computer network, with no transfer of a copy, is not conveying.
An interactive user interface displays "Appropriate Legal Notices"
to the extent that it includes a convenient and prominently visible
feature that (1) displays an appropriate copyright notice, and (2)
tells the user that there is no warranty for the work (except to the
extent that warranties are provided), that licensees may convey the
work under this License, and how to view a copy of this License. If
the interface presents a list of user commands or options, such as a
menu, a prominent item in the list meets this criterion.
1. Source Code.
The "source code" for a work means the preferred form of the work
for making modifications to it. "Object code" means any non-source
form of a work.
A "Standard Interface" means an interface that either is an official
standard defined by a recognized standards body, or, in the case of
interfaces specified for a particular programming language, one that
is widely used among developers working in that language.
The "System Libraries" of an executable work include anything, other
than the work as a whole, that (a) is included in the normal form of
packaging a Major Component, but which is not part of that Major
Component, and (b) serves only to enable use of the work with that
Major Component, or to implement a Standard Interface for which an
implementation is available to the public in source code form. A
"Major Component", in this context, means a major essential component
(kernel, window system, and so on) of the specific operating system
(if any) on which the executable work runs, or a compiler used to
produce the work, or an object code interpreter used to run it.
The "Corresponding Source" for a work in object code form means all
the source code needed to generate, install, and (for an executable
work) run the object code and to modify the work, including scripts to
control those activities. However, it does not include the work's
System Libraries, or general-purpose tools or generally available free
programs which are used unmodified in performing those activities but
which are not part of the work. For example, Corresponding Source
includes interface definition files associated with source files for
the work, and the source code for shared libraries and dynamically
linked subprograms that the work is specifically designed to require,
such as by intimate data communication or control flow between those
subprograms and other parts of the work.
The Corresponding Source need not include anything that users
can regenerate automatically from other parts of the Corresponding
Source.
The Corresponding Source for a work in source code form is that
same work.
2. Basic Permissions.
All rights granted under this License are granted for the term of
copyright on the Program, and are irrevocable provided the stated
conditions are met. This License explicitly affirms your unlimited
permission to run the unmodified Program. The output from running a
covered work is covered by this License only if the output, given its
content, constitutes a covered work. This License acknowledges your
rights of fair use or other equivalent, as provided by copyright law.
You may make, run and propagate covered works that you do not
convey, without conditions so long as your license otherwise remains
in force. You may convey covered works to others for the sole purpose
of having them make modifications exclusively for you, or provide you
with facilities for running those works, provided that you comply with
the terms of this License in conveying all material for which you do
not control copyright. Those thus making or running the covered works
for you must do so exclusively on your behalf, under your direction
and control, on terms that prohibit them from making any copies of
your copyrighted material outside their relationship with you.
Conveying under any other circumstances is permitted solely under
the conditions stated below. Sublicensing is not allowed; section 10
makes it unnecessary.
3. Protecting Users' Legal Rights From Anti-Circumvention Law.
No covered work shall be deemed part of an effective technological
measure under any applicable law fulfilling obligations under article
11 of the WIPO copyright treaty adopted on 20 December 1996, or
similar laws prohibiting or restricting circumvention of such
measures.
When you convey a covered work, you waive any legal power to forbid
circumvention of technological measures to the extent such circumvention
is effected by exercising rights under this License with respect to
the covered work, and you disclaim any intention to limit operation or
modification of the work as a means of enforcing, against the work's
users, your or third parties' legal rights to forbid circumvention of
technological measures.
4. Conveying Verbatim Copies.
You may convey verbatim copies of the Program's source code as you
receive it, in any medium, provided that you conspicuously and
appropriately publish on each copy an appropriate copyright notice;
keep intact all notices stating that this License and any
non-permissive terms added in accord with section 7 apply to the code;
keep intact all notices of the absence of any warranty; and give all
recipients a copy of this License along with the Program.
You may charge any price or no price for each copy that you convey,
and you may offer support or warranty protection for a fee.
5. Conveying Modified Source Versions.
You may convey a work based on the Program, or the modifications to
produce it from the Program, in the form of source code under the
terms of section 4, provided that you also meet all of these conditions:
a) The work must carry prominent notices stating that you modified
it, and giving a relevant date.
b) The work must carry prominent notices stating that it is
released under this License and any conditions added under section
7. This requirement modifies the requirement in section 4 to
"keep intact all notices".
c) You must license the entire work, as a whole, under this
License to anyone who comes into possession of a copy. This
License will therefore apply, along with any applicable section 7
additional terms, to the whole of the work, and all its parts,
regardless of how they are packaged. This License gives no
permission to license the work in any other way, but it does not
invalidate such permission if you have separately received it.
d) If the work has interactive user interfaces, each must display
Appropriate Legal Notices; however, if the Program has interactive
interfaces that do not display Appropriate Legal Notices, your
work need not make them do so.
A compilation of a covered work with other separate and independent
works, which are not by their nature extensions of the covered work,
and which are not combined with it such as to form a larger program,
in or on a volume of a storage or distribution medium, is called an
"aggregate" if the compilation and its resulting copyright are not
used to limit the access or legal rights of the compilation's users
beyond what the individual works permit. Inclusion of a covered work
in an aggregate does not cause this License to apply to the other
parts of the aggregate.
6. Conveying Non-Source Forms.
You may convey a covered work in object code form under the terms
of sections 4 and 5, provided that you also convey the
machine-readable Corresponding Source under the terms of this License,
in one of these ways:
a) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by the
Corresponding Source fixed on a durable physical medium
customarily used for software interchange.
b) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by a
written offer, valid for at least three years and valid for as
long as you offer spare parts or customer support for that product
model, to give anyone who possesses the object code either (1) a
copy of the Corresponding Source for all the software in the
product that is covered by this License, on a durable physical
medium customarily used for software interchange, for a price no
more than your reasonable cost of physically performing this
conveying of source, or (2) access to copy the
Corresponding Source from a network server at no charge.
c) Convey individual copies of the object code with a copy of the
written offer to provide the Corresponding Source. This
alternative is allowed only occasionally and noncommercially, and
only if you received the object code with such an offer, in accord
with subsection 6b.
d) Convey the object code by offering access from a designated
place (gratis or for a charge), and offer equivalent access to the
Corresponding Source in the same way through the same place at no
further charge. You need not require recipients to copy the
Corresponding Source along with the object code. If the place to
copy the object code is a network server, the Corresponding Source
may be on a different server (operated by you or a third party)
that supports equivalent copying facilities, provided you maintain
clear directions next to the object code saying where to find the
Corresponding Source. Regardless of what server hosts the
Corresponding Source, you remain obligated to ensure that it is
available for as long as needed to satisfy these requirements.
e) Convey the object code using peer-to-peer transmission, provided
you inform other peers where the object code and Corresponding
Source of the work are being offered to the general public at no
charge under subsection 6d.
A separable portion of the object code, whose source code is excluded
from the Corresponding Source as a System Library, need not be
included in conveying the object code work.
A "User Product" is either (1) a "consumer product", which means any
tangible personal property which is normally used for personal, family,
or household purposes, or (2) anything designed or sold for incorporation
into a dwelling. In determining whether a product is a consumer product,
doubtful cases shall be resolved in favor of coverage. For a particular
product received by a particular user, "normally used" refers to a
typical or common use of that class of product, regardless of the status
of the particular user or of the way in which the particular user
actually uses, or expects or is expected to use, the product. A product
is a consumer product regardless of whether the product has substantial
commercial, industrial or non-consumer uses, unless such uses represent
the only significant mode of use of the product.
"Installation Information" for a User Product means any methods,
procedures, authorization keys, or other information required to install
and execute modified versions of a covered work in that User Product from
a modified version of its Corresponding Source. The information must
suffice to ensure that the continued functioning of the modified object
code is in no case prevented or interfered with solely because
modification has been made.
If you convey an object code work under this section in, or with, or
specifically for use in, a User Product, and the conveying occurs as
part of a transaction in which the right of possession and use of the
User Product is transferred to the recipient in perpetuity or for a
fixed term (regardless of how the transaction is characterized), the
Corresponding Source conveyed under this section must be accompanied
by the Installation Information. But this requirement does not apply
if neither you nor any third party retains the ability to install
modified object code on the User Product (for example, the work has
been installed in ROM).
The requirement to provide Installation Information does not include a
requirement to continue to provide support service, warranty, or updates
for a work that has been modified or installed by the recipient, or for
the User Product in which it has been modified or installed. Access to a
network may be denied when the modification itself materially and
adversely affects the operation of the network or violates the rules and
protocols for communication across the network.
Corresponding Source conveyed, and Installation Information provided,
in accord with this section must be in a format that is publicly
documented (and with an implementation available to the public in
source code form), and must require no special password or key for
unpacking, reading or copying.
7. Additional Terms.
"Additional permissions" are terms that supplement the terms of this
License by making exceptions from one or more of its conditions.
Additional permissions that are applicable to the entire Program shall
be treated as though they were included in this License, to the extent
that they are valid under applicable law. If additional permissions
apply only to part of the Program, that part may be used separately
under those permissions, but the entire Program remains governed by
this License without regard to the additional permissions.
When you convey a copy of a covered work, you may at your option
remove any additional permissions from that copy, or from any part of
it. (Additional permissions may be written to require their own
removal in certain cases when you modify the work.) You may place
additional permissions on material, added by you to a covered work,
for which you have or can give appropriate copyright permission.
Notwithstanding any other provision of this License, for material you
add to a covered work, you may (if authorized by the copyright holders of
that material) supplement the terms of this License with terms:
a) Disclaiming warranty or limiting liability differently from the
terms of sections 15 and 16 of this License; or
b) Requiring preservation of specified reasonable legal notices or
author attributions in that material or in the Appropriate Legal
Notices displayed by works containing it; or
c) Prohibiting misrepresentation of the origin of that material, or
requiring that modified versions of such material be marked in
reasonable ways as different from the original version; or
d) Limiting the use for publicity purposes of names of licensors or
authors of the material; or
e) Declining to grant rights under trademark law for use of some
trade names, trademarks, or service marks; or
f) Requiring indemnification of licensors and authors of that
material by anyone who conveys the material (or modified versions of
it) with contractual assumptions of liability to the recipient, for
any liability that these contractual assumptions directly impose on
those licensors and authors.
All other non-permissive additional terms are considered "further
restrictions" within the meaning of section 10. If the Program as you
received it, or any part of it, contains a notice stating that it is
governed by this License along with a term that is a further
restriction, you may remove that term. If a license document contains
a further restriction but permits relicensing or conveying under this
License, you may add to a covered work material governed by the terms
of that license document, provided that the further restriction does
not survive such relicensing or conveying.
If you add terms to a covered work in accord with this section, you
must place, in the relevant source files, a statement of the
additional terms that apply to those files, or a notice indicating
where to find the applicable terms.
Additional terms, permissive or non-permissive, may be stated in the
form of a separately written license, or stated as exceptions;
the above requirements apply either way.
8. Termination.
You may not propagate or modify a covered work except as expressly
provided under this License. Any attempt otherwise to propagate or
modify it is void, and will automatically terminate your rights under
this License (including any patent licenses granted under the third
paragraph of section 11).
However, if you cease all violation of this License, then your
license from a particular copyright holder is reinstated (a)
provisionally, unless and until the copyright holder explicitly and
finally terminates your license, and (b) permanently, if the copyright
holder fails to notify you of the violation by some reasonable means
prior to 60 days after the cessation.
Moreover, your license from a particular copyright holder is
reinstated permanently if the copyright holder notifies you of the
violation by some reasonable means, this is the first time you have
received notice of violation of this License (for any work) from that
copyright holder, and you cure the violation prior to 30 days after
your receipt of the notice.
Termination of your rights under this section does not terminate the
licenses of parties who have received copies or rights from you under
this License. If your rights have been terminated and not permanently
reinstated, you do not qualify to receive new licenses for the same
material under section 10.
9. Acceptance Not Required for Having Copies.
You are not required to accept this License in order to receive or
run a copy of the Program. Ancillary propagation of a covered work
occurring solely as a consequence of using peer-to-peer transmission
to receive a copy likewise does not require acceptance. However,
nothing other than this License grants you permission to propagate or
modify any covered work. These actions infringe copyright if you do
not accept this License. Therefore, by modifying or propagating a
covered work, you indicate your acceptance of this License to do so.
10. Automatic Licensing of Downstream Recipients.
Each time you convey a covered work, the recipient automatically
receives a license from the original licensors, to run, modify and
propagate that work, subject to this License. You are not responsible
for enforcing compliance by third parties with this License.
An "entity transaction" is a transaction transferring control of an
organization, or substantially all assets of one, or subdividing an
organization, or merging organizations. If propagation of a covered
work results from an entity transaction, each party to that
transaction who receives a copy of the work also receives whatever
licenses to the work the party's predecessor in interest had or could
give under the previous paragraph, plus a right to possession of the
Corresponding Source of the work from the predecessor in interest, if
the predecessor has it or can get it with reasonable efforts.
You may not impose any further restrictions on the exercise of the
rights granted or affirmed under this License. For example, you may
not impose a license fee, royalty, or other charge for exercise of
rights granted under this License, and you may not initiate litigation
(including a cross-claim or counterclaim in a lawsuit) alleging that
any patent claim is infringed by making, using, selling, offering for
sale, or importing the Program or any portion of it.
11. Patents.
A "contributor" is a copyright holder who authorizes use under this
License of the Program or a work on which the Program is based. The
work thus licensed is called the contributor's "contributor version".
A contributor's "essential patent claims" are all patent claims
owned or controlled by the contributor, whether already acquired or
hereafter acquired, that would be infringed by some manner, permitted
by this License, of making, using, or selling its contributor version,
but do not include claims that would be infringed only as a
consequence of further modification of the contributor version. For
purposes of this definition, "control" includes the right to grant
patent sublicenses in a manner consistent with the requirements of
this License.
Each contributor grants you a non-exclusive, worldwide, royalty-free
patent license under the contributor's essential patent claims, to
make, use, sell, offer for sale, import and otherwise run, modify and
propagate the contents of its contributor version.
In the following three paragraphs, a "patent license" is any express
agreement or commitment, however denominated, not to enforce a patent
(such as an express permission to practice a patent or covenant not to
sue for patent infringement). To "grant" such a patent license to a
party means to make such an agreement or commitment not to enforce a
patent against the party.
If you convey a covered work, knowingly relying on a patent license,
and the Corresponding Source of the work is not available for anyone
to copy, free of charge and under the terms of this License, through a
publicly available network server or other readily accessible means,
then you must either (1) cause the Corresponding Source to be so
available, or (2) arrange to deprive yourself of the benefit of the
patent license for this particular work, or (3) arrange, in a manner
consistent with the requirements of this License, to extend the patent
license to downstream recipients. "Knowingly relying" means you have
actual knowledge that, but for the patent license, your conveying the
covered work in a country, or your recipient's use of the covered work
in a country, would infringe one or more identifiable patents in that
country that you have reason to believe are valid.
If, pursuant to or in connection with a single transaction or
arrangement, you convey, or propagate by procuring conveyance of, a
covered work, and grant a patent license to some of the parties
receiving the covered work authorizing them to use, propagate, modify
or convey a specific copy of the covered work, then the patent license
you grant is automatically extended to all recipients of the covered
work and works based on it.
A patent license is "discriminatory" if it does not include within
the scope of its coverage, prohibits the exercise of, or is
conditioned on the non-exercise of one or more of the rights that are
specifically granted under this License. You may not convey a covered
work if you are a party to an arrangement with a third party that is
in the business of distributing software, under which you make payment
to the third party based on the extent of your activity of conveying
the work, and under which the third party grants, to any of the
parties who would receive the covered work from you, a discriminatory
patent license (a) in connection with copies of the covered work
conveyed by you (or copies made from those copies), or (b) primarily
for and in connection with specific products or compilations that
contain the covered work, unless you entered into that arrangement,
or that patent license was granted, prior to 28 March 2007.
Nothing in this License shall be construed as excluding or limiting
any implied license or other defenses to infringement that may
otherwise be available to you under applicable patent law.
12. No Surrender of Others' Freedom.
If conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot convey a
covered work so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you may
not convey it at all. For example, if you agree to terms that obligate you
to collect a royalty for further conveying from those to whom you convey
the Program, the only way you could satisfy both those terms and this
License would be to refrain entirely from conveying the Program.
13. Use with the GNU Affero General Public License.
Notwithstanding any other provision of this License, you have
permission to link or combine any covered work with a work licensed
under version 3 of the GNU Affero General Public License into a single
combined work, and to convey the resulting work. The terms of this
License will continue to apply to the part which is the covered work,
but the special requirements of the GNU Affero General Public License,
section 13, concerning interaction through a network will apply to the
combination as such.
14. Revised Versions of this License.
The Free Software Foundation may publish revised and/or new versions of
the GNU General Public License from time to time. Such new versions will
be similar in spirit to the present version, but may differ in detail to
address new problems or concerns.
Each version is given a distinguishing version number. If the
Program specifies that a certain numbered version of the GNU General
Public License "or any later version" applies to it, you have the
option of following the terms and conditions either of that numbered
version or of any later version published by the Free Software
Foundation. If the Program does not specify a version number of the
GNU General Public License, you may choose any version ever published
by the Free Software Foundation.
If the Program specifies that a proxy can decide which future
versions of the GNU General Public License can be used, that proxy's
public statement of acceptance of a version permanently authorizes you
to choose that version for the Program.
Later license versions may give you additional or different
permissions. However, no additional obligations are imposed on any
author or copyright holder as a result of your choosing to follow a
later version.
15. Disclaimer of Warranty.
THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. Limitation of Liability.
IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGES.
17. Interpretation of Sections 15 and 16.
If the disclaimer of warranty and limitation of liability provided
above cannot be given local legal effect according to their terms,
reviewing courts shall apply local law that most closely approximates
an absolute waiver of all civil liability in connection with the
Program, unless a warranty or assumption of liability accompanies a
copy of the Program in return for a fee.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Programs
If you develop a new program, and you want it to be of the greatest
possible use to the public, the best way to achieve this is to make it
free software which everyone can redistribute and change under these terms.
To do so, attach the following notices to the program. It is safest
to attach them to the start of each source file to most effectively
state the exclusion of warranty; and each file should have at least
the "copyright" line and a pointer to where the full notice is found.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <https://www.gnu.org/licenses/>.
Also add information on how to contact you by electronic and paper mail.
If the program does terminal interaction, make it output a short
notice like this when it starts in an interactive mode:
<program> Copyright (C) <year> <name of author>
This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it
under certain conditions; type `show c' for details.
The hypothetical commands `show w' and `show c' should show the appropriate
parts of the General Public License. Of course, your program's commands
might be different; for a GUI interface, you would use an "about box".
You should also get your employer (if you work as a programmer) or school,
if any, to sign a "copyright disclaimer" for the program, if necessary.
For more information on this, and how to apply and follow the GNU GPL, see
<https://www.gnu.org/licenses/>.
The GNU General Public License does not permit incorporating your program
into proprietary programs. If your program is a subroutine library, you
may consider it more useful to permit linking proprietary applications with
the library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License. But first, please read
<https://www.gnu.org/licenses/why-not-lgpl.html>.
================================================
FILE: README.md
================================================
# Domino Robot
This is the code for Mark Rober's Domino Robot project. You can find a bunch more info about the project, including details about the software architecture [on my website](https://www.baucomrobotics.com/domino-robot).
Here is the video that Mark made about the project:
[](https://www.youtube.com/watch?v=8HEfIJlcFbs)
## Folder structure
If you are interested in browsing the files in this repo, here is the general structure to get you oriented.
- `doc`: Some high level documentation stuff, much of it is probably out of date
- `experimental_testing`: Various experimental files
- `src`: The real stuff
- `master`: Code for running the master controller and GUI
- `robot`: Code that runs on the Raspberry Pi of the robot
- `robot_motor_driver`: Code that runs on the ClearCore of the robot
- `tools`: Some various utility scripts
## Usage (Not Recommended)
This repository exists mostly for those who are interested in browsing the code to see how it works. It is almost certainly a bad idea to try and download this code and use it unless you have identical hardware to the robot in the video and/or really know what you are doing. You should consider this code completely unsupported as I do not plan to make future updates or fix any bugs you may find.
If for some reason you really want to try and run this code, there are some (out of date) instructions on how to run the [the master code](src/master/README.md) and also some (possibly also out of date) info on which libraries are used for compiling [the robot code](src/robot/README.md). Good luck.
================================================
FILE: doc/DominoRobotControllerFlowchart.drawio
================================================
<mxfile host="app.diagrams.net" modified="2021-06-06T17:16:56.287Z" agent="5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/90.0.4430.212 Safari/537.36" etag="0eKxN2Gj-2nmRRTKrcC-" version="14.7.6" type="device"><diagram id="NM_vhxp-IM1YrRFfsY-A" name="Page-1">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</diagram></mxfile>
================================================
FILE: doc/DominoRobotSoftwareArchitecture.drawio
================================================
<mxfile host="app.diagrams.net" modified="2021-06-24T01:50:07.329Z" agent="5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/91.0.4472.114 Safari/537.36" etag="4WDjcxLQaKL4s1FWm-vA" version="14.7.9" type="device"><diagram id="HjjDRPqPhWfrdKk6NoQq" name="Page-1">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</diagram></mxfile>
================================================
FILE: doc/KukaPsuedoCode.txt
================================================
KukaInit();
int tile_count = 0;
int num_tiles = 333;
int num_rows = 20;
int num_cols = 15;
while (tile_count < num_tiles) {
string line = ReadLineFromFile(tile_count);
vector<char> colors = ParseColors(line);
int color_count = 0;
for (int row = 0; row < num_rows; row++) {
for (int col = 0; col < num_cols; col++) {
Pose pickup_pose = PickupPoseFromColor(colors[color_count]);
Pose dropoff_pose = DropoffPoseFromRowCol(row, col);
MoveToPickup(pickup_pose);
CloseGripper();
MoveToDropoff(dropoff_pose);
OpenGripper();
color_count++;
}
}
tile_count++;
}
================================================
FILE: doc/NetworkPrototcols.txt
================================================
Message spec
- All messages are sent as json encoded byte strings
- All messages will contain a 'type' field that will be a string denoting the command type
- All messages will contain a 'sn' field that will be an int denoting the message serial number
- All messages will contain a 'data' field that will house any additional data required by the message type. This field may be empty
Master to robot commands:
Move:
// Move to a waypoint in a straight line
float x
float y
float a
Place:
// Place dominoes on the floor
null
Dock:
// Dock at charging station
Undock:
// Undock from charging station
Dropoff:
// Drop off pallete on conveyor
Pickup:
// Pick up pallete from conveyor
Position:
// Fallback for sending position data if usb controller for marvelmind sensor doesn't work
float x
float y
float a
float time
Robot to master commands:
RobotStatus:
// Give a status update to server
float est_pos_x
float est_pos_y
float est_pos_a
float est_cur_vel_x
float est_cur_vel_y
float est_cur_vel_a
string cur_task
int cur_task_msg_sn
int bat_percent //Can we even get this?
string error_msg
bool error_present
//Add other fields/sensor statuses as needed
Master to base station commands:
NextTile:
// The next tile to fill
int[tile_x*tile_y] dominoes
int tile_id
Base Station to master commands:
BaseStatus:
// Status update for server
int cur_tile_id
float est_time_to_tile_full
// Other sensor statuses
General commands:
StatusRequest:
// Request status from robot
KeepAlive:
// Keep the connection alive
Ack:
//Acknowledge that command was recieved
int sn
Done:
//Let the server know the command was done
int sn
================================================
FILE: doc/SubsystemDiagram.drawio
================================================
<mxfile modified="2020-01-26T19:53:17.764Z" host="www.draw.io" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/79.0.3945.130 Safari/537.36" etag="FmhtWn5DU_qlXuZeJW7k" version="12.5.8" type="device"><diagram id="cr8PqfeKhw-YyZYMWl9d" name="Page-1">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</diagram></mxfile>
================================================
FILE: doc/SwClassHeirarchy.txt
================================================
- robot
- status updater
- status
- robot server
- socket multi thread wrapper
- robot controller
- serial comms
- localization
- Kalman filter
- controller mode
- position mode
- trajectory generator
- x,y,a controllers
- vision mode
- trajectory generator
- Camera tracker *
- Kalman filter
- x,y,a controllers
- tray controller
- serial comms
- marvelmind wrapper
- camera tracker
- side camera
- rear camera
- motor controller
- serial comms
- robot velocity handler
- lifter action handler
- master
- gui
- runtime
- plan
- robot interface
- robot client
- tcp client
- field planner
================================================
FILE: doc/clearcore_api.txt
================================================
Tentative thoughts on api with clearcore for motor control over serial comms
There's a few options here:
- Clearcore does all trajectory generation, kalman filtering, kinematics, and control
- Central processor sends it marvelmind position data and forwards command from master
- This requires a more extensive api to handle forwarding commands
- Clearcore recieves global velocity command from central processor, handles frame transformation and kinematics only
- Sends back computed global velocity
- Central processor has to send global angle along with command
- Cnetral processor handles kalman filtering and trajectory generation
- Relatively simple api
- Clearcore recieves local velocity command from central processors, handles local kinematics only
- Sends back computed local velocity
- Central processor handles kf, traj gen, frame conversion
- Very simple api - seems like the best starting point
Starting with option 3
Send from central controller:
- Local velocity command [vx, vy, va]
Response from clearcore:
- Estimated local velocity [vx_est, vy_est, va_est]
Clearcore outline:
while true:
if new command:
decode command
do IK
send new motor commands
do FK
encode response
send response (est vel)
else:
pass
================================================
FILE: experimental_testing/ImageParser.py
================================================
import matplotlib.image as mpimg
import matplotlib.pyplot as plt
import skimage.transform as sktf
import numpy as np
# Configuration
image_name = 'MR.jpg'
desired_width = 300
desired_height = 300
dominoes = np.array(
[('black', (0,0,0)),
('red', (1,0,0)),
('blue', (0,0,1)),
('green', (0,1,0)),
('white', (1,1,1))
])
# Load original image
img = mpimg.imread(image_name)
# Scaled image
img_scaled = sktf.resize(img, (desired_height, desired_width),anti_aliasing=False)
# Parse image into domino IDs by choosing 'closest' color
img_parsed_color = np.zeros((desired_width*desired_height, 3))
img_parsed_ids = np.zeros_like(img_parsed_color,dtype=np.int_)[:,0]
count = 0
for row in img_scaled:
for px in row:
best_id = -1
best_score = 9999999
for id,val in enumerate(dominoes):
color = val[1]
score = np.linalg.norm(px - color)
if score < best_score:
best_score = score
best_id = id
img_parsed_ids[count] = best_id
img_parsed_color[count] = dominoes[best_id][1]
count = count + 1
img_parsed_ids = img_parsed_ids.reshape((desired_height, desired_width))
img_parsed_color = img_parsed_color.reshape((desired_height, desired_width, 3))
# Output some metrics
print('Domino usage:')
print('Total number of dominoes: ' + str(img_parsed_ids.size))
print('Colors:')
unique_colors, counts = np.unique(img_parsed_ids, return_counts=True)
for i,id in enumerate(unique_colors):
print(' ' + dominoes[id][0] + ': ' + str(counts[i]))
# Plot images
fig, axes = plt.subplots(nrows=1, ncols=3)
ax = axes.ravel()
ax[0].imshow(img)
ax[0].set_title('Original')
ax[1].imshow(img_scaled)
ax[1].set_title('Scaled')
ax[2].imshow(img_parsed_color)
ax[2].set_title('Dominoes')
plt.tight_layout()
plt.show()
================================================
FILE: experimental_testing/RobotArduinoTesting/StepperDriverMotion_4Motor_Accel/StepperDriverMotion_4Motor_Accel.ino
================================================
#include <StepperDriver.h>
// https://github.com/DIMRobotics/ArduinoStepperDriver
/* Axis identifiers inside the library; on them then
Can be controlled specific. You can do several axles,
The maximum number in the native assembly is 3. It is treated by editing
headerfile, there is a constant NUM_AXSIS
*/
axis_t bl, br, fl, fr;
float MaxSpeed = 3.14; // rad/s
#define PulseRev 3200
const float SECONDS_TO_MICROSECONDS = 1000000;
const float STEPPER_CONVERSION = SECONDS_TO_MICROSECONDS * 2 * PI / PulseRev;
#define bl_pulse 49
#define bl_dir 48
#define br_pulse 51
#define br_dir 50
#define fl_pulse 47
#define fl_dir 46
#define fr_pulse 45
#define fr_dir 44
#define PIN_ENABLE_ALL 40
void setup()
{
Serial.begin(115200);
StepperDriver.init();
delay(100);
// Serial.print("rad conversion: ");
// Serial.println(RAD_PER_SEC_TO_STEPS_PER_SEC);
// Serial.print("sec conversion: ");
// Serial.println(SECONDS_TO_MICROSECONDS);
bl = StepperDriver.newAxis(bl_pulse, bl_dir, 255, PulseRev);
br = StepperDriver.newAxis(br_pulse, br_dir, 255, PulseRev);
fr = StepperDriver.newAxis(fr_pulse, fr_dir, 255, PulseRev);
fl = StepperDriver.newAxis(fl_pulse, fl_dir, 255, PulseRev);
StepperDriver.enable(bl);
StepperDriver.enable(br);
StepperDriver.enable(fl);
StepperDriver.enable(fr);
digitalWrite(PIN_ENABLE_ALL, LOW);
Serial.println("TCCR2A: ");
Serial.println(TCCR2A, BIN);
Serial.println("TCCR2B: ");
Serial.println(TCCR2B, BIN);
Serial.println("TIMSK2: ");
Serial.println(TIMSK2, BIN);
}
void writeAll(float vel)
{
Serial.print("Speed: ");
Serial.println(vel, 4);
Serial.print("Delays: [ ");
writeSpeed(fl, vel);
writeSpeed(fr, vel);
writeSpeed(br, vel);
writeSpeed(bl, vel);
Serial.println("]");
}
void writeSpeed(axis_t motor, float vel)
{
uint16_t delay_us = 0; // This works for if vel is 0
// Compute motor direction
if(vel > 0)
{
StepperDriver.setDir(motor, FORWARD);
}
else
{
vel = -vel;
StepperDriver.setDir(motor, BACKWARD);
}
// Compute delay between steps to achieve desired velocity
if(vel != 0)
{
delay_us = static_cast<uint16_t>(STEPPER_CONVERSION / vel);
}
Serial.print(delay_us);
Serial.print(", ");
// Update motor
StepperDriver.setDelay(motor, delay_us);
}
void loop()
{
writeAll(MaxSpeed);
delay(2000);
writeAll(0);
delay(500);
writeAll(-1*MaxSpeed);
delay(2000);
writeAll(0);
delay(500);
}
================================================
FILE: experimental_testing/RobotArduinoTesting/TestMotor/TestMotor.ino
================================================
#define PIN_ENABLE 52
#define PIN_BL_PWM 6
#define PIN_BL_DIR 48
#define FW 1
#define BW 0
#define SPEED_SLOW 50
#define SPEED_FAST 255
void setup() {
// put your setup code here, to run once:
pinMode(PIN_ENABLE,OUTPUT);
pinMode(PIN_BL_PWM,OUTPUT);
pinMode(PIN_BL_DIR,OUTPUT);
Serial.begin(9600);
}
void loop() {
// put your main code here, to run repeatedly:
Serial.print("Forward");
analogWrite(PIN_BL_PWM, SPEED_SLOW);
digitalWrite(PIN_BL_DIR, FW);
digitalWrite(PIN_ENABLE, HIGH);
// Wait for a little bit
delay(2000);
// Stop
digitalWrite(PIN_ENABLE, LOW);
delay(500);
Serial.print("Reverse");
analogWrite(PIN_BL_PWM, SPEED_FAST);
digitalWrite(PIN_BL_DIR, BW);
digitalWrite(PIN_ENABLE, HIGH);
// Wait for a little bit
delay(2000);
// Stop
digitalWrite(PIN_ENABLE, LOW);
delay(500);
}
================================================
FILE: experimental_testing/RobotArduinoTesting/WifiServerWithESP/RobotServer.cpp
================================================
#include <Arduino.h> // This has to be before ArduinoJson.h to fix compiler issues
#include "RobotServer.h"
#include <ArduinoJson.h>
RobotServer::RobotServer(HardwareSerial& serial, HardwareSerial& debug)
: serial_(serial),
debug_(debug),
clientConnected_(false),
wifiConnected_(false)
{
serial_.begin(115200);
}
RobotServer::oneLoop()
{
COMMAND cmd = COMMAND::NONE;
String newMsg = getAnyIncomingMessage();
if(newMsg.length() != 0)
{
debug_.print("[RobotServer] ");
debug_.print("RCV: ");
debug_.println(newMsg);
if(newMsg == "Client connected")
{
clientConnected_ = true;
wifiConnected_ = true;
}
else if(newMsg == "Client disconnected")
{
clientConnected_ = false;
wifiConnected_ = true;
}
else if(newMsg == "Connecting..")
{
wifiConnected_ = false;
clientConnected_ = false;
}
else if(newMsg.startsWith("Connected to WiFi."))
{
clientConnected_ = false;
wifiConnected_ = true;
}
else if(newMsg == "Waiting for client connection")
{
clientConnected_ = false;
wifiConnected_ = true;
}
else
{
cmd = getCommand(cleanString(newMsg));
}
}
return cmd;
}
String RobotServer::cleanString(String message)
{
int idx_start = message.indexOf("{");
int idx_end = message.lastIndexOf("}") + 1;
return message.substring(idx_start, idx_end);
}
String RobotServer::getAnyIncomingMessage()
{
String msg = "";
if(serial_.available())
{
msg = serial_.readString();
}
return msg;
}
RobotServer::COMMAND RobotServer::getCommand(String message)
{
COMMAND cmd = COMMAND::NONE;
StaticJsonDocument<256> doc;
DeserializationError err = deserializeJson(doc, message);
debug_.print("[RobotServer] GetCommand(): ");
debug_.println(message);
if(err)
{
debug_.print("[RobotServer] Error parsing JSON: ");
debug_.println(err.c_str());
cmd = COMMAND::ERROR_BAD_JSON;
sendErr("bad_json");
}
else
{
String type = doc["type"];
if(type == "move")
{
debug_.println("[RobotServer] Got MOVE command ");
cmd = COMMAND::MOVE;
sendAck(type);
}
else if(type == "place")
{
debug_.println("[RobotServer] Got PLACE command ");
cmd = COMMAND::PLACE;
sendAck(type);
}
else if(type == "dock")
{
debug_.println("[RobotServer] Got DOCK command ");
cmd = COMMAND::DOCK;
sendAck(type);
}
else if(type == "undock")
{
debug_.println("[RobotServer] Got UNDOCK command ");
cmd = COMMAND::UNDOCK;
sendAck(type);
}
else if(type == "dropoff")
{
debug_.println("[RobotServer] Got DROPOFF command ");
cmd = COMMAND::DROPOFF;
sendAck(type);
}
else if(type == "pickup")
{
debug_.println("[RobotServer] Got PICKUP command ");
cmd = COMMAND::PICKUP;
sendAck(type);
}
else if(type == "position")
{
debug_.println("[RobotServer] Got POSITION command ");
cmd = COMMAND::POSITION;
sendAck(type);
}
else if(type == "status")
{
debug_.println("[RobotServer] Got STATUS command ");
cmd = COMMAND::STATUS;
sendStatus();
}
else if(type == "")
{
debug_.println("[RobotServer] ERROR: Type field empty or not specified ");
cmd = COMMAND::ERROR_NO_TYPE;
sendErr("no_type");
}
else
{
debug_.println("[RobotServer] ERROR: Unkown type field ");
cmd = COMMAND::ERROR_UNKOWN_TYPE;
sendErr("unkown_type");
}
}
return cmd;
}
void RobotServer::sendMsg(String msg)
{
serial_.print(msg + '\0');
debug_.print("[RobotServer] Send: ");
debug_.println(msg);
}
void RobotServer::sendAck(String data)
{
StaticJsonDocument<256> doc;
doc["type"] = "ack";
doc["data"] = data;
String msg;
serializeJson(doc, msg);
sendMsg(msg);
}
void RobotServer::sendErr(String data)
{
StaticJsonDocument<256> doc;
doc["type"] = "ack";
doc["data"] = data;
String msg;
serializeJson(doc, msg);
sendMsg(msg);
}
void RobotServer::sendStatus()
{
StaticJsonDocument<256> doc;
doc["type"] = "status";
doc["data"] = "not implimented";
String msg;
serializeJson(doc, msg);
sendMsg(msg);
}
================================================
FILE: experimental_testing/RobotArduinoTesting/WifiServerWithESP/RobotServer.h
================================================
/*
* Robot server that handles gettings messages from master
* and responding correctly
*/
#ifndef RobotServer_h
#define RobotServer_h
#include <HardwareSerial.h>
class RobotServer
{
public:
enum COMMAND
{
NONE,
ERROR_NO_TYPE,
ERROR_UNKOWN_TYPE,
ERROR_BAD_JSON,
MOVE,
PLACE,
DOCK,
UNDOCK,
DROPOFF,
PICKUP,
POSITION,
STATUS
};
// Constructor
RobotServer(HardwareSerial& serial, HardwareSerial& debug);
int oneLoop();
private:
HardwareSerial& serial_;
HardwareSerial& debug_;
bool clientConnected_;
bool wifiConnected_;
String getAnyIncomingMessage();
String cleanString(String message);
COMMAND getCommand(String message);
void sendMsg(String msg);
void sendAck(String data);
void sendErr(String data);
void sendStatus();
};
#endif
================================================
FILE: experimental_testing/RobotArduinoTesting/WifiServerWithESP/WifiServerWithESP.ino
================================================
#include "RobotServer.h"
RobotServer server = RobotServer(Serial3, Serial);
void setup() {
// put your setup code here, to run once:
// Communication with the host computer
Serial.begin(9600);
Serial.println("Wifi client starting");
}
void loop() {
// put your main code here, to run repeatedly:
int cmd = server.oneLoop();
delay(50);
}
================================================
FILE: experimental_testing/RobotArduinoTesting/WifiTesting/WifiTesting.ino
================================================
// Arduino pin 15 (RX3) to ESP8266 TX
// Arduino pin 14 (TX3) to voltage divider then to ESP8266 RX
// Connect GND from the Arduiono to GND on the ESP8266
// Pull ESP8266 CH_PD HIGH
// When a command is entered in to the serial monitor on the computer
// the Arduino will relay it to the ESP8266
void setup()
{
Serial.begin(9600); // communication with the host computer
// Start the serial for communication with the ESP8266
Serial3.begin(115200);
Serial.println("");
Serial.println("Remember to to set Both NL & CR in the serial monitor.");
Serial.println("Ready");
Serial.println("");
}
void loop()
{
// listen for communication from the ESP8266 and then write it to the serial monitor
if ( Serial3.available() ) { Serial.write( Serial3.read() ); }
// listen for user input and send it to the ESP8266
if ( Serial.available() ) { Serial3.write( Serial.read() ); }
}
================================================
FILE: experimental_testing/RobotArduinoTesting/all_motors/all_motors.ino
================================================
// Pinouts
#define PIN_ENABLE 52
#define PIN_FR_PWM 3
#define PIN_FR_DIR 49
#define PIN_FL_PWM 2
#define PIN_FL_DIR 51
#define PIN_BR_PWM 5
#define PIN_BR_DIR 50
#define PIN_BL_PWM 6
#define PIN_BL_DIR 48
// Motor index
#define FR 0
#define FL 1
#define BR 2
#define BL 3
// Motor arrays
const int PWM_ARRAY[4] = {PIN_FR_PWM, PIN_FL_PWM, PIN_BR_PWM, PIN_BL_PWM};
const int DIR_ARRAY[4] = {PIN_FR_DIR, PIN_FL_DIR, PIN_BR_DIR, PIN_BL_DIR};
const int SPEED_ARRAY[4] = {130, 75, 75, 100};
const int NUM_DIRECTIONS = 4;
// Direction vectors
const int MOTOR_DIRECTIONS[NUM_DIRECTIONS][4] =
{
{1, 1, 1, 1}, //Forward
{0, 0, 0, 0}, //Backward
{1, 0, 0, 1}, //Left
{0, 1, 1, 0} //Right
// {1, 0, 1, 0}, //Forward-Left
// {1, 0, 0, 1}, //Forward-Right
// {0, 1, 0, 1}, //Backward-Left
// {0, 1, 1, 0} //Backward-Right
};
void commandMotors(int direction)
{
for(int i = 0; i < 4; i++)
{
analogWrite(PWM_ARRAY[i],SPEED_ARRAY[i]);
digitalWrite(DIR_ARRAY[i],MOTOR_DIRECTIONS[direction][i]);
}
digitalWrite(PIN_ENABLE,HIGH);
}
void setup()
{
// Setup pin modes
pinMode(PIN_ENABLE,OUTPUT);
for(int i = 0; i < 4; i++)
{
pinMode(PWM_ARRAY[i],OUTPUT);
pinMode(DIR_ARRAY[i],OUTPUT);
}
Serial.begin(9600);
}
int stage = 0;
void loop()
{
// Command motors on
commandMotors(stage);
Serial.print("On: ");
Serial.println(stage);
// Wait for a little bit
delay(1000);
// Turn motors off and let coast
digitalWrite(PIN_ENABLE,HIGH);
for(int i = 0; i < 4; i++)
{
analogWrite(PWM_ARRAY[i],0);
}
Serial.println("Off");
// Wait for a little bit
delay(1500);
// Increment stage
if(stage == NUM_DIRECTIONS-1)
{
stage = 0;
}
else
{
stage++;
}
}
================================================
FILE: experimental_testing/RobotArduinoTesting/all_motors_speed_control/Motor.cpp
================================================
#include "Motor.h"
#include <math.h>
Motor::Motor(int pwmPin, int dirPin, int encPinA, int encPinB, double Kp, double Ki, double Kd, float velFilterFreq)
: pwmPin_(pwmPin),
dirPin_(dirPin),
inputVel_(0.0),
currentVelRaw_(0.0),
currentVelFiltered_(0.0),
outputCmd_(0.0),
prevCount_(0),
prevMillis_(millis()),
enc_(encPinA, encPinB),
controller_(¤tVelFiltered_, &outputCmd_, &inputVel_, Kp, Ki, Kd, DIRECT),
velFilter_(LOWPASS, velFilterFreq)
{
pinMode(pwmPin_, OUTPUT);
pinMode(dirPin_, OUTPUT);
controller_.SetMode(AUTOMATIC);
controller_.SetOutputLimits(-255, 255);
}
void Motor::setCommand(double vel)
{
inputVel_ = vel;
}
void Motor::runLoop()
{
// Read current values
long curCount = enc_.read();
unsigned long curMillis = millis();
// Compute delta
long deltaCount = curCount - prevCount_;
double deltaRevs = static_cast<double>(deltaCount) * COUNTS_PER_REV;
unsigned long deltaMillis = curMillis - prevMillis_;
// Compute current velocity in revs/second
currentVelRaw_ = deltaRevs / (static_cast<double>(deltaMillis) * 1000.0);
currentVelFiltered_ = velFilter_.input(currentVelRaw_);
// Run PID controller
controller_.Compute();
// Update output values
if(outputCmd_ < 0)
{
digitalWrite(dirPin_,0);
}
else
{
digitalWrite(dirPin_,1);
}
analogWrite(pwmPin_, abs(outputCmd_));
// Copy current values into previous
prevCount_ = curCount;
prevMillis_ = curMillis;
//
// Serial.print(curCount);
// Serial.print(" ");
// Serial.print(currentVelRaw_);
// Serial.print(" ");
// Serial.print(currentVelFiltered_);
// Serial.print(" ");
// Serial.print(inputVel_);
// Serial.print(" ");
// Serial.print(outputCmd_);
// Serial.println("");
}
================================================
FILE: experimental_testing/RobotArduinoTesting/all_motors_speed_control/Motor.h
================================================
/* Requires libraries:
* Filters: https://github.com/JonHub/Filters
* PID: https://playground.arduino.cc/Code/PIDLibrary/
* Encoder: https://www.pjrc.com/teensy/td_libs_Encoder.html
*/
#ifndef Motor_h
#define Motor_h
#include <Encoder.h>
#include <PID_v1.h>
#include <Filters.h>
#define COUNTS_PER_REV 835;
class Motor
{
public:
/*
* Motor object constructor
* pwmPin - which pin the motor PWM is connected to
* dirPin - which pin the motor direction is connected to
* encPinA - which pin the encoder wire A is connected to. This should be a pin capable of handling interrupts
* encPinB - which pin the encoder wire B is connected to. This should ideally (but not required) be a pin capable of handling interrupts
* Kp - Proportional gain
* Ki - Integral gain
* Kd - Derrivative gain
* velFilterFreq - frequency to use for velocity lowpass filter
*/
Motor(int pwmPin, int dirPin, int encPinA, int encPinB, double Kp, double Ki, double Kd, float velFilterFreq);
/*
* Set the desired velocity in revs/second
*/
void setCommand(double vel);
/*
* Run the actual controller. Make sure this gets called reasonably quickly (i.e. every 20 ms or so)
*/
void runLoop();
private:
// Pins
int pwmPin_;
int dirPin_;
// Values for computation
double inputVel_; // Desired velocity in revs/sec
double currentVelRaw_; // Raw current velocity in revs/sec
double currentVelFiltered_;
double outputCmd_; // Output command in [-255, 255]
long prevCount_; // Encoder count from previous loop
// Timer
unsigned long prevMillis_; // Timer from previous loop
// Other objects
Encoder enc_; // Encoder object
PID controller_; // PID controller
FilterOnePole velFilter_; // Velocity lowpass filter
};
#endif Motor_h
================================================
FILE: experimental_testing/RobotArduinoTesting/all_motors_speed_control/all_motors_speed_control.ino
================================================
/* Requires libraries:
* Filters: https://github.com/JonHub/Filters
* PID: https://playground.arduino.cc/Code/PIDLibrary/
* Encoder: https://www.pjrc.com/teensy/td_libs_Encoder.html
*/
#include "Motor.h"
// Pinouts
#define PIN_ENABLE 52
#define PIN_FR_PWM 3
#define PIN_FR_DIR 49
#define PIN_FL_PWM 2
#define PIN_FL_DIR 51
#define PIN_BR_PWM 5
#define PIN_BR_DIR 50
#define PIN_BL_PWM 6
#define PIN_BL_DIR 48
#define PIN_FR_ENC_A 20
#define PIN_FR_ENC_B 35
#define PIN_FL_ENC_A 21
#define PIN_FL_ENC_B 41
#define PIN_BR_ENC_A 18
#define PIN_BR_ENC_B 23
#define PIN_BL_ENC_A 19
#define PIN_BL_ENC_B 29
#define VEL_FILTER_FREQ 0.5 // HZ
const double Kp = 100;
const double Ki = 1;
const double Kd = 1;
Motor allMotors[4] = {
Motor(PIN_FR_PWM, PIN_FR_DIR, PIN_FR_ENC_A, PIN_FR_ENC_B, Kp, Ki, Kd, VEL_FILTER_FREQ),
Motor(PIN_FL_PWM, PIN_FL_DIR, PIN_FL_ENC_A, PIN_FL_ENC_B, Kp, Ki, Kd, VEL_FILTER_FREQ),
Motor(PIN_BR_PWM, PIN_BR_DIR, PIN_BR_ENC_A, PIN_BR_ENC_B, Kp, Ki, Kd, VEL_FILTER_FREQ),
Motor(PIN_BL_PWM, PIN_BL_DIR, PIN_BL_ENC_A, PIN_BL_ENC_B, Kp, Ki, Kd, VEL_FILTER_FREQ) };
const int MOTOR_DIRECTIONS[4][4] =
{
{1, 1, 1, 1}, //Forward
{-1, -1, -1, -1}, //Backward
{1, -1, -1, 1}, //Left
{-1, 1, 1, -1} //Right
// {1, 0, 1, 0}, //Forward-Left
// {1, 0, 0, 1}, //Forward-Right
// {0, 1, 0, 1}, //Backward-Left
// {0, 1, 1, 0} //Backward-Right
};
const float SPEED[4] = {2, 1, 1, 0.5}; // scale factor for rev/s
void setup()
{
// Setup pin modes
pinMode(PIN_ENABLE,OUTPUT);
Serial.begin(250000);
}
int waitForInput()
{
int motion = 0;
bool gotInput = false;
while(!gotInput)
{
if(Serial.available() > 0)
{
// Serial.println("Serial available");
int incomingByte = Serial.read();
// Serial.print("Read: ");
// Serial.println(incomingByte);
if(incomingByte == 49)
{
motion = 1;
gotInput = true;
}
else if(incomingByte == 50)
{
motion = 2;
gotInput = true;
}
else if(incomingByte == 51)
{
motion = 3;
gotInput = true;
}
else if(incomingByte == 52)
{
motion = 4;
gotInput = true;
}
else
{
// Do nothing
}
}
}
return motion;
}
void setMotorCommands(int motion)
{
for(int i = 0; i < 4; i++)
{
int curDirection = MOTOR_DIRECTIONS[motion-1][i];
float curSpeed = curDirection * SPEED[i];
Serial.print("Motor ");
Serial.print(i);
Serial.print(", Direction ");
Serial.print(curDirection);
Serial.print(", Speed ");
Serial.println(curSpeed);
allMotors[i].setCommand(curSpeed);
}
digitalWrite(PIN_ENABLE, 1);
}
void runMotors()
{
unsigned long prevTime = millis();
unsigned long startTime = prevTime;
int targetDelta = 20;
int targetTotalTime = 2000;
while (millis() - startTime < targetTotalTime)
{
if(millis() - prevTime > targetDelta)
{
prevTime = millis();
for(int i = 0; i < 4; i++)
{
allMotors[i].runLoop();
}
}
}
}
void loop()
{
int motion = waitForInput();
setMotorCommands(motion);
runMotors();
// Stop motors
digitalWrite(PIN_ENABLE, 0);
}
================================================
FILE: experimental_testing/RobotArduinoTesting/kalman_test/KalmanFilter.cpp
================================================
#include "KalmanFilter.h"
KalmanFilter::KalmanFilter(
double dt,
const mat& A,
const mat& B,
const mat& C,
const mat& Q,
const mat& R,
const mat& P)
: A(A), B(B), C(C), Q(Q), R(R), P0(P),
m(C.get_rows()), n(A.get_rows()), dt(dt),
I(mat::identity(n)), x_hat(1,n), x_hat_new(1,n)
{
}
void KalmanFilter::init(double t0, const mat& x0)
{
x_hat = x0;
P = P0;
this->t0 = t0;
t = t0;
}
void KalmanFilter::init()
{
x_hat = mat::zeros(n,1);
P = P0;
t0 = 0;
t = t0;
}
void KalmanFilter::predict(double dt, const mat& A, const mat& u)
{
this->A = A;
this->dt = dt;
x_hat_new = A * x_hat + B * u;
P = A*P*A.t() + Q;
t += dt;
x_hat = x_hat_new;
}
void KalmanFilter::update(const mat& y)
{
K = P*C.t()*(C*P*C.t() + R).inv();
x_hat_new += K * (y - C*x_hat);
P = (I - K*C)*P;
x_hat = x_hat_new;
}
================================================
FILE: experimental_testing/RobotArduinoTesting/kalman_test/KalmanFilter.h
================================================
// Loosely based on kalman-cpp: https://github.com/hmartiro/kalman-cpp
#ifndef KalmanFilter_h
#define KalmanFilter_h
#include <LinearAlgebra.h>
class KalmanFilter
{
public:
/**
* Create a Kalman filter with the specified matrices.
* A - System dynamics matrix
* B - Control matrix
* C - Output matrix
* Q - Process noise covariance
* R - Measurement noise covariance
* P - Estimate error covariance
*/
KalmanFilter(
double dt,
const mat& A,
const mat& B,
const mat& C,
const mat& Q,
const mat& R,
const mat& P
);
/**
* Initialize the filter with initial states as zero.
*/
void init();
/**
* Initialize the filter with a guess for initial states.
*/
void init(double t0, const mat& x0);
/**
* Predict estimated state based on the time step, dynamics matrix, and control input
*/
void predict(double dt, const mat& A, const mat& u);
/**
* Update the estimated state based on measured values.
*/
void update(const mat& y);
/**
* Return the current state and time.
*/
mat state() { return x_hat; };
double time() { return t; };
private:
// Matrices for computation
mat A, B, C, Q, R, P, K, P0;
// System dimensions
uint8_t m, n;
// Initial and current time
double t0, t;
// Discrete time step
double dt;
// n-size identity
mat I;
// Estimated states
mat x_hat, x_hat_new;
};
#endif
================================================
FILE: experimental_testing/RobotArduinoTesting/kalman_test/kalman_test.ino
================================================
#include "KalmanFilter.h"
#include "LinearAlgebra.h"
mat A = mat::identity(6);
mat B = mat::zeros(6,3);
mat C = mat::zeros(3,6);
mat Q = mat::identity(6);
mat R = mat::zeros(3,3);
mat P = mat::identity(6);
KalmanFilter kf = KalmanFilter(0.1, A, B, C, Q, R, P);
void setup()
{
A(3,3) = 0;
A(4,4) = 0;
A(5,5) = 0;
B(3,0) = 1;
B(4,1) = 1;
B(5,2) = 1;
C(0,0) = 1;
C(1,1) = 1;
C(2,2) = 1;
kf.init();
}
void loop()
{
double dt = 0.1;
A(0,3) = dt;
A(1,4) = dt;
A(2,6) = dt;
mat u = mat::ones(1,3);
mat y = mat::ones(1,3);
kf.predict(dt, A, u);
kf.update(y);
mat x = kf.state();
String s;
x.print(s);
}
================================================
FILE: experimental_testing/RobotArduinoTesting/lifter_nano_test/lifter_nano_test.ino
================================================
#define LIFTER_PIN_0 1
#define LIFTER_PIN_1 2
#define LIFTER_PIN_2 3
#define FEEDBACK_PIN 4
void setup ()
{
Serial.begin(115200);
pinMode(LIFTER_PIN_0, OUTPUT);
pinMode(LIFTER_PIN_1, OUTPUT);
pinMode(LIFTER_PIN_2, OUTPUT);
pinMode(FEEDBACK_PIN, INPUT_PULLUP);
}
int getCommand()
{
String instr = "";
int outCmd = 0;
if(Serial.available())
{
instr = Serial.readString();
outCmd = instr.toInt();
}
return outCmd;
}
void sendPos(int num)
{
int p2 = num / 4;
int r = num % 4;
int p1 = r / 2;
r = r % 2;
int p0 = r / 1;
digitalWrite(LIFTER_PIN_0, p0);
digitalWrite(LIFTER_PIN_1, p1);
digitalWrite(LIFTER_PIN_2, p2);
}
bool getFeedback()
{
return !digitalRead(FEEDBACK_PIN);
}
void loop()
{
int cmd = getCommand();
bool in_progress = getFeedback();
if(!in_progress && cmd > 0)
{
sendPos(cmd);
}
delay(100);
}
================================================
FILE: experimental_testing/RobotArduinoTesting/motor_driver_test_script/motor_driver_test_script.ino
================================================
#include "ClearCore.h"
#define LIFTER_MOTOR ConnectorM3
#define INCREMENTAL_UP_PIN DI7
#define INCREMENTAL_DOWN_PIN DI6
#define LATCH_SERVO_PIN IO0 // Only IO0 does pwm
#define HOMING_SWITCH_PIN IO4
#define LIFTER_STEPS_PER_REV 800
#define LIFTER_MAX_VEL 7 // revs/sec
#define LIFTER_MAX_ACC 10 // rev/sec^2
// Max num steps ~51000
#define SAFETY_MAX_POS 120 // Revs, Sanity check on desired position to make sure it isn't larger than this
#define SAFETY_MIN_POS 0 // Revs, Sanity check on desired position to make sure it isn't less than this
#define LATCH_ACTIVE_MS 2000
#define LATCH_OPEN_DUTY_CYCLE 50
#define LATCH_CLOSE_DUTY_CYCLE 200
void setup()
{
pinMode(INCREMENTAL_UP_PIN, INPUT);
pinMode(INCREMENTAL_DOWN_PIN, INPUT);
pinMode(HOMING_SWITCH_PIN, INPUT);
pinMode(LATCH_SERVO_PIN, OUTPUT);
// Sets the input clocking rate. This normal rate is ideal for ClearPath
// step and direction applications.
MotorMgr.MotorInputClocking(MotorManager::CLOCK_RATE_LOW);
// Sets all motor connectors into step and direction mode.
MotorMgr.MotorModeSet(MotorManager::MOTOR_ALL, Connector::CPM_MODE_STEP_AND_DIR);
LIFTER_MOTOR.VelMax(LIFTER_MAX_VEL*LIFTER_STEPS_PER_REV);
LIFTER_MOTOR.AccelMax(LIFTER_MAX_ACC*LIFTER_STEPS_PER_REV);
LIFTER_MOTOR.HlfbMode(MotorDriver::HLFB_MODE_STATIC);
LIFTER_MOTOR.PolarityInvertSDEnable(true);
LIFTER_MOTOR.EnableRequest(true);
Serial.begin(115200);
Serial.println("Test script starting");
}
void test_motor()
{
bool vel_up = digitalRead(INCREMENTAL_UP_PIN);
bool vel_down = digitalRead(INCREMENTAL_DOWN_PIN);
Serial.print("Up: ");
Serial.print(vel_up);
Serial.print(", down: ");
Serial.println(vel_down);
if(vel_up)
{
LIFTER_MOTOR.MoveVelocity(LIFTER_MAX_VEL*LIFTER_STEPS_PER_REV);
Serial.println(LIFTER_MOTOR.PositionRefCommanded());
}
else if(vel_down)
{
LIFTER_MOTOR.MoveVelocity(-1*LIFTER_MAX_VEL*LIFTER_STEPS_PER_REV);
Serial.println(LIFTER_MOTOR.PositionRefCommanded());
}
else
{
LIFTER_MOTOR.MoveStopAbrupt();
}
}
void test_homing_switch()
{
bool switch_active = !digitalRead(HOMING_SWITCH_PIN);
if(switch_active) {
Serial.print("Homing switch: ");
Serial.println(switch_active);
LIFTER_MOTOR.PositionRefSet(0);
}
}
void test_servo()
{
Serial.println("Servo open");
analogWrite(LATCH_SERVO_PIN, LATCH_OPEN_DUTY_CYCLE);
delay(1000);
Serial.println("Servo closed");
analogWrite(LATCH_SERVO_PIN, LATCH_CLOSE_DUTY_CYCLE);
delay(1000);
}
void loop()
{
test_motor();
test_homing_switch();
//test_servo();
delay(100);
}
================================================
FILE: experimental_testing/RobotArduinoTesting/new_dc_motor_tuning/Motor.cpp
================================================
#include "Motor.h"
#include "constants.h"
constexpr double COUNTS_TO_RADS = 2.0 * PI / static_cast<double>(COUNTS_PER_OUTPUT_SHAFT_REV);
Motor::Motor(int pwmPin, int dirPin, int encPinA, int encPinB, double Kp, double Ki, double Kd)
: pwmPin_(pwmPin),
dirPin_(dirPin),
inputVel_(0.0),
currentVelRaw_(0.0),
currentVelFiltered_(0.0),
pidOut_(0.0),
outputCmd_(0.0),
prevCount_(0),
prevMicros_(micros()),
enc_(encPinA, encPinB),
controller_(¤tVelFiltered_, &pidOut_, &inputVel_, Kp, Ki, Kd, DIRECT),
velFilter_(LOWPASS, VEL_FILTER_FREQ)
{
pinMode(pwmPin_, OUTPUT);
pinMode(dirPin_, OUTPUT);
controller_.SetMode(AUTOMATIC);
controller_.SetOutputLimits(-255, 255);
controller_.SetSampleTime(5);
}
void Motor::setCommand(double vel)
{
inputVel_ = vel;
}
float Motor::getCurrentVelocity()
{
return static_cast<float>(currentVelFiltered_);
}
long Motor::getCounts()
{
return enc_.read();
}
void Motor::runLoop(bool pub)
{
// Read current values
long curCount = -1*enc_.read();
unsigned long curMicros = micros();
// Compute delta
long deltaCount = curCount - prevCount_;
double deltaRads = static_cast<double>(deltaCount) * COUNTS_TO_RADS;
unsigned long deltaMicros = curMicros - prevMicros_;
if(deltaMicros == 0)
{
// This will break things due to div by 0, so just skip
return;
}
// Copy current values into previous
prevCount_ = curCount;
prevMicros_ = curMicros;
// Do a check for large time deltas. This likely means the controller hasn't run for a while and we should ignore this value
if(deltaMicros > 100000)
{
return;
}
// Compute current velocity in rads/second
currentVelRaw_ = 1000000.0 * deltaRads / static_cast<double>(deltaMicros);
currentVelFiltered_ = currentVelRaw_; // velFilter_.input(currentVelRaw_);
// Run PID controller
controller_.Compute();
/* Use output from PID to update our current command. Since this is a velocity controller, when the error is 0
* and the PID controller drives the output to 0, we actually want to maintain a certian PWM value. Hence, the
* PID output is used to modify our control singal and not drive it directly.
*/
outputCmd_ += int(pidOut_);
//outputCmd_ = static_cast<int>(pidOut_ + inputVel_ * 10);
// Set a deadband region based on input vel to avoid integral windup
if(fabs(inputVel_) < 0.001)
{
outputCmd_ = 0;
}
// Make sure we don't exceed max/min power
if(outputCmd_ > 255)
{
outputCmd_ = 255;
}
else if(outputCmd_ < -255)
{
outputCmd_ = -255;
}
// Update output direction
if(outputCmd_ < 0)
{
digitalWrite(dirPin_,1);
}
else
{
digitalWrite(dirPin_,0);
}
// Actually write out the motor power
analogWrite(pwmPin_, abs(outputCmd_));
if (pub)
{
// Debugging prints
//Serial.print(deltaMicros);
//Serial.print(" ");
//Serial.print(curCount);
//Serial.print(" ");
Serial.print("currentVelRaw_:");
Serial.print(currentVelRaw_, 5);
Serial.print(" ");
Serial.print("currentVelFiltered_:");
Serial.print(currentVelFiltered_, 5);
Serial.print(" ");
Serial.print("inputVel_:");
Serial.print(inputVel_, 5);
Serial.print(" ");
//Serial.print(pidOut_, 5);
//Serial.print(" ");
Serial.print("outputCmd_:");
Serial.print(outputCmd_/10.0);
Serial.println("");
}
}
================================================
FILE: experimental_testing/RobotArduinoTesting/new_dc_motor_tuning/Motor.h
================================================
#ifndef Motor_h
#define Motor_h
#include <Encoder.h>
#include <PID_v1.h>
#include <Filters.h>
class Motor
{
public:
/*
* Motor object constructor
* pwmPin - which pin the motor PWM is connected to
* dirPin - which pin the motor direction is connected to
* encPinA - which pin the encoder wire A is connected to. This should be a pin capable of handling interrupts
* encPinB - which pin the encoder wire B is connected to. This should ideally (but not required) be a pin capable of handling interrupts
* Kp - Proportional gain
* Ki - Integral gain
* Kd - Derrivative gain
*/
Motor(int pwmPin, int dirPin, int encPinA, int encPinB, double Kp, double Ki, double Kd);
/*
* Set the desired velocity in rad/second
*/
void setCommand(double vel);
/*
* Run the actual controller. Make sure this gets called reasonably quickly (i.e. every 20 ms or so)
*/
void runLoop(bool pub);
/*
* Get the current measured motor velocity in rad/s
*/
float getCurrentVelocity();
// Get number of counts from encoder - for debugging
long getCounts();
private:
// Pins
int pwmPin_;
int dirPin_;
// Values for computation
double inputVel_; // Desired velocity in rad/sec
double currentVelRaw_; // Raw current velocity in rad/sec
double currentVelFiltered_; // Filtered velocity in rad/sec
double pidOut_; // Output from PID controller
int outputCmd_; // Output command in [-255, 255]
long prevCount_; // Encoder count from previous loop
// Timer
unsigned long prevMicros_; // Timer from previous loop
// Other objects
Encoder enc_; // Encoder object
PID controller_; // PID controller
FilterOnePole velFilter_; // Velocity lowpass filter
};
#endif Motor_h
================================================
FILE: experimental_testing/RobotArduinoTesting/new_dc_motor_tuning/constants.h
================================================
#ifndef Constants_h
#define Constants_h
// Pins
#define PIN_PWM_0 3
#define PIN_PWM_1 4
#define PIN_PWM_2 5
#define PIN_PWM_3 6
#define PIN_DIR_0 41
#define PIN_DIR_1 40
#define PIN_DIR_2 39
#define PIN_DIR_3 38
#define PIN_ENABLE_0 49
#define PIN_ENABLE_1 48
#define PIN_ENABLE_2 47
#define PIN_ENABLE_3 46
#define PIN_ENC_A_0 21
#define PIN_ENC_B_0 25
#define PIN_ENC_A_1 20
#define PIN_ENC_B_1 23
#define PIN_ENC_A_2 19
#define PIN_ENC_B_2 24
#define PIN_ENC_A_3 18
#define PIN_ENC_B_3 22
#define PIN_LATCH_SERVO_PIN 12
#define PIN_TRAY_STEPPER_LEFT_PULSE 46
#define PIN_TRAY_STEPPER_LEFT_DIR 47
#define PIN_TRAY_STEPPER_RIGHT_PULSE 44
#define PIN_TRAY_STEPPER_RIGHT_DIR 43
#define PIN_TRAY_HOME_SWITCH 53
// Velocitiy limits
#define MAX_TRANS_SPEED_FINE 0.08 // m/s
#define MAX_ROT_SPEED_FINE 0.5 // rad/2
#define MAX_TRANS_SPEED_COARSE 0.5 // m/s
#define MAX_ROT_SPEED_COARSE 1.0 // rad/2
// Acceleration limits
#define MAX_TRANS_ACC_FINE 0.1 // m/s^2
#define MAX_ROT_ACC_FINE 0.5 // rad/s^2
#define MAX_TRANS_ACC_COARSE 0.5 // m/s^2
#define MAX_ROT_ACC_COARSE 1.0 // rad/s^2
// Cartesian control gains
#define CART_TRANS_KP 2
#define CART_TRANS_KI 0.1
#define CART_TRANS_KD 0
#define CART_ROT_KP 3
#define CART_ROT_KI 0.5
#define CART_ROT_KD 0
// Motor control gains
#define MOTOR_KP 1
#define MOTOR_KI 0
#define MOTOR_KD 0
// Motor control constants
#define VEL_FILTER_FREQ 20 // HZ
#define COUNTS_PER_OUTPUT_SHAFT_REV 36000 // Manually measrued/estimated
// Physical dimensions
#define WHEEL_DIAMETER 0.152 // meters
#define WHEEL_DIST_FROM_CENTER 0.4572 // meters
// Scaling factors
#define TRAJ_MAX_FRACTION 0.7 // Only generate a trajectory to this fraction of max speed to give motors headroom to compensate
// Kalman filter scales
#define PROCESS_NOISE_SCALE 0.08
#define MEAS_NOISE_SCALE 0.01
#define MEAS_NOISE_VEL_SCALE_FACTOR 10000
// Possition accuracy targets
#define TRANS_POS_ERR_COARSE 0.10 // m
#define ANG_POS_ERR_COARSE 0.08 // rad
#define TRANS_VEL_ERR_COARSE 0.05 // m/s
#define ANG_VEL_ERR_COARSE 0.05 // rad/s
#define TRANS_POS_ERR_FINE 0.01 // m
#define ANG_POS_ERR_FINE 0.02 // rad
#define TRANS_VEL_ERR_FINE 0.01 // m/s
#define ANG_VEL_ERR_FINE 0.01 // rad/s
// Set debug printing, comment out to skip debug printing
#define PRINT_DEBUG true
// Tray stepper control values
#define TRAY_STEPPER_STEPS_PER_REV 200 // Steps per rev for tray servos
#define TRAY_DIST_PER_REV 1.8 // mm of linear travel per stepper revolution
#define TRAY_MAX_LINEAR_TRAVEL 300 // mm of total linear travel possible
// Note: all tray positions measured in mm from home pos
#define TRAY_DEFAULT_POS_MM 100 // Default position for driving in mm
#define TRAY_LOAD_POS_MM 50 // Loading position in mm
#define TRAY_PLACE_POS_MM 250 // Placing position in mm
#define TRAY_MAX_SPEED 1000 // Max tray speed in steps/sec
#define TRAY_MAX_ACCEL 2000 // Max tray acceleration in steps/sec/sec
// Tray servo control values
#define LATCH_CLOSE_POS 20 // Latch servo position for close in degrees
#define LATCH_OPEN_POS 150 // Latch servo position for open in degrees
#define TRAY_SERVO_MIN_PW 1000 // Min pulse witdh in microseconds corresponding to 0 position
#define TRAY_SERVO_MAX_PW 2000 // Max pulse witdh in microseconds corresponding to 180 position
#define TRAY_PLACEMENT_PAUSE_TIME 3000 // How many ms to wait after opening the latch for placement
enum COMMAND
{
NONE,
MOVE,
MOVE_REL,
MOVE_FINE,
PLACE_TRAY,
LOAD_TRAY,
INITIALIZE_TRAY,
POSITION,
ESTOP,
LOAD_COMPLETE,
};
#endif
================================================
FILE: experimental_testing/RobotArduinoTesting/new_dc_motor_tuning/new_dc_motor_tuning.ino
================================================
#include "constants.h"
#include "Motor.h"
double Kp = 14;
double Ki = 0.1;
double Kd = 0.08;
Motor m = Motor(PIN_PWM_2, PIN_DIR_2, PIN_ENC_A_2, PIN_ENC_B_2, Kp, Ki, Kd);
int step = 0;
unsigned long prevStepTime = millis();
unsigned long prevLoopTime = millis();
double cur_speed = 0;
double max_vel = 2;
double max_acc = 5;
double max_acc_time = 1000 * max_vel / max_acc;
template <typename T> int sgn(T val) {
return (T(0) < val) - (val < T(0));
}
void setup()
{
pinMode(PIN_ENABLE_3, OUTPUT);
digitalWrite(PIN_ENABLE_3, HIGH);
Serial.begin(115200);
//Serial.println("Start motor tuning");
}
double computeAcc(double dt, double acc)
{
cur_speed += dt/1000.0 * acc;
if (fabsf(cur_speed) > max_vel)
{
cur_speed = sgn(cur_speed) * max_vel;
}
}
void runAccStep(int waitTime, double acc)
{
unsigned long curTime = millis();
unsigned long stepTime = curTime - prevStepTime;
if (stepTime > waitTime)
{
step += 1;
prevStepTime = millis();
prevLoopTime = millis();
}
else
{
unsigned long loopTime = curTime - prevLoopTime;
computeAcc(loopTime, acc);
prevLoopTime = millis();
}
}
double runCVStep(int waitTime, double vel)
{
unsigned long curTime = millis();
unsigned long stepTime = curTime - prevStepTime;
if (stepTime > waitTime)
{
step += 1;
prevStepTime = millis();
prevLoopTime = millis();
}
else
{
cur_speed = vel;
}
}
double runSteps()
{
if (step == 0)
{
int stepTime = max_acc_time;
double stepAcc = max_acc;
runAccStep(stepTime, stepAcc);
}
else if (step == 1)
{
int stepTime = 2000;
runCVStep(stepTime, max_vel);
}
else if (step == 2)
{
int stepTime = max_acc_time;
double stepAcc = -1*max_acc;
runAccStep(stepTime, stepAcc);
}
else if (step == 3)
{
int stepTime = 2000;
runCVStep(stepTime, 0);
}
else if (step == 4)
{
int stepTime = max_acc_time;
double stepAcc = -1 * max_acc;
runAccStep(stepTime, stepAcc);
}
else if (step == 5)
{
int stepTime = 2000;
runCVStep(stepTime, -1*max_vel);
}
else if (step == 6)
{
int stepTime = max_acc_time;
double stepAcc = max_acc;
runAccStep(stepTime, stepAcc);
}
else if (step == 7)
{
int stepTime = 2000;
runCVStep(stepTime, 0);
}
else if (step >= 8)
{
step = 0;
}
}
void loop()
{
runSteps();
m.setCommand(cur_speed);
bool motor_pub = true;
if(step == 3 || step == 7) {motor_pub = false;}
m.runLoop(motor_pub);
// analogWrite(PIN_PWM_2, s);
// Serial.println(s);
// s += 1;
delay(15);
}
================================================
FILE: experimental_testing/RobotArduinoTesting/single_motor_speed_control/Motor.cpp
================================================
#include "Motor.h"
#include <math.h>
Motor::Motor(int pwmPin, int dirPin, int encPinA, int encPinB, double Kp, double Ki, double Kd, float velFilterFreq)
: pwmPin_(pwmPin),
dirPin_(dirPin),
inputVel_(0.0),
currentVelRaw_(0.0),
currentVelFiltered_(0.0),
outputCmd_(0.0),
prevCount_(0),
prevMillis_(millis()),
enc_(encPinA, encPinB),
controller_(¤tVelFiltered_, &outputCmd_, &inputVel_, Kp, Ki, Kd, DIRECT),
velFilter_(LOWPASS, velFilterFreq)
{
pinMode(pwmPin_, OUTPUT);
pinMode(dirPin_, OUTPUT);
controller_.SetMode(AUTOMATIC);
controller_.SetOutputLimits(-255, 255);
controller_.SetSampleTime(5);
}
void Motor::setCommand(double vel)
{
inputVel_ = vel;
}
void Motor::runLoop()
{
// Read current values
long curCount = enc_.read();
unsigned long curMillis = millis();
// Compute delta
long deltaCount = curCount - prevCount_;
double deltaRevs = static_cast<double>(deltaCount) / COUNTS_PER_SHAFT_REV;
unsigned long deltaMillis = curMillis - prevMillis_;
// Copy current values into previous
prevCount_ = curCount;
prevMillis_ = curMillis;
// Do a check for large time deltas. This likely means the controller hasn't run for a while and we should ignore this value
if(deltaMillis > 100)
{
return;
}
// Compute current velocity in revs/second
currentVelRaw_ = 1000.0 * deltaRevs / static_cast<double>(deltaMillis);
currentVelFiltered_ = velFilter_.input(currentVelRaw_);
// Run PID controller
controller_.Compute();
// Update output values
if(outputCmd_ < 0)
{
digitalWrite(dirPin_,0);
}
else
{
digitalWrite(dirPin_,1);
}
analogWrite(pwmPin_, abs(outputCmd_));
// Serial.print(curCount);
// Serial.print(" ");
// Serial.print(currentVelRaw_);
// Serial.print(" ");
Serial.print(currentVelFiltered_);
Serial.print(" ");
Serial.print(inputVel_);
Serial.print(" ");
Serial.print(outputCmd_/255.0);
// Serial.print(" ");
// Serial.print(deltaMillis);
Serial.println("");
}
================================================
FILE: experimental_testing/RobotArduinoTesting/single_motor_speed_control/Motor.h
================================================
/* Requires libraries:
* Filters: https://github.com/JonHub/Filters
* PID: https://playground.arduino.cc/Code/PIDLibrary/
* Encoder: https://www.pjrc.com/teensy/td_libs_Encoder.html
*/
#ifndef Motor_h
#define Motor_h
#include <Encoder.h>
#include <PID_v1.h>
#include <Filters.h>
const unsigned int COUNTS_PER_MOTOR_REV = 44;
const unsigned int MOTOR_GEAR_RATIO = 40;
const double FUDGE_FACTOR = 1.5;
const double COUNTS_PER_SHAFT_REV = COUNTS_PER_MOTOR_REV * MOTOR_GEAR_RATIO * FUDGE_FACTOR;
class Motor
{
public:
/*
* Motor object constructor
* pwmPin - which pin the motor PWM is connected to
* dirPin - which pin the motor direction is connected to
* encPinA - which pin the encoder wire A is connected to. This should be a pin capable of handling interrupts
* encPinB - which pin the encoder wire B is connected to. This should ideally (but not required) be a pin capable of handling interrupts
* Kp - Proportional gain
* Ki - Integral gain
* Kd - Derrivative gain
* velFilterFreq - frequency to use for velocity lowpass filter
*/
Motor(int pwmPin, int dirPin, int encPinA, int encPinB, double Kp, double Ki, double Kd, float velFilterFreq);
/*
* Set the desired velocity in revs/second
*/
void setCommand(double vel);
/*
* Run the actual controller. Make sure this gets called reasonably quickly (i.e. every 20 ms or so)
*/
void runLoop();
private:
// Pins
int pwmPin_;
int dirPin_;
// Values for computation
double inputVel_; // Desired velocity in revs/sec
double currentVelRaw_; // Raw current velocity in revs/sec
double currentVelFiltered_;
double outputCmd_; // Output command in [-255, 255]
long prevCount_; // Encoder count from previous loop
// Timer
unsigned long prevMillis_; // Timer from previous loop
// Other objects
Encoder enc_; // Encoder object
PID controller_; // PID controller
FilterOnePole velFilter_; // Velocity lowpass filter
};
#endif Motor_h
================================================
FILE: experimental_testing/RobotArduinoTesting/single_motor_speed_control/single_motor_speed_control.ino
================================================
/* Requires libraries:
* Filters: https://github.com/JonHub/Filters
* PID: https://playground.arduino.cc/Code/PIDLibrary/
* Encoder: https://www.pjrc.com/teensy/td_libs_Encoder.html
*/
#include "Motor.h"
// Pinouts
#define PIN_ENABLE 52
#define PIN_BL_PWM 6
#define PIN_BL_DIR 48
#define PIN_BL_ENC_A 19
#define PIN_BL_ENC_B 29
#define VEL_FILTER_FREQ 10 // HZ
const double Kp = 200;
const double Ki = 2000;
const double Kd = 0;
Motor myMotor = Motor(PIN_BL_PWM, PIN_BL_DIR, PIN_BL_ENC_A, PIN_BL_ENC_B, Kp, Ki, Kd, VEL_FILTER_FREQ);
void setup()
{
// Setup pin modes
pinMode(PIN_ENABLE,OUTPUT);
Serial.begin(250000);
}
void setMotorCommands(float rps)
{
int curDirection = 1;
float curSpeed = curDirection * rps;
// Serial.print("Direction ");
// Serial.print(curDirection);
// Serial.print(", Speed ");
// Serial.println(curSpeed);
myMotor.setCommand(curSpeed);
digitalWrite(PIN_ENABLE, 1);
}
void runMotors()
{
unsigned long prevTime = millis();
unsigned long startTime = prevTime;
int targetDelta = 15;
int targetTotalTime = 3000;
while (millis() - startTime < targetTotalTime)
{
if(millis() - prevTime > targetDelta)
{
prevTime = millis();
myMotor.runLoop();
}
}
}
void loop()
{
// // Get revolutions per second
// Serial.println("Please enter target revolutions per second");
// bool gotInput = false;
// float rps = 0.0;
// while(!gotInput)
// {
// if(Serial.available() > 0)
// {
// rps = Serial.parseFloat();
// if(rps == 0)
// {
// continue;
// }
// else
// {
// break;
// }
// }
// delay(10);
// }
delay(1000);
float rps = 0.5;
setMotorCommands(rps);
runMotors();
// Stop motors
digitalWrite(PIN_ENABLE, 0);
}
================================================
FILE: experimental_testing/RobotArduinoTesting/stepper_nano_test/stepper_nano_test.ino
================================================
// Uses stepper driver library from here: https://www.airspayce.com/mikem/arduino/AccelStepper/classAccelStepper.html
#include <AccelStepper.h>
#define STEP_PIN 2
#define DIR_PIN 1 // Note that the DIR pin is actually controlled by the mster arduino so this won't be connected to anything
#define MILLIS_BETWEEN_PRINTING 100
const int max_vel = 800; //steps/second
AccelStepper motor;
unsigned long count = 0;
unsigned long prevMillisPrint;
void setup()
{
Serial.begin(115200);
Serial.println("Starting...");
motor = AccelStepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
motor.setMaxSpeed(max_vel);
motor.setAcceleration(max_vel);
motor.setMinPulseWidth(10); // Min from docs is 2.5 microseconds
prevMillisPrint = millis();
motor.move(800);
}
void loop()
{
if(motor.isRunning() && millis() - prevMillisPrint > MILLIS_BETWEEN_PRINTING)
{
Serial.print(motor.speed());
Serial.print(", ");
Serial.println(motor.currentPosition());
prevMillisPrint = millis();
count = 0;
}
motor.run();
count++;
}
================================================
FILE: experimental_testing/RobotArduinoTesting/test_encoders/test_encoders.ino
================================================
// Pins
#define PIN_ENCA_1 21
#define PIN_ENCA_2 20
#define PIN_ENCA_3 19
#define PIN_ENCA_4 18
#define PIN_ENCB_1 25
#define PIN_ENCB_2 23
#define PIN_ENCB_3 24
#define PIN_ENCB_4 22
#include <Encoder.h>
Encoder e[4] = { Encoder(PIN_ENCA_1, PIN_ENCB_1),
Encoder(PIN_ENCA_2, PIN_ENCB_2),
Encoder(PIN_ENCA_3, PIN_ENCB_3),
Encoder(PIN_ENCA_4, PIN_ENCB_4) } ;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
Serial.println("Encoder Test:");
}
void loop() {
Serial.print("Pos: [");
for (int i = 0; i < 4; ++i )
{
Serial.print(e[i].read());
Serial.print(", ");
}
Serial.println("]");
// if a character is sent from the serial monitor,
// reset both back to zero.
if (Serial.available())
{
Serial.read();
Serial.println("Reset position to zero");
e[0].write(0);
e[1].write(0);
e[2].write(0);
e[3].write(0);
}
delay(100);
}
================================================
FILE: experimental_testing/TrajGen.py
================================================
# Testing out trajectory generation where I have a gui
import math
import numpy as np
from matplotlib import pyplot as plt
# Parameters
MAX_VEL = 3.0 # m/s
MAX_ACC = 2.0 # m/s^2
# Find min time and position change to reach full speed
T_ACC = MAX_VEL / MAX_ACC
print("T_ACC: {}".format(T_ACC))
P_ACC = 0.5 * MAX_ACC * T_ACC * T_ACC
print("P_ACC: {}".format(P_ACC))
def sign(n):
if n > 0:
return 1
elif n < 0:
return -1
else:
return 0
def gen_traj2(p1, p2, dt):
position_change = abs(p2 - p1)
# Determine if position change is less than required for steady state
if position_change < 2*P_ACC:
return gen_triangle_traj(p1, p2, dt)
else:
return gen_trap_traj(p1, p2, dt)
def gen_triangle_traj(p1, p2, dt):
turn_pt = (p2 - p1)/2
dir = sign(p2 - p1)
traj = {'time': [0], 'pos': [p1], 'vel': [0], 'acc': [dir * MAX_ACC]}
gen_const_acc_until_pos(traj, dt, dir*MAX_ACC, turn_pt, dir)
gen_const_acc_until_pos(traj, dt, -1*dir*MAX_ACC, p2, dir)
return traj
def gen_trap_traj(p1, p2, dt):
dir = sign(p2 - p1)
max_vel_pt = dir*P_ACC
slow_down_pt = p2 - dir * P_ACC
traj = {'time': [0], 'pos': [p1], 'vel': [0], 'acc': [dir * MAX_ACC]}
gen_const_acc_until_pos(traj, dt, dir*MAX_ACC, max_vel_pt, dir)
gen_const_acc_until_pos(traj, dt, 0, slow_down_pt, dir)
gen_const_acc_until_pos(traj, dt, -1*dir*MAX_ACC, p2, dir)
return traj
def gen_const_acc_until_pos(traj, dt, acc, stop_pos, stop_dir):
"""
Generate constant acceleration trajectory
"""
n_iter = 0
while True:
new_vel = traj['vel'][-1] + acc * dt
new_pos = traj['pos'][-1] + traj['vel'][-1] * dt
new_time = traj['time'][-1] + dt
if stop_dir > 0 and new_vel < 0:
break
if stop_dir < 0 and new_vel > 0:
break
if stop_dir > 0 and new_pos >= stop_pos:
break
elif stop_pos < 0 and new_pos <= stop_pos:
break
elif n_iter > 100000:
raise StopIteration("Too many iterations")
traj['time'].append(new_time)
traj['pos'].append(new_pos)
traj['vel'].append(new_vel)
traj['acc'].append(acc)
n_iter += 1
return traj
def plot_traj(pvt_data):
"""
Makes a plot of position, velocity, and acceleration versus time
"""
ax = plt.axes()
ax.plot(pvt_data["time"], pvt_data["pos"],'r',label='Position')
ax.plot(pvt_data["time"], pvt_data["vel"],'g',label='Velocity')
ax.plot(pvt_data["time"], pvt_data["acc"],'b',label='Acceleration')
ax.legend()
plt.show()
if __name__ == '__main__':
p1 = 0
p2 = 20
dt = 0.05
pvt = gen_traj2(p1, p2, dt)
plot_traj(pvt)
================================================
FILE: experimental_testing/TrajGenv2.py
================================================
import math
import numpy as np
from matplotlib import pyplot as plt
p_target = 20.0 # m
v_max = 1.0 #m/s
a_max = 2.0 #m/s^2
j_max = 8.0 #m/s^3
alpha = 0.8 # velocity decay
beta = 0.8 # acceleation decay
loop_limit = 10 # max loops for convergence
plot_timestep = 0.01
def generate(target_position):
v_lim = v_max
a_lim = a_max
j_lim = j_max
output = {}
output['done'] = False
for i in range(loop_limit):
print("Starting loop {}".format(i))
output = generate_once(target_position, v_lim, a_lim, j_lim)
if output['done']:
break
else:
v_lim = output['v_lim']
a_lim = output['a_lim']
j_lim = output['j_lim']
if output['done']:
print("Trajectory found")
return output
else:
print("Trajectory not found")
return {}
def generate_once(p, v_lim, a_lim, j_lim):
output = {}
output['done'] = False
output['v_lim'] = v_lim
output['a_lim'] = a_lim
output['j_lim'] = j_lim
output['t'] = []
# Constant jerk regions
dt_j = a_lim / j_lim
print("dt_j = {}".format(dt_j))
dv_j = 0.5 * j_lim * dt_j ** 2 # Positive jerk region
dp_j1 = 1/6.0 * j_lim * dt_j ** 3 # Positive jerk region
dp_j2 = (v_lim - dv_j) * dt_j + 0.5 * a_lim * dt_j ** 2 - 1/6 * j_lim * dt_j ** 3 # Negative jerk region
print("dv_j: {}, dp_j1: {}, dp_j2: {}".format(dv_j, dp_j1, dp_j2))
# Constant accel region
dt_a = (v_lim - 2*dv_j ) / a_lim
print("dt_a = {}".format(dt_a))
if dt_a <= 0:
output['a_lim'] = a_lim * beta
print("New a_lim = {}".format(output['a_lim'] ))
return output
dp_a = dv_j * dt_a + 0.5 * a_lim * dt_a ** 2
print("dp_a: {}".format(dp_a))
# Constant vel region
dt_v = (p - 2 * dp_j1 - 2 * dp_j2 - 2 * dp_a) / v_lim
print("dt_v = {}".format(dt_v))
if dt_v <= 0:
output['v_lim'] = alpha * v_lim
print("New v_lim = {}".format(output['v_lim'] ))
return output
dp_v = v_lim * dt_v
print("dp_v: {}".format(dp_v))
# If we get here, the genertation should be correct, so compute time regions and return
output['done'] = True
t = [0,0,0,0,0,0,0,0]
t[0] = 0
t[1] = t[0] + dt_j
t[2] = t[1] + dt_a
t[3] = t[2] + dt_j
t[4] = t[3] + dt_v
t[5] = t[4] + dt_j
t[6] = t[5] + dt_a
t[7] = t[6] + dt_j
output['t'] = t
print("Times: {}".format(t))
return output
def generate_inverse(p, dt_j, dt_a, dt_v):
output = {}
output['done'] = True
t = [0,0,0,0,0,0,0,0]
t[0] = 0
t[1] = t[0] + dt_j
t[2] = t[1] + dt_a
t[3] = t[2] + dt_j
t[4] = t[3] + dt_v
t[5] = t[4] + dt_j
t[6] = t[5] + dt_a
t[7] = t[6] + dt_j
output['t'] = t
# Solve system of equations to get kinematic limits
A = np.array([
[dt_j, -1, 0 ],
[dt_j ** 2, dt_a, -1 ],
[(dt_j ** 2) * (dt_a - dt_j), dt_j ** 2 + dt_a ** 2, dt_v + 2* dt_j ]])
b = np.array([0, 0, p])
lims = np.linalg.solve(A, b)
output['v_lim'] = lims[2]
output['a_lim'] = lims[1]
output['j_lim'] = lims[0]
return output
def generate_profile_from_params(output, timestep):
j_lim = output['j_lim']
a_lim = output['a_lim']
v_lim = output['v_lim']
T = output['t']
t_vals = np.arange(0, T[7], timestep)
p = [0]
v = [0]
a = [0]
j = [j_lim]
for t in t_vals[1:]:
if t >= T[0] and t < T[1]:
p.append(p[-1] + v[-1] * timestep + 0.5 * a[-1] * timestep **2 + 1/6.0 * j[-1] * timestep ** 3)
v.append(v[-1] + a[-1] * timestep + 0.5* j[-1] * timestep ** 2)
a.append(a[-1] + j[-1] * timestep)
j.append(j_lim)
elif t >= T[1] and t < T[2]:
p.append(p[-1] + v[-1] * timestep + 0.5 * a[-1] * timestep **2 + 1/6.0 * j[-1] * timestep ** 3)
v.append(v[-1] + a[-1] * timestep + 0.5* j[-1] * timestep ** 2)
a.append(a_lim)
j.append(0)
elif t >= T[2] and t < T[3]:
p.append(p[-1] + v[-1] * timestep + 0.5 * a[-1] * timestep **2 + 1/6.0 * j[-1] * timestep ** 3)
v.append(v[-1] + a[-1] * timestep + 0.5* j[-1] * timestep ** 2)
a.append(a[-1] + j[-1] * timestep)
j.append(-j_lim)
elif t >= T[3] and t < T[4]:
p.append(p[-1] + v[-1] * timestep + 0.5 * a[-1] * timestep **2 + 1/6.0 * j[-1] * timestep ** 3)
v.append(v_lim)
a.append(0)
j.append(0)
elif t >= T[4] and t < T[5]:
p.append(p[-1] + v[-1] * timestep + 0.5 * a[-1] * timestep **2 + 1/6.0 * j[-1] * timestep ** 3)
v.append(v[-1] + a[-1] * timestep + 0.5* j[-1] * timestep ** 2)
a.append(a[-1] + j[-1] * timestep)
j.append(-j_lim)
elif t >= T[5] and t < T[6]:
p.append(p[-1] + v[-1] * timestep + 0.5 * a[-1] * timestep **2 + 1/6.0 * j[-1] * timestep ** 3)
v.append(v[-1] + a[-1] * timestep + 0.5* j[-1] * timestep ** 2)
a.append(-a_lim)
j.append(0)
elif t >= T[6] and t < T[7]:
p.append(p[-1] + v[-1] * timestep + 0.5 * a[-1] * timestep **2 + 1/6.0 * j[-1] * timestep ** 3)
v.append(v[-1] + a[-1] * timestep + 0.5* j[-1] * timestep ** 2)
a.append(a[-1] + j[-1] * timestep)
j.append(j_lim)
return (t_vals, p, v, a, j)
def plot_data(t,p,v,a,j, output):
ax = plt.axes()
ax.plot(t, p,'r',label='Position', linewidth=4)
ax.plot(t, v,'g',label='Velocity', linewidth=4)
ax.plot(t, a,'b',label='Acceleration', linewidth=4)
ax.plot(t, j,'k',label='Jerk', linewidth=4)
ax.legend()
ax.set_xlabel('Time (s)')
ax.set_ylabel("Value (m, m/s, m/s^2, m/s^3)")
ax.set_title("Trajectory Generation\nDist: {} m, Vel limit: {} m/s, Acc limit: {} m/s^2, Jerk limit: {} m/s^3".format(p_target, v_max, a_max, j_max))
# Plot vertical lines
# ax.set_ylabel("Velocity (m/s)")
# ax.set_ylim((-0.5, 2))
# t_vert = output['t']
# y_vert = np.linspace(-10.0, 10.0, 100)
# for new_t in t_vert:
# ax.plot([new_t]*100, y_vert, 'k--', linewidth=3)
# ax.grid(True)
# ax.text(t_vert[0] + (t_vert[1] - t_vert[0])/6.0, -0.2, "Constant Jerk", fontsize='x-large')
# ax.text(t_vert[1] + (t_vert[2] - t_vert[1])/6.0, -0.1, "Constant Accel", fontsize='x-large')
# ax.text(t_vert[2] + (t_vert[3] - t_vert[2])/6.0, -0.2, "Constant Jerk", fontsize='x-large')
# ax.text(t_vert[3] + (t_vert[4] - t_vert[3])/3.0, 0.0, "Constant Velocity", fontsize='x-large')
# ax.text(t_vert[4] + (t_vert[5] - t_vert[4])/6.0, -0.2, "Constant Jerk", fontsize='x-large')
# ax.text(t_vert[5] + (t_vert[6] - t_vert[5])/6.0, -0.1, "Constant Accel", fontsize='x-large')
# ax.text(t_vert[6] + (t_vert[7] - t_vert[6])/6.0, -0.2, "Constant Jerk", fontsize='x-large')
plt.show()
def plot_trapazoidal_profile():
n_divisions = 91
a_steps = int(n_divisions/3)
t = np.linspace(0, 5, n_divisions)
a = [1]*a_steps + [0]*a_steps + [-1]*a_steps + [0]
v = [0]
p = [0]
for i in range(1, n_divisions):
dt = t[i] - t[i-1]
new_v = v[i-1] + a[i-1] * dt
new_p = p[i-1] + v[i-1] * dt + 0.5 * a[i-1] * dt * dt
v.append(new_v)
p.append(new_p)
ax = plt.axes()
# ax.plot(t, p,'r',label='Position')
ax.plot(t, v,'g',label='Velocity',linewidth=5)
# ax.plot(t, a,'b',label='Acceleration')
ax.set_xlabel('Time (s)')
ax.set_ylabel("Velocity (m/s)")
ax.legend()
# Plot vertical lines
t_vert = [t[0], t[a_steps], t[2*a_steps], t[3*a_steps]]
y_vert = np.linspace(-10.0, 10.0, 100)
for new_t in t_vert:
ax.plot([new_t]*100, y_vert, 'k--', linewidth=3)
ax.set_ylim((-0.5, 3))
ax.grid(True)
ax.text(t_vert[0] + (t_vert[1] - t_vert[0])/3.0, -0.2, "Constant Accel", fontsize='x-large')
ax.text(t_vert[1] + (t_vert[2] - t_vert[1])/3.0, -0.1, "Constant Velocity", fontsize='x-large')
ax.text(t_vert[2] + (t_vert[3] - t_vert[2])/3.0, -0.2, "Constant Accel", fontsize='x-large')
plt.show()
if __name__ == '__main__':
output = generate(p_target)
if output:
# Test inverse
dt_j = output['t'][1]
dt_a = output['t'][2] - output['t'][1]
dt_v = output['t'][4] - output['t'][3]
output2 = generate_inverse(p_target, dt_j, dt_a, dt_v)
eps = 0.01
all_valid = True
if output2:
if abs(output2['v_lim'] - output['v_lim']) / output['v_lim'] > eps:
print("v_lim not close. Expected: {}, Actual: {}, Percent diff: {}".format(output['v_lim'], output2['v_lim'], abs(output2['v_lim'] - output['v_lim']) / output['v_lim'] ))
all_valid = False
if abs(output2['a_lim'] - output['a_lim']) / output['a_lim'] > eps:
print("a_lim not close. Expected: {}, Actual: {}, Percent diff: {}".format(output['a_lim'], output2['a_lim'], abs(output2['a_lim'] - output['a_lim']) / output['a_lim'] ))
all_valid = False
if abs(output2['j_lim'] - output['j_lim']) / output['j_lim'] > eps:
print("j_lim not close. Expected: {}, Actual: {}, Percent diff: {}".format(output['j_lim'], output2['j_lim'], abs(output2['j_lim'] - output['j_lim']) / output['j_lim'] ))
all_valid = False
if all_valid:
print("All inverse values valid")
else:
print("Inverse failed")
# Generate plot
data = generate_profile_from_params(output, plot_timestep)
plot_data(*data, output)
# plot_trapazoidal_profile()
================================================
FILE: src/master/FieldPlanner.py
================================================
import matplotlib.image as mpimg
import matplotlib.pyplot as plt
import skimage.transform as sktf
import numpy as np
import matplotlib.patches as patches
import math
import enum
import logging
import os
import Utils
from Utils import ActionTypes
import config
import pickle
import csv
import copy
def generateVisionOffsetMap(cfg, max_x, max_y):
vision_offset_map = { (x,y): cfg.default_vision_offset for x in range(max_x) for y in range(max_y) }
with open(cfg.vision_offset_file) as csvfile:
reader = csv.reader(csvfile)
for idx,row in enumerate(reader):
if idx == 0:
continue
if row[0] == 'X':
tile_x = [x for x in range(max_x)]
else:
tile_x = [int(row[0])]
if row[1] == 'X':
tile_y = [ y for y in range(max_y)]
else:
tile_y = [int(row[1])]
offset_x_meters = int(row[2]) / 1000.0
offset_y_meters = int(row[3]) / 1000.0
offset_a_degrees = float(row[4])
for x in tile_x:
for y in tile_y:
key = (x,y)
add_value = np.array((offset_x_meters,offset_y_meters,offset_a_degrees))
vision_offset_map[key] = vision_offset_map[key] + add_value
return vision_offset_map
class DominoField:
"""
Data and methods for parsing and image and generating a field of tiles of dominos
"""
def __init__(self, cfg):
self.cfg = cfg
self.img = None
self.img_scaled = None
self.img_parsed_ids = None
self.img_parsed_color = None
self.n_tiles_x = 0
self.n_tiles_y = 0
self.tiles = []
def generate(self):
logging.info('Generating domino field from image...')
self._generateField()
logging.info('done.')
logging.info('Generating tiles from field...')
self._generateTiles()
logging.info('done.')
def printStats(self):
# Output some metrics
logging.info("Original image size: {}".format(self.img.shape[:2]))
logging.info("Scaled image size: {}".format(self.img_scaled.shape[:2]))
logging.info('Domino usage:')
logging.info('Total number of dominos: ' + str(self.img_parsed_ids.size))
logging.info('Colors:')
unique_colors, counts = np.unique(self.img_parsed_ids, return_counts=True)
for i, id in enumerate(unique_colors):
logging.info(' ' + self.cfg.dominos[id][0] + ': ' + str(counts[i]))
def show_image_parsing(self):
# Plot images
fig, axes = plt.subplots(nrows=1, ncols=3)
ax = axes.ravel()
ax[0].imshow(self.img)
ax[0].set_title('Original')
ax[1].imshow(self.img_scaled)
ax[1].set_title('Scaled')
ax[2].imshow(self.img_parsed_color)
ax[2].set_title('Dominos')
plt.tight_layout()
figManager = plt.get_current_fig_manager()
figManager.window.state('zoomed')
plt.show()
def render_domino_image_tiles(self):
# Allocate memory for image
tile_size_x_in_px = (self.cfg.domino_width_px + self.cfg.domino_spacing_width_px) * self.cfg.tile_width
tile_size_y_in_px = (self.cfg.domino_height_px + self.cfg.domino_spacing_height_px) * self.cfg.tile_height
array_size_x = tile_size_x_in_px * self.n_tiles_x
array_size_y = tile_size_y_in_px * self.n_tiles_y
image_array = np.zeros((array_size_x, array_size_y, 3))
# Generate image
for tile in self.tiles:
tile.draw(image_array)
# Modify array to show image correctly
image_array = np.transpose(image_array, (1, 0, 2))
image_array = np.flip(image_array, 0)
# Actually show image
plt.imshow(image_array)
figManager = plt.get_current_fig_manager()
figManager.window.state('zoomed')
plt.show()
def show_tile_ordering(self):
# Build array of order to show and write
order_array = np.zeros((self.n_tiles_x,self.n_tiles_y))
# print(self.n_tiles_x)
# print(self.n_tiles_y)
for tile in self.tiles:
# print(tile.coordinate)
order_array[tile.coordinate] = tile.order
# Modify array to show image correctly
order_array = np.transpose(order_array, (1, 0))
# Show figure with colors
fig = plt.figure()
ax = fig.add_subplot(111)
im = ax.imshow(order_array, origin='lower', cmap='cool', interpolation='None')
for x in range(self.n_tiles_x):
for y in range(self.n_tiles_y):
label = int(order_array[y, x])
ax.text(x, y, label, color='black', ha='center', va='center')
fig.colorbar(im)
plt.show()
def _generateField(self):
# Load original image
img = mpimg.imread(self.cfg.image_name)
# Skip A value for RGBA files
if img.shape[2] is 4:
img = img[:,:,:3]
# Do some modifications to MR logo to get it the right shape
if self.cfg.MR_LOGO_PLAN:
# Crop vertical
vert_size = img.shape[0]
crop_factor = 0.4
lb = int(vert_size/2 - crop_factor*vert_size)
ub = int(vert_size/2 + crop_factor*vert_size)
img = img[lb:ub,:,:]
# Padd horizontal
hz_size = img.shape[1]
pad_amount = int(0.1 * hz_size)
img = np.pad(img, pad_width=[(0, 0), (pad_amount, pad_amount),(0, 0)], mode='constant')
# Scaled image
img_scaled = sktf.resize(img, (self.cfg.desired_height_dominos, self.cfg.desired_width_dominos), anti_aliasing=False)
# Parse image into domino IDs by choosing 'closest' color
img_parsed_color = np.zeros((self.cfg.desired_width_dominos * self.cfg.desired_height_dominos, 3))
img_parsed_ids = np.zeros_like(img_parsed_color, dtype=np.int_)[:, 0]
count = 0
for row in img_scaled:
for px in row:
best_id = -1
best_score = 9999999
for id, val in enumerate(self.cfg.dominos):
color = val[1]
score = np.linalg.norm(px - color)
if score < best_score:
best_score = score
best_id = id
img_parsed_ids[count] = best_id
img_parsed_color[count] = self.cfg.dominos[best_id][1]
count = count + 1
img_parsed_ids = img_parsed_ids.reshape((self.cfg.desired_height_dominos, self.cfg.desired_width_dominos))
img_parsed_color = img_parsed_color.reshape((self.cfg.desired_height_dominos, self.cfg.desired_width_dominos, 3))
self.img = img
self.img_scaled = img_scaled
self.img_parsed_ids = img_parsed_ids
self.img_parsed_color = img_parsed_color
def _addTile(self, tile_coordinate, tile_values, tile_order, vision_offset_map):
vision_offset = vision_offset_map[tile_coordinate]
# print("Tile: order {}, coord {}, vision offset: {}".format(tile_order, tile_coordinate, vision_offset))
new_tile = Tile(self.cfg, tile_coordinate, tile_values, tile_order, vision_offset)
self.tiles.append(new_tile)
def _generateTiles(self):
# Check sizes and make sure things line up
if self.cfg.desired_height_dominos % self.cfg.tile_height != 0:
raise ValueError('Field height is not evenly divisible by tile height!')
if self.cfg.desired_width_dominos % self.cfg.tile_width != 0:
raise ValueError('Field width is not evenly divisible by tile width!')
# Determine number of tiles needed in x and y
self.n_tiles_x = int(self.cfg.desired_width_dominos / self.cfg.tile_width)
self.n_tiles_y = int(self.cfg.desired_height_dominos / self.cfg.tile_height)
print("Generating tiles {} x {}".format(self.n_tiles_x, self.n_tiles_y))
order_map = self._generateTileOrdering(self.n_tiles_x, self.n_tiles_y)
vision_offset_map = generateVisionOffsetMap(self.cfg, self.n_tiles_x, self.n_tiles_y)
# Loop over tiles and assign id and colors to each
for i in range(self.n_tiles_x):
x_start_idx = i * self.cfg.tile_width
x_end_idx = (i + 1) * self.cfg.tile_width
for j in range(self.n_tiles_y):
# Need to account for flipped y axis with array
y_start_idx = - j * self.cfg.tile_height - 1
y_end_idx = - (j + 1) * self.cfg.tile_height - 1
tile_values = np.copy(self.img_parsed_ids[y_start_idx:y_end_idx:-1, x_start_idx:x_end_idx])
tile_coord = (i,j)
tile_order = order_map[tile_coord]
self._addTile(tile_coord, tile_values, tile_order, vision_offset_map)
# Sort tile array so they are in order
self.tiles = sorted(self.tiles, key = lambda tile: tile.order)
@classmethod
def _generateTileOrdering(cls, num_x, num_y):
"""
Generates and ordering that maps x y tile coordinate -> order number
"""
# return cls._generateTileOrderingDiagonal(num_x, num_y)
return cls._generateTileOrderingColumns(num_x, num_y)
@classmethod
def _generateTileOrderingColumns(cls, num_x, num_y):
def coordToOrderLR(coord, num_x, num_y):
return coord[0]*num_y + num_y - coord[1] -1
def coordToOrderRL(coord, num_x, num_y):
return (num_x - coord[0] - 1)*num_y + num_y - coord[1] -1
all_coords = [(x,y) for x in range(num_x) for y in range(num_y)]
order_map = {coord: coordToOrderRL(coord, num_x, num_y) for coord in all_coords}
return order_map
@classmethod
def _generateTileOrderingDiagonal(cls, num_x, num_y):
coord = [num_x - 1, num_y - 1] # Starting at top right
order = 0
order_map = {tuple(coord): order}
done = False
next_pass = np.copy(coord)
next_pass[1] = coord[1] - 1 # Next pass starts at far right, next row down
order = order + 1
while not done:
# If you are at the top or left, go to next pass
if coord[1] + 1 >= num_y or coord[0] - 1 < 0:
coord = np.copy(next_pass)
# Compute new value for next pass
if next_pass[1] > 0:
next_pass[1] = next_pass[1] - 1 # Next pass starts at far right, next row down
elif next_pass[1] == 0:
next_pass[0] = next_pass[0] - 1 # If we reach the bottom row, shift to the left instead
else:
raise ValueError("Whoops! Shouldn't ever get here!")
else:
coord[0] = coord[0] - 1 # Move up and left
coord[1] = coord[1] + 1
# Save the order in the map for this coordinate
order_map[tuple(coord)] = order
order = order + 1
# If we got to 0,0 then we are done
if coord[0] == 0 and coord[1] == 0:
done = True
return order_map
class Tile:
"""
Holds info and useful methods related to an individual domino tile
"""
def __init__(self, cfg, coordinate, values, order, vision_offset):
self.coordinate = coordinate
self.vision_offset = vision_offset
self.values = values
self.cfg = cfg
self.order = order
def getPlacementPositionInMeters(self):
# Returns bottom left corner position of the tile relative to the origin of the field
tile_start_x = self.coordinate[0] * self.cfg.tile_size_width_meters
tile_start_y = self.coordinate[1] * self.cfg.tile_size_height_meters
return (tile_start_x, tile_start_y)
def draw(self, array):
# Note that this function draws pixels assuming the array is indexed as x,y instead of rows and columns
# the array is flipped to plot as an image in the parent function
# Determine tile location
tile_size_x_in_px = (self.cfg.domino_width_px + self.cfg.domino_spacing_width_px) * self.cfg.tile_width
tile_size_y_in_px = (self.cfg.domino_height_px + self.cfg.domino_spacing_height_px) * self.cfg.tile_height
tile_start_x_px = self.coordinate[0] * tile_size_x_in_px
tile_start_y_px = self.coordinate[1] * tile_size_y_in_px
tile_end_x_px = tile_start_x_px + tile_size_x_in_px
tile_end_y_px = tile_start_y_px + tile_size_y_in_px
self.draw_single(array, tile_start_x_px, tile_start_y_px, tile_end_x_px, tile_end_y_px)
def draw_single(self, array, tile_start_x_px, tile_start_y_px, tile_end_x_px, tile_end_y_px, draw_edge=True):
# Fill in tile with background color
array[tile_start_x_px:tile_end_x_px, tile_start_y_px:tile_end_y_px, 0] = self.cfg.tile_background_color[0]
array[tile_start_x_px:tile_end_x_px, tile_start_y_px:tile_end_y_px, 1] = self.cfg.tile_background_color[1]
array[tile_start_x_px:tile_end_x_px, tile_start_y_px:tile_end_y_px, 2] = self.cfg.tile_background_color[2]
# Fill in tile edge color (only have to do start locations since next tile over will fill in end locations)
if draw_edge:
array[tile_start_x_px:tile_end_x_px, tile_start_y_px, 0] = self.cfg.tile_edge_color[0]
array[tile_start_x_px:tile_end_x_px, tile_start_y_px, 1] = self.cfg.tile_edge_color[1]
array[tile_start_x_px:tile_end_x_px, tile_start_y_px, 2] = self.cfg.tile_edge_color[2]
array[tile_start_x_px, tile_start_y_px:tile_end_y_px, 0] = self.cfg.tile_edge_color[0]
array[tile_start_x_px, tile_start_y_px:tile_end_y_px, 1] = self.cfg.tile_edge_color[1]
array[tile_start_x_px, tile_start_y_px:tile_end_y_px, 2] = self.cfg.tile_edge_color[2]
# Draw dominos
for i in range(self.cfg.tile_width):
domino_start_x = tile_start_x_px + self.cfg.domino_spacing_width_px + (self.cfg.domino_width_px + self.cfg.domino_spacing_width_px) * i
domino_end_x = domino_start_x + self.cfg.domino_width_px
for j in range(self.cfg.tile_height):
domino_start_y = tile_start_y_px + self.cfg.domino_spacing_height_px + (self.cfg.domino_height_px + self.cfg.domino_spacing_height_px) * j
domino_end_y = domino_start_y + self.cfg.domino_height_px
domino_id = self.values[j, i]
domino_color = self.cfg.dominos[domino_id][1]
array[domino_start_x:domino_end_x, domino_start_y:domino_end_y, 0] = domino_color[0]
array[domino_start_x:domino_end_x, domino_start_y:domino_end_y, 1] = domino_color[1]
array[domino_start_x:domino_end_x, domino_start_y:domino_end_y, 2] = domino_color[2]
class Action:
def __init__(self, action_type, name):
self.action_type = action_type
self.name = name
def draw(self, ax, text=""):
pass
class SetPoseAction(Action):
def __init__(self, action_type, name, x, y, a):
# action_type (enum)
# string name
# X position [m]
# Y position [m]
# Angle [deg]
super().__init__(action_type, name)
self.x = float(x)
self.y = float(y)
self.a = math.radians(float(a))
class WaitAction(Action):
def __init__(self,action_type, name, time):
super().__init__(action_type, name)
self.time = time
class MoveConstVelAction(Action):
def __init__(self, action_type, name, vx, vy, va, t):
# action_type (enum)
# string name
# X velocity [m/s]
# Y velocity [m/s]
# Angle [rad/s]
# time [sec]
super().__init__(action_type, name)
self.vx = float(vx)
self.vy = float(vy)
self.va = float(va)
self.t = float(t)
class MoveAction(Action):
def __init__(self, action_type, name, x, y, a):
# action_type (enum)
# string name
# X position [m]
# Y position [m]
# Angle [deg]
super().__init__(action_type, name)
self.x = float(x)
self.y = float(y)
self.a = math.radians(float(a))
def getPos(self):
return np.array([self.x, self.y])
def getAngleDegrees(self):
return math.degrees(self.a)
def getAngleRadians(self):
return self.a
def draw(self, ax, text="", show_label=True):
if self.action_type in [ActionTypes.MOVE_WITH_VISION, ActionTypes.MOVE_REL, ActionTypes.MOVE_REL_SLOW]:
return
# Base triangle at 0 degrees
scale = 0.3
p1 = np.array([scale, 0])
s = math.sin(45*math.pi/180.0)
c = math.cos(45 * math.pi / 180.0)
p2 = np.array([-scale*s, scale*c])
p3 = np.array([-scale*s, -scale*c])
points = np.vstack((p1, p2 ,p3))
# Rotate for orientation
s = math.sin(self.getAngleRadians())
c = math.cos(self.getAngleRadians())
R = np.array([[c, -s],[s, c]])
for i in range(3):
# Do local rotation
points[i,:] = np.matmul(R, np.reshape(points[i,:],(2,1))).ravel()
# Then offset for position
points[i, :] = points[i, :] + self.getPos()
ax.add_patch(patches.Polygon(points,
fill=True,
edgecolor='c',
facecolor='c'))
text_point = points[0]
text_to_show = self.name
if text is not "":
text_to_show = text
if show_label:
ax.annotate(text_to_show, xy=text_point[:2], xytext=[1, 1], textcoords="offset points", fontsize=8, color="green")
def generate_full_action_sequence(cfg, tile):
"""
Standard sequence:
- Move to load
- Do load
- Move out of load
- Move to field entry
- Move to coarse drop off
- Wait for localization
- Move to fine drop off
- Drop off
- Move to coarse drop off
- Move to field exit
- Move to near load
"""
# Setup positions in field frame
tile_pos_in_field_frame = np.array(tile.getPlacementPositionInMeters())
robot_placement_pos_field_frame = tile_pos_in_field_frame + Utils.TransformPos(cfg.tile_to_robot_offset, [0,0], cfg.field_to_robot_frame_angle)
robot_placement_fine_pos_field_frame = robot_placement_pos_field_frame + Utils.TransformPos(cfg.tile_placement_fine_offset, [0,0], cfg.field_to_robot_frame_angle)
robot_placement_coarse_pos_field_frame = robot_placement_fine_pos_field_frame + Utils.TransformPos(cfg.tile_placement_coarse_offset, [0,0], cfg.field_to_robot_frame_angle)
# Convert positions to global frame
robot_placement_coarse_pos_global_frame = Utils.TransformPos(robot_placement_coarse_pos_field_frame, cfg.domino_field_origin, cfg.domino_field_angle)
robot_placement_fine_pos_global_frame = Utils.TransformPos(robot_placement_fine_pos_field_frame, cfg.domino_field_origin, cfg.domino_field_angle)
robot_field_angle = cfg.domino_field_angle + cfg.field_to_robot_frame_angle
# Setup various entry and intermediate positions
intermediate_entry_pos_global_frame = np.array([cfg.highway_x, cfg.intermediate_entry_hz_y])
entry_y = robot_placement_coarse_pos_global_frame[1]+cfg.enter_position_distance
field_entry_pos_global_frame = np.array([cfg.highway_x+0.75, entry_y])
intermediate_place_pos_global_frame = np.array([robot_placement_coarse_pos_global_frame[0], entry_y])
# Make pose adjustments based on config
coord = tile.coordinate
x_offset_row = cfg.x_offset_rows[coord[1]]
y_offset_row = cfg.y_offset_rows[coord[1]]
y_offset_col = cfg.y_offset_cols[coord[0]]
extra_y_offset_fine = y_offset_col + y_offset_row
robot_placement_fine_pos_global_frame[0] += x_offset_row
robot_placement_fine_pos_global_frame[1] += extra_y_offset_fine
robot_placement_coarse_pos_global_frame[1] += extra_y_offset_fine
intermediate_place_pos_global_frame[1] += extra_y_offset_fine
field_entry_pos_global_frame[1] += extra_y_offset_fine
# Figure out if intermediate steps are needed
intermediate_hz = robot_placement_coarse_pos_global_frame[1] < cfg.intermediate_entry_hz_y - 1
intermediate_vt = robot_placement_coarse_pos_global_frame[0] > cfg.intermediate_place_vt_x + 1
# Tiles near the back wall don't have enough space for backwards offset
relative_tile_offset = copy.deepcopy(cfg.tile_placement_coarse_offset)
if robot_placement_coarse_pos_global_frame[0] < 2:
intermediate_place_pos_global_frame[0] = robot_placement_fine_pos_global_frame[0]
robot_placement_coarse_pos_global_frame[0] = robot_placement_fine_pos_global_frame[0]
relative_tile_offset = np.asarray([0, 1])
actions = []
name = "Move to load waypoint - coarse"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, cfg.load_waypoint[0], cfg.load_waypoint[1], cfg.highway_angle))
# name = "Wait for localization"
# actions.append(Action(ActionTypes.WAIT_FOR_LOCALIZATION, name))
name = "Move to near load area - coarse"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, cfg.base_station_prep_pos[0], cfg.base_station_prep_pos[1], cfg.base_station_target_angle))
# name = "Pause plan for load"
# actions.append(Action(ActionTypes.PAUSE_PLAN, name))
# name = "Start cameras"
# actions.append(Action(ActionTypes.START_CAMERAS, name))
# name = "Wait for localization"
# actions.append(Action(ActionTypes.WAIT_FOR_LOCALIZATION, name))
# name = "Move to load prep - fine"
# actions.append(MoveAction(ActionTypes.MOVE_FINE, name, cfg.base_station_prep_pos[0], cfg.base_station_prep_pos[1], cfg.base_station_target_angle))
# name = "Move to load prep - vision"
# actions.append(MoveAction(ActionTypes.MOVE_WITH_VISION, name, cfg.base_station_prep_vision_offset[0], cfg.base_station_prep_vision_offset[1], cfg.base_station_prep_vision_offset[2]))
# name = "Move to load - relative slow"
# actions.append(MoveAction(ActionTypes.MOVE_REL_SLOW, name, cfg.base_station_relative_offset[0], cfg.base_station_relative_offset[1], cfg.base_station_relative_offset[2]))
# name = "Align with load"
# actions.append(MoveAction(ActionTypes.MOVE_WITH_VISION, name, cfg.base_station_vision_offset[0], cfg.base_station_vision_offset[1], cfg.base_station_vision_offset[2]))
# name = "Stop cameras"
# actions.append(Action(ActionTypes.STOP_CAMERAS, name))
name = "Load tile"
actions.append(Action(ActionTypes.LOAD, name))
# name = "Move away from load - relative slow"
# actions.append(MoveAction(ActionTypes.MOVE_REL_SLOW, name, -1.3*cfg.base_station_relative_offset[0], -1.3*cfg.base_station_relative_offset[1], -1.3*cfg.base_station_relative_offset[2]))
# name = "Pause plan for QC"
# actions.append(Action(ActionTypes.PAUSE_PLAN, name))
name = "Move to load waypoint - coarse"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, cfg.load_waypoint[0], cfg.load_waypoint[1], cfg.highway_angle))
# name = "Wait for localization"
# actions.append(Action(ActionTypes.WAIT_FOR_LOCALIZATION, name))
# if intermediate_hz:
# name = "Move to intermediate enter - coarse"
# actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, intermediate_entry_pos_global_frame[0], intermediate_entry_pos_global_frame[1], cfg.highway_angle))
# name = "Wait for motor cooldown (long)"
# actions.append(WaitAction(ActionTypes.WAIT, name, 20))
name = "Move to enter - coarse"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, field_entry_pos_global_frame[0], field_entry_pos_global_frame[1], robot_field_angle))
name = "Wait for localization"
actions.append(Action(ActionTypes.WAIT_FOR_LOCALIZATION, name))
# name = "Wait for motor cooldown"
# actions.append(WaitAction(ActionTypes.WAIT, name, 10))
# if intermediate_vt:
# name = "Move to intermediate place - coarse"
# actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, intermediate_place_pos_global_frame[0], intermediate_place_pos_global_frame[1], robot_field_angle))
# name = "Wait for localization"
# actions.append(Action(ActionTypes.WAIT_FOR_LOCALIZATION, name))
# name = "Wait for motor cooldown"
# actions.append(WaitAction(ActionTypes.WAIT, name, 10))
name = "Move to near place - coarse"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, robot_placement_coarse_pos_global_frame[0], robot_placement_coarse_pos_global_frame[1], robot_field_angle))
name = "Start cameras"
actions.append(Action(ActionTypes.START_CAMERAS, name))
name = "Wait for localization"
actions.append(Action(ActionTypes.WAIT_FOR_LOCALIZATION, name))
name = "Move to place - fine"
actions.append(MoveAction(ActionTypes.MOVE_FINE_STOP_VISION, name, robot_placement_fine_pos_global_frame[0], robot_placement_fine_pos_global_frame[1], robot_field_angle + cfg.angle_adjust_fine))
name = "Move to place - vision"
actions.append(MoveAction(ActionTypes.MOVE_WITH_VISION, name, tile.vision_offset[0], tile.vision_offset[1], tile.vision_offset[2]))
name = "Stop cameras"
actions.append(Action(ActionTypes.STOP_CAMERAS, name))
# name = "Pause plan for QC"
# actions.append(Action(ActionTypes.PAUSE_PLAN, name))
name = "Place tile"
actions.append(Action(ActionTypes.PLACE, name))
name = "Move away from place - relative slow"
actions.append(MoveAction(ActionTypes.MOVE_REL_SLOW, name, relative_tile_offset[0], relative_tile_offset[1], 0))
# if intermediate_vt:
# name = "Move to intermediate place - coarse"
# actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, intermediate_place_pos_global_frame[0], intermediate_place_pos_global_frame[1], robot_field_angle))
# name = "Wait for motor cooldown"
# actions.append(WaitAction(ActionTypes.WAIT, name, 10))
name = "Move to intermediate place - coarse"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, intermediate_place_pos_global_frame[0], intermediate_place_pos_global_frame[1], robot_field_angle))
name = "Move to exit - coarse"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, field_entry_pos_global_frame[0], field_entry_pos_global_frame[1], cfg.highway_angle))
name = "Wait for localization"
actions.append(Action(ActionTypes.WAIT_FOR_LOCALIZATION, name))
# name = "Wait for motor cooldown"
# actions.append(WaitAction(ActionTypes.WAIT, name, 10))
# if intermediate_hz:
# name = "Move to intermediate exit - coarse"
# actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, intermediate_entry_pos_global_frame[0], intermediate_entry_pos_global_frame[1], cfg.highway_angle))
# name = "Wait for motor cooldown (long)"
# actions.append(WaitAction(ActionTypes.WAIT, name, 20))
return actions
def generate_hax_action_sequence(cfg, tile):
actions = []
x_pose = 7
name = "Move 1 - coarse 90"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, x_pose, 5, 90))
name = "Move 1 - coarse 0"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, x_pose, 5, 0))
name = "Move 1 - coarse -90"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, x_pose, 5, -90))
name = "Wait 1"
actions.append(WaitAction(ActionTypes.WAIT, name, 5))
name = "Move 2 - coarse -90"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, x_pose, -3.5, -90))
name = "Move 2 - coarse 0 "
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, x_pose, -3.5, 0))
name = "Move 2 - coarse 90"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, x_pose, -3.5, 90))
name = "Wait 2"
actions.append(WaitAction(ActionTypes.WAIT, name, 5))
return actions
def generate_small_testing_action_sequence(cfg, tile):
"""
Short sequence for testing
Load
Move to near place - coarse
Wait for localization
Move to place - fine
Place
Move away from place - fine
Move to load - coarse
"""
# Setup positions in field frame
tile_pos_in_field_frame = np.array(tile.getPlacementPositionInMeters())
robot_placement_fine_pos_field_frame = tile_pos_in_field_frame + Utils.TransformPos(cfg.tile_to_robot_offset, [0,0], cfg.field_to_robot_frame_angle)
robot_placement_coarse_pos_field_frame = robot_placement_fine_pos_field_frame + Utils.TransformPos(cfg.tile_placement_coarse_offset, [0,0], cfg.field_to_robot_frame_angle)
enter_field_prep_pos_field_frame = [robot_placement_coarse_pos_field_frame[0], - cfg.prep_position_distance]
exit_field_prep_pos_field_frame = [-cfg.exit_position_distance, robot_placement_coarse_pos_field_frame[1]]
# Convert positions to global frame
tile_pos_in_global_frame = Utils.TransformPos(tile_pos_in_field_frame, cfg.domino_field_origin, cfg.domino_field_angle)
robot_placement_coarse_pos_global_frame = Utils.TransformPos(robot_placement_coarse_pos_field_frame, cfg.domino_field_origin, cfg.domino_field_angle)
robot_placement_fine_pos_global_frame = Utils.TransformPos(robot_placement_fine_pos_field_frame, cfg.domino_field_origin, cfg.domino_field_angle)
robot_field_angle = cfg.domino_field_angle + cfg.field_to_robot_frame_angle
enter_field_prep_global_frame = Utils.TransformPos(enter_field_prep_pos_field_frame, cfg.domino_field_origin, cfg.domino_field_angle)
exit_field_prep_global_frame = Utils.TransformPos(exit_field_prep_pos_field_frame, cfg.domino_field_origin, cfg.domino_field_angle)
# print(tile.order)
# print(tile.coordinate)
# print(tile_pos_in_field_frame)
# print(tile_pos_in_global_frame)
# print(robot_placement_coarse_pos_global_frame)
# print(robot_placement_fine_pos_global_frame)
actions = []
name = "Load tile"
actions.append(Action(ActionTypes.LOAD, name))
name = "Move to prep - coarse"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, enter_field_prep_global_frame[0], enter_field_prep_global_frame[1], robot_field_angle))
name = "Wait for localization"
actions.append(Action(ActionTypes.WAIT_FOR_LOCALIZATION, name))
name = "Move to near place - coarse"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, robot_placement_coarse_pos_global_frame[0], robot_placement_coarse_pos_global_frame[1], robot_field_angle))
name = "Wait for localization"
actions.append(Action(ActionTypes.WAIT_FOR_LOCALIZATION, name))
name = "Start cameras"
actions.append(Action(ActionTypes.START_CAMERAS, name))
name = "Move to place - fine"
actions.append(MoveAction(ActionTypes.MOVE_FINE, name, robot_placement_fine_pos_global_frame[0], robot_placement_fine_pos_global_frame[1], robot_field_angle))
name = "Move to place - vision"
actions.append(MoveAction(ActionTypes.MOVE_WITH_VISION, name, tile.vision_offset[0], tile.vision_offset[1], tile.vision_offset[2]))
name = "Stop cameras"
actions.append(Action(ActionTypes.STOP_CAMERAS, name))
name = "Place tile"
actions.append(Action(ActionTypes.PLACE, name))
name = "Move away from place - fine"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, robot_placement_coarse_pos_global_frame[0], robot_placement_coarse_pos_global_frame[1], robot_field_angle))
name = "Move to exit - coarse"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, enter_field_prep_global_frame[0], enter_field_prep_global_frame[1], robot_field_angle))
name = "Move to near load - coarse"
actions.append(MoveAction(ActionTypes.MOVE_COARSE, name, cfg.load_pose[0], cfg.load_pose[1], cfg.load_pose[2]))
return actions
def draw_env(cfg):
"""
Draws a figure of the environment
"""
fig,ax = plt.subplots(1)
# Draw overall map boundaries
ax.add_patch(patches.Rectangle(cfg.robot_boundaries[0],
cfg.robot_boundaries[1][0] - cfg.robot_boundaries[0][0],
cfg.robot_boundaries[1][1] - cfg.robot_boundaries[0][1],
fill=False,
edgecolor='b'))
# Draw field boundaries
ax.add_patch(patches.Rectangle(cfg.domino_field_origin,
cfg.domino_field_top_right[0] - cfg.domino_field_origin[0],
cfg.domino_field_top_right[1] - cfg.domino_field_origin[1],
fill=False,
edgecolor='r'))
# Draw base station
ax.add_patch(patches.Rectangle(cfg.base_station_boundaries[0],
cfg.base_station_boundaries[1][0] - cfg.base_station_boundaries[0][0],
cfg.base_station_boundaries[1][1] - cfg.base_station_boundaries[0][1],
fill=True,
edgecolor='k',
facecolor='k'))
ax.set_xlim(left=-1, right=cfg.robot_boundaries[1][0]+1)
ax.set_ylim(bottom=-1, top=cfg.robot_boundaries[1][1]+1)
ax.axis('equal')
return ax
class Cycle:
def __init__(self, id, cfg, robot_id, tile, action_sequence):
self.id = id
self.cfg = cfg
self.robot_id = robot_id
self.tile = tile
self.action_sequence = action_sequence
def draw_cycle(self, ax):
for action in self.action_sequence:
action.draw(ax)
def draw_action(self, ax, idx, text=""):
self.action_sequence[idx].draw(ax, text)
def generate_standard_cycles(cfg, field, cycle_generator_fn):
start_num = 1
robot_num = start_num
n_robots = len(cfg.ip_map)
cycles = []
for tile in field.tiles:
action_sequence = cycle_generator_fn(cfg, tile)
cycles.append(Cycle(tile.order, cfg, "robot{}".format(robot_num), tile, action_sequence))
robot_num += 1
if robot_num > n_robots:
robot_num = start_num
return cycles
class BasePlan:
def __init__(self, cycles):
self.cycles = cycles
def get_cycle(self, cycle_num):
try:
return self.cycles[cycle_num]
except IndexError:
return None
def get_action(self, cycle_id, action_id):
cycle = self.get_cycle(cycle_id)
if cycle:
try:
return cycle.action_sequence[action_id]
except IndexError:
return None
else:
return None
class RealPlan(BasePlan):
def __init__(self, cfg, field, cycles):
self.cfg = cfg
self.field = field
super().__init__(cycles)
def draw_cycle(self, cycle_num):
ax = draw_env(self.cfg)
self.get_cycle(cycle_num).draw_cycle(ax)
plt.show()
def find_pose_move_idx(self, cycle):
# Figure out what id the tile pose is
place_idx = -1
for i,action in enumerate(cycle.action_sequence):
if action.action_type == ActionTypes.PLACE:
place_idx = i
break
if place_idx == -1:
raise ValueError("Couldn't find placement index")
tile_pose_move_idx = -1
for j in range(place_idx, 0, -1):
action = cycle.action_sequence[j]
if action.action_type == ActionTypes.MOVE_FINE or action.action_type == ActionTypes.MOVE_COARSE :
tile_pose_move_idx = j
break
if tile_pose_move_idx == -1:
raise ValueError("Couldn't find movement index")
return tile_pose_move_idx
def draw_all_tile_poses(self):
ax = draw_env(self.cfg)
for cycle in self.cycles:
tile_pose_move_idx = self.find_pose_move_idx(cycle)
cycle.draw_action(ax, tile_pose_move_idx, text=cycle.tile.order)
plt.show()
class Plan(RealPlan):
def __init__(self, cfg, cycle_generator_fn):
field = DominoField(cfg)
field.generate()
logging.info('Generating robot actions...')
cycles = generate_standard_cycles(cfg, field, cycle_generator_fn)
logging.info('done.')
super().__init__(cfg, field, cycles)
class SubsectionPlan(RealPlan):
def __init__(self, full_plan):
self.full_plan = full_plan
self.start_coords = full_plan.cfg.start_coords
self.end_coords = full_plan.cfg.end_coords
self.delta_coords = (self.end_coords[0] - self.start_coords[0],
self.end_coords[1] - self.start_coords[1] )
new_field = DominoField(full_plan.cfg)
new_field.n_tiles_x = self.delta_coords[0]
new_field.n_tiles_y = self.delta_coords[1]
counter = 0
new_field.tiles = []
new_cycles = []
for i in range(len(full_plan.field.tiles)):
tile = full_plan.field.tiles[i]
cycle = full_plan.cycles[i]
tile_coords = tile.coordinate
if tile_coords[0] >= self.start_coords[0] and \
tile_coords[0] <= self.end_coords[0] and \
tile_coords[1] >= self.start_coords[1] and \
tile_coords[1] <= self.end_coords[1]:
# Make new tile
new_tile = copy.deepcopy(tile)
new_tile.order = counter
new_field.tiles.append(new_tile)
# Make new cycle
new_cycle = copy.deepcopy(cycle)
new_cycle.id = counter
new_cycle.tile = new_tile
new_cycles.append(new_cycle)
counter += 1
super().__init__(self.full_plan.cfg, new_field, new_cycles)
class TestPlan(BasePlan):
"""
Test plan used for debugging and testing various action sequences
"""
def __init__(self):
actions = []
actions.append(MoveAction(ActionTypes.MOVE_COARSE, "TestMoveCoarse", 0.5, 0.5, 0))
actions.append(MoveAction(ActionTypes.MOVE_FINE, 'TestMoveFine', 1,1,0))
actions.append(MoveAction(ActionTypes.MOVE_COARSE, 'Blah', 0,0,3.14))
cycles = [Cycle(i,None,'robot1','TestCycle{}'.format(i), actions) for i in range(3)]
super().__init__(cycles)
def RunFieldPlanning(autosave=False):
cfg = config.Config()
logging.basicConfig(
level=logging.INFO,
format="%(asctime)s [%(levelname)s] %(message)s",
handlers=[
logging.FileHandler(os.path.join(cfg.log_folder,"planner.log")),
logging.StreamHandler()
]
)
plan = None
if cfg.USE_SMALL_TESTING_CONFIG:
plan = Plan(cfg, generate_small_testing_action_sequence)
else:
plan = Plan(cfg, generate_full_action_sequence)
# plan = Plan(cfg, generate_hax_action_sequence)
if cfg.USE_SUBSECTION:
plan = SubsectionPlan(plan)
if autosave:
fname = os.path.join(cfg.plans_dir,"autosaved.p")
with open(fname, 'wb') as f:
pickle.dump(plan, f)
logging.info("Saved plan to {}".format(fname))
return plan
def GeneratePDF(plan):
from reportlab.lib.pagesizes import letter
from reportlab.pdfgen import canvas
from PIL import Image
logging.info("Generating PDF")
# Initialize pdf
name = "domino_plan.pdf"
if plan.cfg.MR_LOGO_PLAN:
name = "domnino_plan_logo.pdf"
full_path = os.path.join(plan.cfg.plans_dir, name)
page_height, page_width = letter # Flipped to get landscape
c = canvas.Canvas(full_path, pagesize=(page_width, page_height))
# Pre allocate image array
tile_size_x_in_px = (plan.cfg.domino_width_px + plan.cfg.domino_spacing_width_px) * plan.cfg.tile_width
tile_size_y_in_px = (plan.cfg.domino_height_px + plan.cfg.domino_spacing_height_px) * plan.cfg.tile_height
for i in range(len(plan.field.tiles)):
# Get tile to draw on this page
tile = plan.field.tiles[i]
# Draw title
text_width = 0.5 * page_width
text_height = 0.9 * page_height
text = "Tile {}, Coordinate: ({}, {})".format(i, tile.coordinate[0], tile.coordinate[1])
c.setFont("Helvetica", 20)
c.drawCentredString(text_width,text_height,text)
# Draw orientation note
text_width = 0.5 * page_width
text_height = 0.1 * page_height
text = "This side towards robot body"
c.drawCentredString(text_width,text_height,text)
# Draw image
image_array = np.zeros((tile_size_x_in_px, tile_size_y_in_px, 3))
tile.draw_single(image_array, 0, 0, tile_size_x_in_px, tile_size_y_in_px, draw_edge=False)
image_array = np.transpose(image_array, (1, 0, 2))
image_array = np.flip(image_array, 0)
im = Image.fromarray(np.uint8(255*image_array), mode='RGB')
image_fraction = 0.7
start_width = (1-image_fraction)/2.0 * page_width
start_height = (1-image_fraction)/2.0 * page_height
image_width = image_fraction * page_width
image_height = image_fraction * page_height
c.drawInlineImage(im, start_width , start_height, width=image_width, height=image_height)
# Complete page
c.showPage()
c.save()
if __name__ == '__main__':
import PySimpleGUI as sg
plan = RunFieldPlanning(autosave=False)
# plan.field.printStats()
# plan.field.show_image_parsing()
# plan.field.render_domino_image_tiles()
# plan.field.show_tile_ordering()
plan.draw_cycle(0)
plan.draw_cycle(10)
plan.draw_cycle(19)
plan.draw_all_tile_poses()
# GeneratePDF(plan)
# sg.change_look_and_feel('Dark Blue 3')
# clicked_value = sg.popup_yes_no('Save plan to file?')
# if clicked_value == "Yes":
# fname = sg.popup_get_file("Location to save", save_as=True)
# with open(fname, 'wb') as f:
# pickle.dump(plan, f)
# logging.info("Saved plan to {}".format(fname))
================================================
FILE: src/master/MarvelMindHandler.py
================================================
"""
Python wrapper for Marvelmind C API
"""
import ctypes
import logging
from collections import defaultdict
from Utils import NonBlockingTimer
class MarvelmindWrapper:
def __init__(self, cfg):
self.lib = ctypes.windll.LoadLibrary(cfg.mm_api_path)
self.devices = defaultdict(dict)
self.expected_devices = []
self.wake_timer = None
for name,beacons in cfg.device_map.items():
for b in beacons:
self.expected_devices.append(b)
self._open_serial_port()
def __del__(self):
self._close_serial_port()
def _open_serial_port(self):
"""
Establish communication with marvelmind router. Required for all other functions.
"""
logging.info("Opening communication with MarvelMind")
fn = self.lib.mm_open_port
fn.restype = ctypes.c_bool
fn.argtypes = [ctypes.POINTER(ctypes.c_void_p)]
status = fn(None)
def _close_serial_port(self):
"""
Close communication with marvelmind router.
"""
fn = self.lib.mm_close_port
fn.restype = ctypes.c_bool
fn.argtypes = [ctypes.POINTER(ctypes.c_void_p)]
status = fn(None)
def check_all_devices_status(self):
"""
Checks to see if all devices are awake and ready
"""
logging.info("Getting device list")
fn = self.lib.mm_get_devices_list
fn.restype = ctypes.c_bool
fn.argtypes = [ctypes.POINTER(ctypes.c_uint8)]
buff_size = 255
dataptr = (ctypes.c_uint8*buff_size)()
dataptr = ctypes.cast(dataptr, ctypes.POINTER(ctypes.c_uint8))
status = fn(dataptr)
num_devices = dataptr[0]
logging.info("Num devices found: " + str(num_devices))
idx = 1
device_offset = 9
addr_offset = 0
sleep_offset = 2
for i in range(num_devices):
addr = dataptr[idx + addr_offset]
sleep = dataptr[idx + sleep_offset]
self.devices[addr] = {'sleep': bool(sleep)}
idx += device_offset
ready = True
if self.wake_timer:
if not self.wake_timer.check():
ready = False
else:
for d in self.expected_devices:
if d not in self.devices.keys():
ready = False
logging.warning("Could not find expected device {}".format(d))
continue
if self.devices[d]['sleep'] is False:
ready = False
logging.warning("Device {} is not awake".format(d))
continue
return ready
def wake_all_devices(self):
for d in self.expected_devices:
self.wake_device(d)
def wake_all_devices_only_if_needed(self):
ok = self.check_all_devices_status()
if not ok:
logging.info("Triggering marvelmind device wakeup. Will wait for a moment to let them warmup")
self.wake_timer = NonBlockingTimer(30)
self.wake_all_devices()
def sleep_all_devices(self):
for d in self.expected_devices:
self.sleep_device(d)
def wake_device(self, address):
"""
Wake up the device with the given address
"""
logging.info("Waking device: {}".format(address))
fn = self.lib.mm_wake_device
fn.restype = ctypes.c_bool
fn.argtypes = [ctypes.c_uint8]
status = fn(address)
if not status:
logging.info("Warning: unable to wake device {}".format(address))
def sleep_device(self, address):
"""
Sleep the device with the given address
"""
logging.info("Sleeping device: {}".format(address))
fn = self.lib.mm_send_to_sleep_device
fn.restype = ctypes.c_bool
fn.argtypes = [ctypes.c_uint8]
status = fn(address)
if not status:
logging.info("Warning: unable to sleep device {}".format(address))
def get_metrics(self):
return self.devices
class MockMarvelmindWrapper:
def __init__(self, cfg):
pass
def check_all_devices_status(self):
return True
def wake_all_devices_only_if_needed(self):
pass
def wake_all_devices(self):
pass
def sleep_all_devices(self):
pass
def wake_device(self, address):
pass
def sleep_device(self, address):
pass
def get_metrics(self):
return {}
================================================
FILE: src/master/MasterMain.py
================================================
import time
import math
import copy
import config
import os
import sys
import logging
import PySimpleGUI as sg
import traceback
from FieldPlanner import *
from Runtime import RuntimeManager, PlanStatus
import Utils
STATUS_PANEL_OK_COLOR = "green"
STATUS_PANEL_BAD_COLOR = "red"
def status_panel(name):
width = 40
height = 20
return [[sg.Text("{} status".format(name))], [sg.Text("{} offline".format(name), size=(width, height), \
relief=sg.RELIEF_RIDGE, key='_{}_STATUS_'.format(name.upper()), background_color=STATUS_PANEL_BAD_COLOR) ]]
def setup_gui_layout(config, panel_names, target_names):
plan_button_size = [9,2]
plan_button_pad = (2, 10)
# Left hand column with status panels
col1 = []
for name in panel_names:
col1 += status_panel(name)
# Plan cycle/action modification buttons
incremenet_cycle_button = sg.Button('Cycle +', button_color=('white','blue'), size=plan_button_size, pad=plan_button_pad, key='_INC_CYCLE_', disabled=True)
decremenet_cycle_button = sg.Button('Cycle -', button_color=('white','blue'), size=plan_button_size, pad=plan_button_pad, key='_DEC_CYCLE_', disabled=True)
incremenet_action_button = sg.Button('Action +', button_color=('white','blue'), size=plan_button_size, pad=plan_button_pad, key='_INC_ACTION_', disabled=True)
decremenet_action_button = sg.Button('Action -', button_color=('white','blue'), size=plan_button_size, pad=plan_button_pad, key='_DEC_ACTION_', disabled=True)
set_cycle_field = sg.Input(key='_CYCLE_FIELD_', size=(5,1), pad=(0,20))
set_action_field = sg.Input(key='_ACTION_FIELD_', size=(5,1), pad=(0,20))
set_cycle_button = sg.Button('Set Cycle', button_color=('white','blue'), size=plan_button_size, pad=plan_button_pad, key='_SET_CYCLE_', disabled=True)
set_action_button = sg.Button('Set Action', button_color=('white','blue'), size=plan_button_size, pad=plan_button_pad, key='_SET_ACTION_', disabled=True)
cycle_buttons = [[sg.Column([[decremenet_cycle_button], [decremenet_action_button]]), sg.Column([[incremenet_cycle_button], [incremenet_action_button]]),
sg.Column([[set_cycle_field], [set_action_field]]), sg.Column([[set_cycle_button], [set_action_button]]) ]]
col1 += cycle_buttons
# Middle column with plot and buttons
target_element = [ [sg.Text("Target: ")], [sg.Combo(target_names, key='_TARGET_', default_value='robot1')] ]
actions = [a for a in ActionTypes]
action_element = [ [sg.Text("Action: ")], [sg.Combo(actions, key='_ACTION_')] ]
data_element = [ [sg.Text('Data:')], [sg.Input(key='_ACTION_DATA_')] ]
plan_file_field = sg.Input(key='_PLAN_FILE_', visible=False, enable_events=True)
load_plan_button = sg.FileBrowse(button_text='Load Plan', button_color=('white','blue'), size=plan_button_size, pad=plan_button_pad, \
key='_LOAD_PLAN_', file_types=(('Robot Plans', ('*.json','*.p')),))
run_plan_button = sg.Button('Run Plan', button_color=('white','blue'), size=plan_button_size, pad=plan_button_pad, key='_RUN_PLAN_', disabled=True)
pause_plan_button = sg.Button('Pause Plan', button_color=('white','blue'), size=plan_button_size, pad=plan_button_pad, key='_PAUSE_PLAN_', disabled=True)
abort_plan_button = sg.Button('Abort Plan', button_color=('white','blue'), size=plan_button_size, pad=plan_button_pad, key='_ABORT_PLAN_', disabled=True)
plan_buttons = [[sg.Column([[plan_file_field, load_plan_button], [run_plan_button]]), sg.Column([[pause_plan_button], [abort_plan_button]])]]
button_size = [20,6]
button_pad = (2,10)
estop_button = [[sg.Button('ESTOP', button_color=('white','red'), size=button_size, pad=button_pad) ]]
manual_button = [[sg.Button('Send Command', button_color=('white','green'), size=button_size, pad=button_pad) ]]
clear_error_button = [[sg.Button('Clear Error', button_color=('white','orange red'), size=[10,6], pad=button_pad)]]
col2 = [[sg.Graph(canvas_size=(700,700), graph_bottom_left=config.graph_bottom_left, graph_top_right=config.graph_top_right, float_values=True, key="_GRAPH_", background_color="light grey") ],
[sg.Column(target_element), sg.Column(action_element), sg.Column(data_element)],
[sg.Column(plan_buttons), sg.Column(estop_button), sg.Column(manual_button), sg.Column(clear_error_button)] ]
# Right hand column with text ouput
col3 = [[sg.Output(size=(70, 50), echo_stdout_stderr=True)]]
return [[ sg.Column(col1), sg.Column(col2), sg.Column(col3)]]
class CmdGui:
def __init__(self, config):
self.config = config
sg.change_look_and_feel('DarkBlack')
panel_names = ["{}".format(n) for n in self.config.ip_map]
panel_names += ['plan']
target_names = copy.deepcopy(panel_names)
target_names.remove('plan')
layout = setup_gui_layout(config, panel_names, target_names)
self.window = sg.Window('Robot Controller', layout, return_keyboard_events=True, use_default_focus=False)
self.window.finalize()
self.viz_figs = {}
self.manual_action_debounce_timer = Utils.NonBlockingTimer(1.0)
self.drawn_plan_grid = False
self.last_cycle_number_drawn = -1
self._draw_environment()
def close(self):
self.window.close()
def update(self):
event, values = self.window.read(timeout=20)
# if event != "__TIMEOUT__":
# print(event)
# print(values)
# At exit, check if we should keep marvelmind on
if event is None or event == 'Exit':
if self.config.OFFLINE_TESTING or self.config.SKIP_MARVELMIND:
return 'Exit', None
else:
clicked_value = sg.popup_yes_no('Do you want to keep the Marvelmind running')
if clicked_value == "Yes":
return "ExitMM", None
else:
return 'Exit', None
# Sending a manual action (via button or pressing enter)
if event in ('Send Command', "Clear Error", '\r', '\n'):
manual_action = None
if self.manual_action_debounce_timer.check():
if event == "Clear Error":
manual_action = (values['_TARGET_'], Action(ActionTypes.CLEAR_ERROR, "ClearError"))
else:
manual_action = self._parse_manual_action(values)
if manual_action is not None:
self.window['_ACTION_DATA_'].update("")
self.manual_action_debounce_timer = Utils.NonBlockingTimer(1.0)
return 'Action', manual_action
# Pressing the run plan button
if event == "_RUN_PLAN_":
clicked_value = sg.popup_yes_no('Ready to start plan?')
if clicked_value == "Yes":
return "Run", None
if event == "_PLAN_FILE_":
return "Load", values["_PLAN_FILE_"]
if event == "_PAUSE_PLAN_":
return "Pause", None
if event == "_ABORT_PLAN_":
clicked_value = sg.popup_yes_no('Abort plan?')
if clicked_value == "Yes":
return "Abort", None
if event in ["_INC_CYCLE_","_DEC_CYCLE_","_INC_ACTION_","_DEC_ACTION_"]:
return event, None
if event in ["_SET_CYCLE_", "_SET_ACTION_"]:
return event, (values['_CYCLE_FIELD_'], values["_ACTION_FIELD_"])
if event == "ESTOP" or event == " ":
return "ESTOP", None
return None, None
def update_status_panels(self, metrics):
for key, metric in metrics.items():
if key == 'mm' or key == 'base':
continue
elif key == 'plan':
self._update_plan_panel(metric)
else:
self._update_robot_panel(key, metric)
def _parse_manual_action(self, values):
target = values['_TARGET_']
action_type = ActionTypes(values['_ACTION_'])
data_str = values['_ACTION_DATA_']
name = 'ManualAction'
action = None
if action_type in [ActionTypes.MOVE_COARSE, ActionTypes.MOVE_REL, ActionTypes.MOVE_REL_SLOW,
ActionTypes.MOVE_FINE, ActionTypes.MOVE_FINE_STOP_VISION]:
data = data_str.split(',')
data = [x.strip() for x in data]
if len(data) != 3:
logging.warning("Invalid data: {}".format(data))
return None
action = MoveAction(action_type, name, data[0], data[1], data[2])
elif action_type in [ActionTypes.MOVE_CONST_VEL]:
data = data_str.split(',')
data = [x.strip() for x in data]
if len(data) != 4:
logging.warning("Invalid data: {}".format(data))
return None
action = MoveConstVelAction(action_type, name, data[0], data[1], data[2], data[3])
elif action_type in [ActionTypes.SET_POSE]:
data = data_str.split(',')
data = [x.strip() for x in data]
if len(data) != 3:
logging.warning("Invalid data: {}".format(data))
return None
action = SetPoseAction(action_type, name, data[0], data[1], data[2])
elif action_type == ActionTypes.MOVE_WITH_VISION:
data = data_str.split(',')
data = [x.strip() for x in data]
if len(data) != 3:
data = (0,0,0)
logging.warning("Assuming position of (0,0,0) for vision move")
action = MoveAction(action_type, name, data[0], data[1], data[2])
elif action_type == ActionTypes.WAIT:
data = data_str.split(',')
data = [x.strip() for x in data]
action = WaitAction(action_type, name, float(data[0]))
else:
action = Action(action_type, name)
return (target, action)
def _udpate_cycle_button_status(self, disabled):
self.window['_INC_CYCLE_'].update(disabled=disabled)
self.window['_DEC_CYCLE_'].update(disabled=disabled)
self.window['_INC_ACTION_'].update(disabled=disabled)
self.window['_DEC_ACTION_'].update(disabled=disabled)
self.window['_CYCLE_FIELD_'].update(disabled=disabled)
self.window['_ACTION_FIELD_'].update(disabled=disabled)
self.window['_SET_CYCLE_'].update(disabled=disabled)
self.window['_SET_ACTION_'].update(disabled=disabled)
def _update_plan_button_status(self, plan_status):
if plan_status == PlanStatus.NONE:
self.window['_RUN_PLAN_'].update(text='Run', disabled=True)
self.window['_LOAD_PLAN_'].update(disabled=False)
self.window['_PAUSE_PLAN_'].update(disabled=True)
self.window['_ABORT_PLAN_'].update(disabled=True)
self._udpate_cycle_button_status(disabled=True)
elif plan_status == PlanStatus.LOADED or plan_status == PlanStatus.DONE:
self.window['_RUN_PLAN_'].update(text='Run', disabled=False)
self.window['_LOAD_PLAN_'].update(disabled=False)
self.window['_PAUSE_PLAN_'].update(disabled=True)
self.window['_ABORT_PLAN_'].update(disabled=True)
self._udpate_cycle_button_status(disabled=False)
elif plan_status == PlanStatus.RUNNING:
self.window['_RUN_PLAN_'].update(text='Run', disabled=True)
self.window['_LOAD_PLAN_'].update(disabled=True)
self.window['_PAUSE_PLAN_'].update(disabled=False)
self.window['_ABORT_PLAN_'].update(disabled=False)
self._udpate_cycle_button_status(disabled=True)
elif plan_status == PlanStatus.PAUSED:
self.window['_RUN_PLAN_'].update(text='Resume', disabled=False)
self.window['_LOAD_PLAN_'].update(disabled=True)
self.window['_PAUSE_PLAN_'].update(disabled=True)
self.window['_ABORT_PLAN_'].update(disabled=False)
self._udpate_cycle_button_status(disabled=False)
elif plan_status == PlanStatus.ABORTED:
self.window['_RUN_PLAN_'].update(text='Restart', disabled=False)
self.window['_LOAD_PLAN_'].update(disabled=False)
self.window['_PAUSE_PLAN_'].update(disabled=True)
self.window['_ABORT_PLAN_'].update(disabled=True)
self._udpate_cycle_button_status(disabled=False)
else:
logging.warning("Unhandled plan status for button state: {}".format(plan_status))
def _update_plan_panel(self, status_dict):
status_str = "Plan is not running"
color_str = STATUS_PANEL_BAD_COLOR
if status_dict:
try:
self._update_plan_button_status(status_dict['status'])
status_str = ""
plan_state_str = "{}".format(status_dict['status']).split('.')[1]
status_str += "Plan status: {}\n".format(plan_state_str)
status_str += "Plan filename: {}\n".format(status_dict['filename'])
status_str += "Next cycle num: {}\n".format(status_dict['next_cycle_number'])
status_str += "Idle bots: {}\n".format(status_dict['idle_bots'])
for id, data in status_dict['robots'].items():
needs_restart_str = ''
if data['needs_restart']:
needs_restart_str = "(Needs Restart)"
status_str += "{}{}:\n".format(id, needs_restart_str)
status_str += " Cycle id: {}\n".format(data["cycle_id"])
status_str += " Action id: {}\n".format(data["action_id"])
status_str += " Action name: {}\n".format(data["action_name"])
status_str += " Tile Coordinate: {}\n".format(data["tile_coordinate"])
status_str += " Vision offset: {}\n".format(data["vision_offset"])
# Set panel coloring based on state
if plan_state_str == "PAUSED" or plan_state_str == "ABORTED":
color_str = STATUS_PANEL_BAD_COLOR
elif plan_state_str != "NONE":
color_str = STATUS_PANEL_OK_COLOR
except Exception as e:
status_str = "Bad dict: " + str(status_dict)
self.window['_PLAN_STATUS_'].update(status_str, background_color=color_str)
def _update_robot_panel(self, robot_id, status_dict):
status_str = "Cannot get {} status".format(robot_id)
color_str = STATUS_PANEL_BAD_COLOR
if status_dict:
try:
robot_pose = [status_dict['pos_x'], status_dict['pos_y'], math.degrees(status_dict['pos_a'])]
last_mm_pose = [status_dict['last_mm_x'],status_dict['last_mm_y'], math.degrees(status_dict['last_mm_a']), status_dict['last_mm_used']]
status_str = ""
status_str += "Position: [{0:.3f} m, {1:.3f} m, {2:.2f} deg]\n".format(robot_pose[0], robot_pose[1], robot_pose[2])
status_str += "Last MM Pose: [{:.3f} m, {:.3f} m, {:.2f} deg]\n".format(
last_mm_pose[0], last_mm_pose[1], last_mm_pose[2])
status_str += "Velocity: [{0:.3f} m/s, {1:.3f} m/s, {2:.2f} deg/s]\n".format(status_dict['vel_x'],status_dict['vel_y'], math.degrees(status_dict['vel_a']))
status_str += "Vision Pose : [{0:.3f} m, {1:.3f} m, {2:.2f} deg]\n".format(status_dict['vision_x'],status_dict['vision_y'], math.degrees(status_dict['vision_a']))
status_str += "Camera Raw Pose : [{0:.3f} m, {1:.3f} m, {2:.2f} deg]\n".format(status_dict['cam_pose_x'],status_dict['cam_pose_y'], math.degrees(status_dict['cam_pose_a']))
status_str += "Cam Status: Both-{} | Side-{} | Rear-{}\n".format(status_dict["cam_both_ok"],status_dict["cam_side_ok"], status_dict["cam_rear_ok"])
status_str += "Raw Camera px: Side [{:d}, {:d}] Rear: [{:d}, {:d}]\n".format( \
int(status_dict['cam_side_u']),int(status_dict['cam_side_v']), int(status_dict['cam_rear_u']), int(status_dict['cam_rear_v']))
status_str += "Localization total confidence: {:.1f}%\n".format(status_dict['localization_total_confidence']*100)
status_str += " Axes confidence: [{:.1f}%, {:.1f}%, {:.1f}%]\n".format(
status_dict['localization_confidence_x']*100,status_dict['localization_confidence_y']*100,status_dict['localization_confidence_a']*100)
status_str += "Localization position uncertainty: {:.2f}\n".format(status_dict['last_position_uncertainty'])
status_str += "Controller timing: {} ms\n".format(status_dict['controller_loop_ms'])
status_str += "Position timing: {} ms\n".format(status_dict['position_loop_ms'])
status_str += "Camera timing: {} ms\n".format(status_dict['cam_loop_ms'])
status_str += "Current action: {}\n".format(status_dict['current_action'].split('.')[-1])
status_str += "Motion in progress: {}\n".format(status_dict["in_progress"])
status_str += "Has error: {}\n".format(status_dict["error_status"])
status_str += "Counter: {}\n".format(status_dict['counter'])
# Also update the visualization position
self._update_robot_viz_position(robot_id, robot_pose)
self._update_last_mm_pose(robot_id, last_mm_pose)
# If there is target position data populated, draw the target too
if 'current_move_data' in status_dict.keys():
self._update_target_viz_position(robot_id, robot_pose, status_dict['current_move_data'])
color_str = STATUS_PANEL_OK_COLOR
if status_dict["error_status"]:
color_str = STATUS_PANEL_BAD_COLOR
except Exception as e:
if "offline" in str(status_dict):
status_str = str(status_dict)
else:
status_str = "Bad dict: " + str(status_dict)
self.window['_{}_STATUS_'.format(robot_id.upper())].update(status_str, background_color=color_str)
def _update_robot_viz_position(self, robot_id, robot_pose):
if robot_id in self.viz_figs.keys():
for f in self.viz_figs[robot_id]:
self.window['_GRAPH_'].DeleteFigure(f)
self.viz_figs[robot_id] = self._draw_robot(robot_pose, use_target_color=False)
def _update_target_viz_position(self, robot_id, robot_pose, target_pose):
viz_key = "{}_target".format(robot_id)
if viz_key in self.viz_figs.keys():
for f in self.viz_figs[viz_key]:
self.window['_GRAPH_'].DeleteFigure(f)
if robot_pose and target_pose:
target_line = self.window['_GRAPH_'].draw_line(point_from=robot_pose[:2], point_to=target_pose[:2], color='yellow', width=2)
self.viz_figs[viz_key] = self._draw_robot(target_pose, use_target_color=True)
self.viz_figs[viz_key].append(target_line)
def _update_last_mm_pose(self, robot_id, last_mm_pose):
viz_key = "{}_last_mm".format(robot_id)
if viz_key in self.viz_figs.keys():
for f in self.viz_figs[viz_key]:
self.window['_GRAPH_'].DeleteFigure(f)
used = last_mm_pose[3]
if used:
color = 'green'
thickness = 2
else:
color = 'red'
thickness = 2
global_pose_2d = np.array(last_mm_pose[:2])
angle = last_mm_pose[2]
viz_handles = []
direction_pt = np.array([self.config.robot_direction_indicator_length, 0])
direction_arrow_x = self.config.robot_direction_indicator_arrow_length*math.cos(math.radians(self.config.robot_direction_indicator_arrow_angle))
direction_arrow_y = self.config.robot_direction_indicator_arrow_length*math.sin(math.radians(self.config.robot_direction_indicator_arrow_angle))
direction_arrow_left = direction_pt + np.array([-direction_arrow_x, direction_arrow_y])
direction_arrow_right = direction_pt + np.array([-direction_arrow_x, -direction_arrow_y])
global_direction_pt = Utils.TransformPos(direction_pt, global_pose_2d, angle)
viz_handles.append(self.window['_GRAPH_'].draw_line(point_from=list(global_pose_2d), point_to=list(global_direction_pt), color=color, width=thickness))
global_direction_arrow_left = Utils.TransformPos(direction_arrow_left, global_pose_2d, angle)
viz_handles.append(self.window['_GRAPH_'].draw_line(point_from=list(global_direction_pt), point_to=list(global_direction_arrow_left), color=color, width=thickness))
global_direction_arrow_right = Utils.TransformPos(direction_arrow_right, global_pose_2d, angle)
viz_handles.append(self.window['_GRAPH_'].draw_line(point_from=list(global_direction_pt), point_to=list(global_direction_arrow_right), color=color, width=thickness))
self.viz_figs[viz_key] = viz_handles
def _draw_robot(self, robot_pose, use_target_color):
robot_line_color = 'black'
robot_line_thickness = 2
direction_line_color = 'red'
direction_line_thickness = 1
if use_target_color:
robot_line_color = 'green'
robot_line_thickness = 2
direction_line_color = 'tomato'
direction_line_thickness = 1
robot_global_pos_2d = np.array([robot_pose[0],robot_pose[1]])
robot_angle = robot_pose[2]
robot_fig_handles = []
# Robot chassis points in robot frame
chassis_fl = np.array([self.config.robot_rotation_center_offset, self.config.robot_chassis_size/2.0])
chassis_fr = np.array([self.config.robot_rotation_center_offset, -self.config.robot_chassis_size/2.0])
chassis_bl = np.array([self.config.robot_rotation_center_offset - self.config.robot_chassis_size, self.config.robot_chassis_size/2.0])
chassis_br = np.array([self.config.robot_rotation_center_offset - self.config.robot_chassis_size, -self.config.robot_chassis_size/2.0])
# Tile points in robot frame
tile_bl = -self.config.tile_to_robot_offset
tile_br = tile_bl + np.array([0, -self.config.tile_size_width_meters])
tile_fl = tile_bl + np.array([self.config.tile_size_height_meters, 0])
tile_fr = tile_br + np.array([self.config.tile_size_height_meters, 0])
tile_front_center = (tile_fl + tile_fr) / 2.0
# Robot direction indicator
scale_factor = 0.7
direction_pt = np.array([scale_factor*self.config.robot_direction_indicator_length, 0])
direction_arrow_x = scale_factor*self.config.robot_direction_indicator_arrow_length*math.cos(math.radians(self.config.robot_direction_indicator_arrow_angle))
direction_arrow_y = scale_factor*self.config.robot_direction_indicator_arrow_length*math.sin(math.radians(self.config.robot_direction_indicator_arrow_angle))
direction_arrow_left = direction_pt + np.array([-direction_arrow_x, direction_arrow_y])
direction_arrow_right = direction_pt + np.array([-direction_arrow_x, -direction_arrow_y])
# Collect, transform, and draw chassis points
chassis_points_robot_frame = [chassis_fl, chassis_fr, chassis_br, chassis_bl] # order matters for drawing
chassis_points_global_frame = []
for pt in chassis_points_robot_frame:
global_pt = Utils.TransformPos(pt, robot_global_pos_2d, robot_angle)
chassis_points_global_frame.append(global_pt)
for i in range(4):
start_pt = list(chassis_points_global_frame[i])
end_pt = list(chassis_points_global_frame[(i+1)%4])
robot_fig_handles.append(self.window['_GRAPH_'].draw_line(point_from=start_pt, point_to=end_pt, color=robot_line_color, width = robot_line_thickness))
# Collect, transform, and draw tile points
tile_points_robot_frame = [tile_bl, tile_br, tile_fr, tile_fl] # order matters for drawing
tile_points_global_frame = []
for pt in tile_points_robot_frame:
global_pt = Utils.TransformPos(pt, robot_global_pos_2d, robot_angle)
tile_points_global_frame.append(global_pt)
for i in range(4):
start_pt = list(tile_points_global_frame[i])
end_pt = list(tile_points_global_frame[(i+1)%4])
robot_fig_handles.append(self.window['_GRAPH_'].draw_line(point_from=start_pt, point_to=end_pt, color=robot_line_color, width = robot_line_thickness))
# Draw angle indicator line
angle_root_pt = Utils.TransformPos(tile_front_center, robot_global_pos_2d, robot_angle)
global_direction_pt = Utils.TransformPos(direction_pt, angle_root_pt, robot_angle)
robot_fig_handles.append(self.window['_GRAPH_'].draw_line(point_from=list(angle_root_pt), point_to=list(global_direction_pt), color=direction_line_color, width=direction_line_thickness))
global_direction_arrow_left = Utils.TransformPos(direction_arrow_left, angle_root_pt, robot_angle)
robot_fig_handles.append(self.window['_GRAPH_'].draw_line(point_from=list(global_direction_pt), point_to=list(global_direction_arrow_left), color=direction_line_color, width=direction_line_thickness))
global_direction_arrow_right = Utils.TransformPos(direction_arrow_right, angle_root_pt, robot_angle)
robot_fig_handles.append(self.window['_GRAPH_'].draw_line(point_from=list(global_direction_pt), point_to=list(global_direction_arrow_right), color=direction_line_color, width=direction_line_thickness))
return robot_fig_handles
def _draw_environment(self):
# Robot boundaries
self.viz_figs["boundaires"] = self.window['_GRAPH_'].draw_rectangle(self.config.robot_boundaries[0], self.config.robot_boundaries[1], line_color='red')
# Domino field boundaries
bottom_left = self.config.domino_field_origin
rect_width_height = Utils.TransformPos(np.array([self.config.field_width, self.config.field_height]), [0,0], self.config.domino_field_angle)
top_left = (bottom_left[0], bottom_left[1]+rect_width_height[1])
bottom_right = (bottom_left[0] + rect_width_height[0], bottom_left[1])
self.viz_figs["field"] = self.window['_GRAPH_'].draw_rectangle(top_left, bottom_right, line_color='green', line_width=2)
# Base station
base_station_top_left = (self.config.base_station_boundaries[0][0], self.config.base_station_boundaries[1][1])
base_station_bottom_right = (self.config.base_station_boundaries[1][0], self.config.base_station_boundaries[0][1])
self.viz_figs["base"] = self.window['_GRAPH_'].draw_rectangle(base_station_top_left, base_station_bottom_right, line_color='blue')
# X axis
left_side = (self.config.graph_bottom_left[0], 0)
right_side = (self.config.graph_top_right[0], 0)
self.viz_figs["xaxis"] = self.window['_GRAPH_'].draw_line(left_side, right_side, color="black", width=2)
x_text_location = (right_side[0] - self.config.axes_label_offset, right_side[1] + self.config.axes_label_offset)
self.viz_figs["xaxis_label"] = self.window['_GRAPH_'].draw_text("X", x_text_location)
x_ticks = [i for i in range(0, int(right_side[0]), self.config.axes_tick_spacing)] + [i for i in range(0, int(left_side[0]), -self.config.axes_tick_spacing)]
x_ticks = np.sort(np.unique(np.array(x_ticks)))
for val in x_ticks:
bottom = (val, -self.config.axes_tick_size/2)
top = (val, self.config.axes_tick_size/2)
self.viz_figs["xtick_{}".format(val)] = self.window['_GRAPH_'].draw_line(bottom, top, color="black")
label_location = (bottom[0] + self.config.axes_label_offset, bottom[1])
self.viz_figs["xtick_label_{}".format(val)] = self.window['_GRAPH_'].draw_text("{}".format(val), label_location)
# Y axis
bottom_side = (0, self.config.graph_bottom_left[1])
top_side = (0, self.config.graph_top_right[1])
self.viz_figs["yaxis"] = self.window['_GRAPH_'].draw_line(bottom_side, top_side, color="black", width=2)
y_text_location = (top_side[0] + self.config.axes_label_offset, top_side[1] - self.config.axes_label_offset)
self.viz_figs["yaxis_label"] = self.window['_GRAPH_'].draw_text("Y", y_text_location)
y_ticks = [i for i in range(0, int(top_side[1]), self.config.axes_tick_spacing)] + [i for i in range(0, int(bottom_side[1]), -self.config.axes_tick_spacing)]
y_ticks = np.sort(np.unique(np.array(y_ticks)))
for val in y_ticks:
left = (-self.config.axes_tick_size/2, val)
right = (self.config.axes_tick_size/2, val)
self.viz_figs["ytick_{}".format(val)] = self.window['_GRAPH_'].draw_line(left, right, color="black")
label_location = (right[0], right[1]+self.config.axes_label_offset)
self.viz_figs["ytick_label_{}".format(val)] = self.window['_GRAPH_'].draw_text("{}".format(val), label_location)
def update_plan_display(self, plan_info):
if plan_info is None:
return
plan = plan_info[0]
plan_status = plan_info[1]
cycle_number = plan_info[2]
if not self.drawn_plan_grid:
rect_width_height = Utils.TransformPos(np.array([self.config.field_width, self.config.field_height]), [0,0], self.config.domino_field_angle)
bottom_left = self.config.domino_field_origin
bottom_right = (bottom_left[0], bottom_left[1]+rect_width_height[1])
top_left = (bottom_left[0] + rect_width_height[0], bottom_left[1])
for i in range(1, self.config.num_tiles_width):
dx = Utils.TransformPos(np.array([self.config.tile_size_width_meters, 0]), [0,0], self.config.domino_field_angle)
p1 = list(bottom_left + dx * i)
p2 = list(top_left + dx * i)
self.viz_figs["plan_vert_{}".format(i)] = self.window['_GRAPH_'].draw_line(p1, p2, color = "grey17")
for i in range(1, self.config.num_tiles_height):
dx = Utils.TransformPos(np.array([0, self.config.tile_size_height_meters]), [0,0], self.config.domino_field_angle)
p1 = list(bottom_left + dx * i)
p2 = list(bottom_right + dx * i)
self.viz_figs["plan_hz_{}".format(i)] = self.window['_GRAPH_'].draw_line(p1, p2, color = "grey17")
if type(plan) is SubsectionPlan:
origin = self.config.domino_field_origin
bottom_left = Utils.TransformPos(np.array(plan.field.tiles[-1].getPlacementPositionInMeters()), origin, self.config.domino_field_angle)
top_right = Utils.TransformPos(np.array(plan.field.tiles[0].getPlacementPositionInMeters())+
np.array((self.config.tile_size_width_meters,self.config.tile_size_height_meters)), origin, self.config.domino_field_angle)
self.viz_figs["subfield"] = self.window['_GRAPH_'].draw_rectangle(bottom_left, top_right, line_color='green', line_width=3)
self.drawn_plan_grid = True
if cycle_number != self.last_cycle_number_drawn:
tile_color = "red"
if plan_status in [PlanStatus.RUNNING, PlanStatus.PAUSED]:
tile_color = "blue"
if cycle_number <= len(plan.field.tiles):
if cycle_number < 0:
cycle_number = 0
if "plan_marker" in self.viz_figs:
self.window['_GRAPH_'].DeleteFigure(self.viz_figs["plan_marker"])
tile = plan.field.tiles[cycle_number]
location_field_frame = np.array(tile.getPlacementPositionInMeters()) + np.array([self.config.tile_size_width_meters, self.config.tile_size_height_meters])/2.0
location_global_frame = list(Utils.TransformPos(location_field_frame, self.config.domino_field_origin, self.config.domino_field_angle))
self.viz_figs["plan_marker"] = self.window['_GRAPH_'].draw_circle(location_global_frame, 0.25, fill_color=tile_color)
self.last_cycle_number_drawn = cycle_number
class Master:
def __init__(self, cfg, gui_handle):
self.cfg = cfg
self.cmd_gui = gui_handle
self.keep_mm_running = False
logging.info("Initializing Master")
self.runtime_manager = RuntimeManager(self.cfg)
self.runtime_manager.initialize()
def loop(self):
ready_to_exit = False
while not ready_to_exit:
try:
self.runtime_manager.update()
ready_to_exit = self.update_gui_and_handle_input()
except Exception as e:
logging.warning("Unhandled exception {}".format(str(e)))
if self.runtime_manager.get_plan_status() is PlanStatus.RUNNING:
self.runtime_manager.set_plan_status(PlanStatus.PAUSED)
# Clean up whenever loop exits
self.runtime_manager.shutdown(self.keep_mm_running)
self.cmd_gui.close()
def update_gui_and_handle_input(self):
# Handle any input from gui
event_type, event_data = self.cmd_gui.update()
if event_type == "Exit":
return True
if event_type == "ExitMM":
self.keep_mm_running = True
return True
if event_type == "Run":
if self.runtime_manager.get_plan_status() == PlanStatus.PAUSED:
logging.info("PLAN RESUMED")
self.runtime_manager.set_plan_status(PlanStatus.RUNNING)
if event_type == "Load":
if event_data is not '':
self.runtime_manager.load_plan(event_data)
if event_type == "Pause":
logging.info("PLAN PAUSED")
self.runtime_manager.estop()
self.runtime_manager.set_plan_status(PlanStatus.PAUSED)
if event_type == "Abort":
logging.warning("PLAN ABORTED")
self.runtime_manager.estop()
self.runtime_manager.set_plan_status(PlanStatus.ABORTED)
if event_type == "Action":
self.runtime_manager.run_manual_action(event_data)
if event_type == "_INC_CYCLE_":
self.runtime_manager.increment_robot_cycle("robot1")
if event_type == "_DEC_CYCLE_":
self.runtime_manager.decrement_robot_cycle("robot1")
if event_type == "_INC_ACTION_":
self.runtime_manager.increment_robot_action("robot1")
if event_type == "_DEC_ACTION_":
self.runtime_manager.decrement_robot_action("robot1")
if event_type == "_SET_CYCLE_":
self.runtime_manager.set_cycle("robot1", int(event_data[0]))
if event_type == "_SET_ACTION_":
self.runtime_manager.set_action("robot1", int(event_data[1]))
if event_type == "ESTOP":
self.runtime_manager.estop()
if self.runtime_manager.get_plan_status() == PlanStatus.RUNNING:
self.runtime_manager.set_plan_status(PlanStatus.PAUSED)
logging.warning("Pausing plan due to ESTOP event")
# Get metrics and update the displayed info
metrics = self.runtime_manager.get_all_metrics()
self.cmd_gui.update_status_panels(metrics)
self.cmd_gui.update_plan_display(self.runtime_manager.get_plan_info())
return False
def configure_logging(path):
rootLogger = logging.getLogger()
rootLogger.setLevel(logging.INFO)
if not os.path.isdir(path):
os.mkdir(path)
gitextract_eu8r6t07/
├── .gitignore
├── LICENSE
├── README.md
├── doc/
│ ├── DominoRobotControllerFlowchart.drawio
│ ├── DominoRobotSoftwareArchitecture.drawio
│ ├── KukaPsuedoCode.txt
│ ├── MassAndPowerCalcs.xlsx
│ ├── NetworkPrototcols.txt
│ ├── SpeedConversions.ods
│ ├── SubsystemDiagram.drawio
│ ├── SwClassHeirarchy.txt
│ └── clearcore_api.txt
├── experimental_testing/
│ ├── ImageParser.py
│ ├── RobotArduinoTesting/
│ │ ├── StepperDriverMotion_4Motor_Accel/
│ │ │ └── StepperDriverMotion_4Motor_Accel.ino
│ │ ├── TestMotor/
│ │ │ └── TestMotor.ino
│ │ ├── WifiServerWithESP/
│ │ │ ├── RobotServer.cpp
│ │ │ ├── RobotServer.h
│ │ │ └── WifiServerWithESP.ino
│ │ ├── WifiTesting/
│ │ │ └── WifiTesting.ino
│ │ ├── all_motors/
│ │ │ └── all_motors.ino
│ │ ├── all_motors_speed_control/
│ │ │ ├── Motor.cpp
│ │ │ ├── Motor.h
│ │ │ └── all_motors_speed_control.ino
│ │ ├── kalman_test/
│ │ │ ├── KalmanFilter.cpp
│ │ │ ├── KalmanFilter.h
│ │ │ └── kalman_test.ino
│ │ ├── lifter_nano_test/
│ │ │ └── lifter_nano_test.ino
│ │ ├── motor_driver_test_script/
│ │ │ └── motor_driver_test_script.ino
│ │ ├── new_dc_motor_tuning/
│ │ │ ├── Motor.cpp
│ │ │ ├── Motor.h
│ │ │ ├── constants.h
│ │ │ └── new_dc_motor_tuning.ino
│ │ ├── single_motor_speed_control/
│ │ │ ├── Motor.cpp
│ │ │ ├── Motor.h
│ │ │ └── single_motor_speed_control.ino
│ │ ├── stepper_nano_test/
│ │ │ └── stepper_nano_test.ino
│ │ └── test_encoders/
│ │ └── test_encoders.ino
│ ├── TrajGen.py
│ └── TrajGenv2.py
└── src/
├── master/
│ ├── DominoDesign-Questions.psd
│ ├── DominoDesign.psd
│ ├── FieldPlanner.py
│ ├── MarvelMindHandler.py
│ ├── MasterMain.py
│ ├── README.md
│ ├── RobotClient.py
│ ├── Runtime.py
│ ├── Utils.py
│ ├── __init__.py
│ ├── camera_utils.py
│ ├── config.py
│ ├── plans/
│ │ ├── AccuracyTesting_1x3_withdistance.p
│ │ ├── AccuracyTesting_1x3_withdistance086.p
│ │ ├── AccuracyTesting_2x2.p
│ │ ├── AccuracyTesting_3x3_withdistance_2axis.p
│ │ ├── DockingTest1.p
│ │ ├── DockingTest2.p
│ │ ├── FullPlan_DominoBros.p
│ │ ├── FullTest1.p
│ │ ├── HackyTestPlan.p
│ │ ├── LargeScale1_5x5.p
│ │ ├── LargeScale2_5x5.p
│ │ ├── VisionAccuracyTesting_2x2_SmallTestArea.p
│ │ ├── autosaved.p
│ │ ├── plan.p
│ │ ├── plan2.p
│ │ ├── vision_offsets small_testing_area.csv
│ │ └── vision_offsets_full_plan.csv
│ ├── plot_logs2.py
│ └── requirements.txt
├── robot/
│ ├── Makefile
│ ├── README.md
│ ├── src/
│ │ ├── KalmanFilter.cpp
│ │ ├── KalmanFilter.h
│ │ ├── Localization.cpp
│ │ ├── Localization.h
│ │ ├── MarvelmindWrapper.cpp
│ │ ├── MarvelmindWrapper.h
│ │ ├── RobotController.cpp
│ │ ├── RobotController.h
│ │ ├── RobotServer.cpp
│ │ ├── RobotServer.h
│ │ ├── SmoothTrajectoryGenerator.cpp
│ │ ├── SmoothTrajectoryGenerator.h
│ │ ├── StatusUpdater.cpp
│ │ ├── StatusUpdater.h
│ │ ├── TrayController.cpp
│ │ ├── TrayController.h
│ │ ├── camera_tracker/
│ │ │ ├── CameraPipeline.cpp
│ │ │ ├── CameraPipeline.h
│ │ │ ├── CameraTracker.cpp
│ │ │ ├── CameraTracker.h
│ │ │ ├── CameraTrackerBase.h
│ │ │ ├── CameraTrackerFactory.cpp
│ │ │ ├── CameraTrackerFactory.h
│ │ │ └── CameraTrackerMock.h
│ │ ├── constants.cfg
│ │ ├── constants.h
│ │ ├── main.cpp
│ │ ├── robot.cpp
│ │ ├── robot.h
│ │ ├── robot_controller_modes/
│ │ │ ├── RobotControllerModeBase.cpp
│ │ │ ├── RobotControllerModeBase.h
│ │ │ ├── RobotControllerModePosition.cpp
│ │ │ ├── RobotControllerModePosition.h
│ │ │ ├── RobotControllerModeStopFast.cpp
│ │ │ ├── RobotControllerModeStopFast.h
│ │ │ ├── RobotControllerModeVision.cpp
│ │ │ └── RobotControllerModeVision.h
│ │ ├── serial/
│ │ │ ├── MockSerialComms.cpp
│ │ │ ├── MockSerialComms.h
│ │ │ ├── SerialComms.cpp
│ │ │ ├── SerialComms.h
│ │ │ ├── SerialCommsBase.cpp
│ │ │ ├── SerialCommsBase.h
│ │ │ ├── SerialCommsFactory.cpp
│ │ │ └── SerialCommsFactory.h
│ │ ├── sockets/
│ │ │ ├── ClientSocket.cpp
│ │ │ ├── ClientSocket.h
│ │ │ ├── MockSocketMultiThreadWrapper.cpp
│ │ │ ├── MockSocketMultiThreadWrapper.h
│ │ │ ├── README.md
│ │ │ ├── ServerSocket.cpp
│ │ │ ├── ServerSocket.h
│ │ │ ├── Socket.cpp
│ │ │ ├── Socket.h
│ │ │ ├── SocketException.h
│ │ │ ├── SocketMultiThreadWrapper.cpp
│ │ │ ├── SocketMultiThreadWrapper.h
│ │ │ ├── SocketMultiThreadWrapperBase.h
│ │ │ ├── SocketMultiThreadWrapperFactory.cpp
│ │ │ ├── SocketMultiThreadWrapperFactory.h
│ │ │ └── SocketTimeoutException.h
│ │ ├── utils.cpp
│ │ └── utils.h
│ └── test/
│ ├── CameraPipeline_Test.cpp
│ ├── CameraTracker_Test.cpp
│ ├── Localization_Test.cpp
│ ├── RobotController_Test.cpp
│ ├── RobotServer_Test.cpp
│ ├── Robot_Test.cpp
│ ├── SanityCheckTest.cpp
│ ├── SmoothTrajectoryGenerator_Test.cpp
│ ├── SocketWrapper_Test.cpp
│ ├── StatusUpdater_Test.cpp
│ ├── TrayController_Test.cpp
│ ├── test-main.cpp
│ ├── test-utils.h
│ ├── test_constants.cfg
│ └── utils_Test.cpp
├── robot_motor_driver/
│ ├── SerialComms.cpp
│ ├── SerialComms.h
│ └── robot_motor_driver.ino
└── tools/
├── 80-domino-bot.rules
├── IR_calibration_1.yml
├── IR_calibration_2.yml
├── camera_calibration.py
├── motor_test_script.py
└── plot_logs.py
SYMBOL INDEX (411 symbols across 79 files)
FILE: experimental_testing/RobotArduinoTesting/WifiServerWithESP/RobotServer.cpp
function String (line 59) | String RobotServer::cleanString(String message)
function String (line 66) | String RobotServer::getAnyIncomingMessage()
FILE: experimental_testing/RobotArduinoTesting/WifiServerWithESP/RobotServer.h
function class (line 11) | class RobotServer
FILE: experimental_testing/RobotArduinoTesting/all_motors_speed_control/Motor.h
function class (line 16) | class Motor
FILE: experimental_testing/RobotArduinoTesting/kalman_test/KalmanFilter.h
function class (line 8) | class KalmanFilter
FILE: experimental_testing/RobotArduinoTesting/new_dc_motor_tuning/Motor.h
function class (line 8) | class Motor
FILE: experimental_testing/RobotArduinoTesting/new_dc_motor_tuning/constants.h
type COMMAND (line 109) | enum COMMAND
FILE: experimental_testing/RobotArduinoTesting/single_motor_speed_control/Motor.h
function class (line 19) | class Motor
FILE: experimental_testing/TrajGen.py
function sign (line 18) | def sign(n):
function gen_traj2 (line 27) | def gen_traj2(p1, p2, dt):
function gen_triangle_traj (line 37) | def gen_triangle_traj(p1, p2, dt):
function gen_trap_traj (line 45) | def gen_trap_traj(p1, p2, dt):
function gen_const_acc_until_pos (line 56) | def gen_const_acc_until_pos(traj, dt, acc, stop_pos, stop_dir):
function plot_traj (line 87) | def plot_traj(pvt_data):
FILE: experimental_testing/TrajGenv2.py
function generate (line 16) | def generate(target_position):
function generate_once (line 47) | def generate_once(p, v_lim, a_lim, j_lim):
function generate_inverse (line 101) | def generate_inverse(p, dt_j, dt_a, dt_v):
function generate_profile_from_params (line 129) | def generate_profile_from_params(output, timestep):
function plot_data (line 182) | def plot_data(t,p,v,a,j, output):
function plot_trapazoidal_profile (line 212) | def plot_trapazoidal_profile():
FILE: src/master/FieldPlanner.py
function generateVisionOffsetMap (line 18) | def generateVisionOffsetMap(cfg, max_x, max_y):
class DominoField (line 49) | class DominoField:
method __init__ (line 54) | def __init__(self, cfg):
method generate (line 64) | def generate(self):
method printStats (line 75) | def printStats(self):
method show_image_parsing (line 86) | def show_image_parsing(self):
method render_domino_image_tiles (line 103) | def render_domino_image_tiles(self):
method show_tile_ordering (line 126) | def show_tile_ordering(self):
method _generateField (line 152) | def _generateField(self):
method _addTile (line 204) | def _addTile(self, tile_coordinate, tile_values, tile_order, vision_of...
method _generateTiles (line 211) | def _generateTiles(self):
method _generateTileOrdering (line 246) | def _generateTileOrdering(cls, num_x, num_y):
method _generateTileOrderingColumns (line 255) | def _generateTileOrderingColumns(cls, num_x, num_y):
method _generateTileOrderingDiagonal (line 265) | def _generateTileOrderingDiagonal(cls, num_x, num_y):
class Tile (line 303) | class Tile:
method __init__ (line 308) | def __init__(self, cfg, coordinate, values, order, vision_offset):
method getPlacementPositionInMeters (line 315) | def getPlacementPositionInMeters(self):
method draw (line 323) | def draw(self, array):
method draw_single (line 338) | def draw_single(self, array, tile_start_x_px, tile_start_y_px, tile_en...
class Action (line 368) | class Action:
method __init__ (line 370) | def __init__(self, action_type, name):
method draw (line 374) | def draw(self, ax, text=""):
class SetPoseAction (line 377) | class SetPoseAction(Action):
method __init__ (line 378) | def __init__(self, action_type, name, x, y, a):
class WaitAction (line 391) | class WaitAction(Action):
method __init__ (line 392) | def __init__(self,action_type, name, time):
class MoveConstVelAction (line 396) | class MoveConstVelAction(Action):
method __init__ (line 398) | def __init__(self, action_type, name, vx, vy, va, t):
class MoveAction (line 413) | class MoveAction(Action):
method __init__ (line 415) | def __init__(self, action_type, name, x, y, a):
method getPos (line 428) | def getPos(self):
method getAngleDegrees (line 431) | def getAngleDegrees(self):
method getAngleRadians (line 434) | def getAngleRadians(self):
method draw (line 437) | def draw(self, ax, text="", show_label=True):
function generate_full_action_sequence (line 475) | def generate_full_action_sequence(cfg, tile):
function generate_hax_action_sequence (line 663) | def generate_hax_action_sequence(cfg, tile):
function generate_small_testing_action_sequence (line 689) | def generate_small_testing_action_sequence(cfg, tile):
function draw_env (line 766) | def draw_env(cfg):
class Cycle (line 803) | class Cycle:
method __init__ (line 805) | def __init__(self, id, cfg, robot_id, tile, action_sequence):
method draw_cycle (line 813) | def draw_cycle(self, ax):
method draw_action (line 817) | def draw_action(self, ax, idx, text=""):
function generate_standard_cycles (line 821) | def generate_standard_cycles(cfg, field, cycle_generator_fn):
class BasePlan (line 837) | class BasePlan:
method __init__ (line 839) | def __init__(self, cycles):
method get_cycle (line 842) | def get_cycle(self, cycle_num):
method get_action (line 848) | def get_action(self, cycle_id, action_id):
class RealPlan (line 858) | class RealPlan(BasePlan):
method __init__ (line 860) | def __init__(self, cfg, field, cycles):
method draw_cycle (line 865) | def draw_cycle(self, cycle_num):
method find_pose_move_idx (line 870) | def find_pose_move_idx(self, cycle):
method draw_all_tile_poses (line 890) | def draw_all_tile_poses(self):
class Plan (line 897) | class Plan(RealPlan):
method __init__ (line 899) | def __init__(self, cfg, cycle_generator_fn):
class SubsectionPlan (line 908) | class SubsectionPlan(RealPlan):
method __init__ (line 910) | def __init__(self, full_plan):
class TestPlan (line 945) | class TestPlan(BasePlan):
method __init__ (line 950) | def __init__(self):
function RunFieldPlanning (line 960) | def RunFieldPlanning(autosave=False):
function GeneratePDF (line 991) | def GeneratePDF(plan):
FILE: src/master/MarvelMindHandler.py
class MarvelmindWrapper (line 11) | class MarvelmindWrapper:
method __init__ (line 13) | def __init__(self, cfg):
method __del__ (line 23) | def __del__(self):
method _open_serial_port (line 26) | def _open_serial_port(self):
method _close_serial_port (line 37) | def _close_serial_port(self):
method check_all_devices_status (line 47) | def check_all_devices_status(self):
method wake_all_devices (line 96) | def wake_all_devices(self):
method wake_all_devices_only_if_needed (line 100) | def wake_all_devices_only_if_needed(self):
method sleep_all_devices (line 107) | def sleep_all_devices(self):
method wake_device (line 111) | def wake_device(self, address):
method sleep_device (line 126) | def sleep_device(self, address):
method get_metrics (line 140) | def get_metrics(self):
class MockMarvelmindWrapper (line 144) | class MockMarvelmindWrapper:
method __init__ (line 145) | def __init__(self, cfg):
method check_all_devices_status (line 147) | def check_all_devices_status(self):
method wake_all_devices_only_if_needed (line 149) | def wake_all_devices_only_if_needed(self):
method wake_all_devices (line 151) | def wake_all_devices(self):
method sleep_all_devices (line 153) | def sleep_all_devices(self):
method wake_device (line 155) | def wake_device(self, address):
method sleep_device (line 157) | def sleep_device(self, address):
method get_metrics (line 159) | def get_metrics(self):
FILE: src/master/MasterMain.py
function status_panel (line 19) | def status_panel(name):
function setup_gui_layout (line 26) | def setup_gui_layout(config, panel_names, target_names):
class CmdGui (line 80) | class CmdGui:
method __init__ (line 82) | def __init__(self, config):
method close (line 104) | def close(self):
method update (line 108) | def update(self):
method update_status_panels (line 167) | def update_status_panels(self, metrics):
method _parse_manual_action (line 177) | def _parse_manual_action(self, values):
method _udpate_cycle_button_status (line 222) | def _udpate_cycle_button_status(self, disabled):
method _update_plan_button_status (line 232) | def _update_plan_button_status(self, plan_status):
method _update_plan_panel (line 266) | def _update_plan_panel(self, status_dict):
method _update_robot_panel (line 300) | def _update_robot_panel(self, robot_id, status_dict):
method _update_robot_viz_position (line 347) | def _update_robot_viz_position(self, robot_id, robot_pose):
method _update_target_viz_position (line 353) | def _update_target_viz_position(self, robot_id, robot_pose, target_pose):
method _update_last_mm_pose (line 364) | def _update_last_mm_pose(self, robot_id, last_mm_pose):
method _draw_robot (line 394) | def _draw_robot(self, robot_pose, use_target_color):
method _draw_environment (line 465) | def _draw_environment(self):
method update_plan_display (line 513) | def update_plan_display(self, plan_info):
class Master (line 563) | class Master:
method __init__ (line 565) | def __init__(self, cfg, gui_handle):
method loop (line 575) | def loop(self):
method update_gui_and_handle_input (line 591) | def update_gui_and_handle_input(self):
function configure_logging (line 643) | def configure_logging(path):
FILE: src/master/RobotClient.py
class TcpClient (line 16) | class TcpClient:
method __init__ (line 18) | def __init__(self, ip, port, timeout):
method send (line 24) | def send(self, msg, print_debug=True):
method recieve (line 40) | def recieve(self, timeout=0.1, print_debug=True):
class ClientBase (line 86) | class ClientBase:
method __init__ (line 88) | def __init__(self, ip):
method wait_for_server_response (line 93) | def wait_for_server_response(self, expected_msg_type, timeout=0.1, pri...
method send_msg_and_wait_for_ack (line 135) | def send_msg_and_wait_for_ack(self, msg, print_debug=False, timeout=0.1):
method net_status (line 167) | def net_status(self):
method request_status (line 179) | def request_status(self):
method estop (line 189) | def estop(self):
class RobotClient (line 195) | class RobotClient(ClientBase):
method __init__ (line 197) | def __init__(self, cfg, robot_id):
method move (line 203) | def move(self, x, y, a):
method move_rel (line 208) | def move_rel(self, x, y, a):
method move_rel_slow (line 213) | def move_rel_slow(self, x, y, a):
method move_fine (line 218) | def move_fine(self, x, y, a):
method move_fine_stop_vision (line 223) | def move_fine_stop_vision(self, x, y, a):
method move_with_vision (line 228) | def move_with_vision(self, x, y, a):
method move_const_vel (line 233) | def move_const_vel(self, vx, vy, va, t):
method place (line 238) | def place(self):
method load (line 243) | def load(self):
method tray_init (line 248) | def tray_init(self):
method load_complete (line 253) | def load_complete(self):
method clear_error (line 258) | def clear_error(self):
method wait_for_localization (line 263) | def wait_for_localization(self):
method set_pose (line 268) | def set_pose(self, x, y, a):
method toggle_vision_debug (line 273) | def toggle_vision_debug(self):
method start_cameras (line 278) | def start_cameras(self):
method stop_cameras (line 283) | def stop_cameras(self):
class BaseStationClient (line 288) | class BaseStationClient(ClientBase):
method __init__ (line 290) | def __init__(self, cfg):
method load (line 294) | def load(self):
class MockRobotClient (line 301) | class MockRobotClient:
method __init__ (line 302) | def __init__(self, cfg, robot_id):
method move (line 307) | def move(self, x, y, a):
method move_rel (line 310) | def move_rel(self, x, y, a):
method move_rel_slow (line 313) | def move_rel_slow(self, x, y, a):
method move_fine (line 316) | def move_fine(self, x, y, a):
method place (line 319) | def place(self):
method net_status (line 322) | def net_status(self):
method estop (line 325) | def estop(self):
method load (line 328) | def load(self):
method request_status (line 331) | def request_status(self):
method clear_error (line 334) | def clear_error(self):
method wait_for_localization (line 337) | def wait_for_localization(self):
method toggle_distance (line 340) | def toggle_distance(self):
class MockBaseStationClient (line 343) | class MockBaseStationClient:
method __init__ (line 345) | def __init__(self, cfg):
method load (line 349) | def load(self):
method net_status (line 352) | def net_status(self):
method estop (line 355) | def estop(self):
method request_status (line 358) | def request_status(self):
FILE: src/master/Runtime.py
class RobotInterface (line 18) | class RobotInterface:
method __init__ (line 19) | def __init__(self, config, robot_id):
method bring_online (line 33) | def bring_online(self, use_mock=False):
method check_online (line 36) | def check_online(self):
method get_robot_metrics (line 39) | def get_robot_metrics(self):
method _setup_action_map (line 48) | def _setup_action_map(self):
method _bring_comms_online (line 62) | def _bring_comms_online(self, use_mock=False):
method _get_status_from_robot (line 79) | def _get_status_from_robot(self):
method update (line 90) | def update(self):
method run_action (line 108) | def run_action(self, action):
class BaseStationInterface (line 157) | class BaseStationInterface:
method __init__ (line 159) | def __init__(self, config):
method bring_online (line 166) | def bring_online(self, use_mock=False):
method get_last_status (line 176) | def get_last_status(self):
method check_online (line 179) | def check_online(self):
method update (line 182) | def update(self):
method _get_status_from_base_station (line 192) | def _get_status_from_base_station(self):
method run_action (line 198) | def run_action(self, action):
class PlanStatus (line 219) | class PlanStatus(enum.Enum):
class RuntimeManager (line 227) | class RuntimeManager:
method __init__ (line 233) | def __init__(self, config):
method initialize (line 270) | def initialize(self):
method shutdown (line 302) | def shutdown(self, keep_mm_awake):
method get_initialization_status (line 308) | def get_initialization_status(self):
method get_all_metrics (line 312) | def get_all_metrics(self):
method update (line 315) | def update(self):
method run_manual_action (line 331) | def run_manual_action(self, manual_action):
method estop (line 336) | def estop(self):
method load_plan (line 342) | def load_plan(self, plan_file):
method set_plan_status (line 348) | def set_plan_status(self, status):
method increment_robot_cycle (line 361) | def increment_robot_cycle(self, target):
method decrement_robot_cycle (line 364) | def decrement_robot_cycle(self, target):
method increment_robot_action (line 367) | def increment_robot_action(self, target):
method decrement_robot_action (line 370) | def decrement_robot_action(self, target):
method set_cycle (line 373) | def set_cycle(self, target, cycle_num):
method set_action (line 376) | def set_action(self, target, action_num):
method get_plan_status (line 379) | def get_plan_status(self):
method get_plan_info (line 382) | def get_plan_info(self):
method _load_plan_from_object (line 388) | def _load_plan_from_object(self, plan, plan_path=""):
method _load_plan_from_file (line 396) | def _load_plan_from_file(self, plan_file):
method _update_plan (line 408) | def _update_plan(self):
method _any_idle_bots (line 424) | def _any_idle_bots(self):
method _assign_new_cycle (line 427) | def _assign_new_cycle(self, cycle):
method _run_action (line 437) | def _run_action(self, target, action):
method _check_initialization_status (line 450) | def _check_initialization_status(self):
method _update_all_metrics (line 462) | def _update_all_metrics(self):
method _get_plan_metrics (line 478) | def _get_plan_metrics(self):
method _update_cycle_actions (line 505) | def _update_cycle_actions(self):
method _modify_cycle_state (line 572) | def _modify_cycle_state(self, target, add_cycle_id=None, add_action_id...
method _cycle_state_to_file (line 619) | def _cycle_state_to_file(self):
method _load_cycle_state_from_file (line 635) | def _load_cycle_state_from_file(self, filepath):
method _erase_cycle_state_file (line 660) | def _erase_cycle_state_file(self):
FILE: src/master/Utils.py
class ActionTypes (line 6) | class ActionTypes(enum.Enum):
class NonBlockingTimer (line 30) | class NonBlockingTimer:
method __init__ (line 32) | def __init__(self, trigger_time):
method check (line 36) | def check(self):
function write_file (line 42) | def write_file(filename, text):
function TransformPos (line 49) | def TransformPos(pos_2d, frame_offset_2d, frame_angle):
class ActionValidator (line 62) | class ActionValidator:
method __init__ (line 64) | def __init__(self):
method update_expected_action (line 68) | def update_expected_action(self, expected_action):
method update_action_validation (line 72) | def update_action_validation(self, metric):
FILE: src/master/camera_utils.py
function scp_image (line 11) | def scp_image(ip, remote_path, local_path):
function display_debug_image (line 20) | def display_debug_image(local_path):
function get_and_display_multiple_images (line 27) | def get_and_display_multiple_images(cam_name, remote_ip, remote_path, lo...
FILE: src/master/config.py
class Config (line 5) | class Config:
FILE: src/master/plot_logs2.py
function scp_last_motion_log (line 29) | def scp_last_motion_log(ip, remote_path, local_path):
function init_data (line 38) | def init_data():
function parse_log_file (line 44) | def parse_log_file(path):
function parse_row (line 59) | def parse_row(row, data):
function plot_data (line 79) | def plot_data(data, rows_to_plot=None):
function plot_rows_axes (line 95) | def plot_rows_axes(data, rows, axes):
FILE: src/robot/src/KalmanFilter.h
function class (line 13) | class KalmanFilter
FILE: src/robot/src/Localization.h
function class (line 8) | class Localization
FILE: src/robot/src/MarvelmindWrapper.h
function class (line 10) | class MarvelmindWrapper
FILE: src/robot/src/RobotController.h
function class (line 11) | class RobotController
FILE: src/robot/src/RobotServer.cpp
function COMMAND (line 22) | COMMAND RobotServer::getCommand(std::string message)
function COMMAND (line 218) | COMMAND RobotServer::oneLoop()
FILE: src/robot/src/RobotServer.h
function class (line 19) | class RobotServer
FILE: src/robot/src/SmoothTrajectoryGenerator.cpp
function PVTPoint (line 18) | PVTPoint SmoothTrajectoryGenerator::lookup(float time)
function lookup_1D (line 40) | std::vector<float> lookup_1D(float time, const SCurveParameters& params)
function MotionPlanningProblem (line 122) | MotionPlanningProblem buildMotionPlanningProblem(Point initialPoint, Poi...
function sCurveWithinLimits (line 170) | bool sCurveWithinLimits(const SCurveParameters& params, const DynamicLim...
function Trajectory (line 175) | Trajectory generateTrajectory(MotionPlanningProblem problem)
function generateSCurve (line 234) | bool generateSCurve(float dist, DynamicLimits limits, const SolverParame...
function populateSwitchTimeParameters (line 307) | void populateSwitchTimeParameters(SCurveParameters* params, float dt_j, ...
function synchronizeParameters (line 343) | bool synchronizeParameters(SCurveParameters* params1, SCurveParameters* ...
function slowDownParamsToMatchTime (line 363) | bool slowDownParamsToMatchTime(SCurveParameters* params, float time_to_m...
function solveInverse (line 380) | bool solveInverse(SCurveParameters* params)
function computeKinematicsBasedOnRegion (line 413) | std::vector<float> computeKinematicsBasedOnRegion(const SCurveParameters...
FILE: src/robot/src/SmoothTrajectoryGenerator.h
type DynamicLimits (line 26) | struct DynamicLimits
type SwitchPoint (line 44) | struct SwitchPoint
type SolverParameters (line 88) | struct SolverParameters
type class (line 96) | enum class
type MotionPlanningProblem (line 105) | struct MotionPlanningProblem
function class (line 126) | class SmoothTrajectoryGenerator
FILE: src/robot/src/StatusUpdater.h
function setErrorStatus (line 27) | void setErrorStatus() { currentStatus_.error_status = true;}
function clearErrorStatus (line 29) | void clearErrorStatus() {currentStatus_.error_status = false;}
function updateCameraDebug (line 41) | void updateCameraDebug(CameraDebug camera_debug) {currentStatus_.camera_...
FILE: src/robot/src/TrayController.cpp
type LifterPosType (line 6) | enum class LifterPosType
function getPos (line 13) | int getPos(LifterPosType pos_type)
FILE: src/robot/src/TrayController.h
function class (line 7) | class TrayController
FILE: src/robot/src/camera_tracker/CameraPipeline.cpp
function getBestKeypoint (line 9) | cv::Point2f getBestKeypoint(std::vector<cv::KeyPoint> keypoints)
function CameraPipelineOutput (line 347) | CameraPipelineOutput CameraPipeline::getData()
FILE: src/robot/src/camera_tracker/CameraPipeline.h
type class (line 10) | enum class
type CameraPipelineOutput (line 16) | struct CameraPipelineOutput
function class (line 24) | class CameraPipeline
FILE: src/robot/src/camera_tracker/CameraTracker.cpp
function CameraTrackerOutput (line 85) | CameraTrackerOutput CameraTracker::getPoseFromCamera()
function CameraDebug (line 90) | CameraDebug CameraTracker::getCameraDebug()
function Point (line 116) | Point CameraTracker::computeRobotPoseFromImagePoints(Eigen::Vector2f p_s...
FILE: src/robot/src/camera_tracker/CameraTracker.h
function class (line 9) | class CameraTracker : public CameraTrackerBase
FILE: src/robot/src/camera_tracker/CameraTrackerBase.h
type CameraTrackerOutput (line 6) | struct CameraTrackerOutput
function class (line 14) | class CameraTrackerBase
FILE: src/robot/src/camera_tracker/CameraTrackerFactory.cpp
function CameraTrackerFactory (line 10) | CameraTrackerFactory* CameraTrackerFactory::getFactoryInstance()
function CameraTrackerBase (line 24) | CameraTrackerBase* CameraTrackerFactory::get_camera_tracker()
FILE: src/robot/src/camera_tracker/CameraTrackerFactory.h
type class (line 9) | enum class
function class (line 15) | class CameraTrackerFactory
FILE: src/robot/src/camera_tracker/CameraTrackerMock.h
function virtual (line 12) | virtual void stop() override {}
function virtual (line 14) | virtual void update() override {}
function virtual (line 16) | virtual bool running() override {return true;}
function virtual (line 18) | virtual void toggleDebugImageOutput() override {}
function virtual (line 20) | virtual CameraTrackerOutput getPoseFromCamera() override { return {{0,0,...
FILE: src/robot/src/constants.h
type class (line 26) | enum class
FILE: src/robot/src/main.cpp
function configure_logger (line 17) | void configure_logger()
function setup_mock_socket (line 47) | void setup_mock_socket()
function capture_images (line 62) | void capture_images()
function main (line 81) | int main(int argc, char** argv)
FILE: src/robot/src/robot.h
function class (line 12) | class WaitForLocalizeHelper
function class (line 27) | class Robot
FILE: src/robot/src/robot_controller_modes/RobotControllerModeBase.h
function class (line 7) | class RobotControllerModeBase
FILE: src/robot/src/robot_controller_modes/RobotControllerModePosition.cpp
function Velocity (line 45) | Velocity RobotControllerModePosition::computeTargetVelocity(Point curren...
FILE: src/robot/src/robot_controller_modes/RobotControllerModePosition.h
function class (line 8) | class RobotControllerModePosition : public RobotControllerModeBase
FILE: src/robot/src/robot_controller_modes/RobotControllerModeStopFast.cpp
function Velocity (line 50) | Velocity RobotControllerModeStopFast::computeTargetVelocity(Point curren...
FILE: src/robot/src/robot_controller_modes/RobotControllerModeStopFast.h
function class (line 8) | class RobotControllerModeStopFast : public RobotControllerModeBase
FILE: src/robot/src/robot_controller_modes/RobotControllerModeVision.cpp
function Velocity (line 64) | Velocity RobotControllerModeVision::computeTargetVelocity(Point current_...
FILE: src/robot/src/robot_controller_modes/RobotControllerModeVision.h
function class (line 11) | class RobotControllerModeVision : public RobotControllerModeBase
FILE: src/robot/src/serial/MockSerialComms.h
function class (line 9) | class MockSerialComms : public SerialCommsBase
FILE: src/robot/src/serial/SerialComms.h
function class (line 12) | class SerialComms : public SerialCommsBase
FILE: src/robot/src/serial/SerialCommsBase.h
function class (line 9) | class SerialCommsBase
FILE: src/robot/src/serial/SerialCommsFactory.cpp
function SerialCommsFactory (line 10) | SerialCommsFactory* SerialCommsFactory::getFactoryInstance()
function SerialCommsBase (line 24) | SerialCommsBase* SerialCommsFactory::get_serial_comms(std::string portName)
FILE: src/robot/src/serial/SerialCommsFactory.h
type class (line 9) | enum class
function class (line 15) | class SerialCommsFactory
FILE: src/robot/src/sockets/ClientSocket.cpp
function ClientSocket (line 22) | const ClientSocket& ClientSocket::operator << ( const std::string& s ) c...
function ClientSocket (line 34) | const ClientSocket& ClientSocket::operator >> ( std::string& s ) const
FILE: src/robot/src/sockets/ClientSocket.h
function class (line 9) | class ClientSocket : private Socket
FILE: src/robot/src/sockets/MockSocketMultiThreadWrapper.h
function class (line 10) | class MockSocketMultiThreadWrapper : public SocketMultiThreadWrapperBase
FILE: src/robot/src/sockets/ServerSocket.cpp
function ServerSocket (line 33) | const ServerSocket& ServerSocket::operator << ( const std::string& s ) c...
function ServerSocket (line 45) | const ServerSocket& ServerSocket::operator >> ( std::string& s ) const
FILE: src/robot/src/sockets/ServerSocket.h
function class (line 11) | class ServerSocket : private Socket
FILE: src/robot/src/sockets/Socket.cpp
type sockaddr (line 66) | struct sockaddr
FILE: src/robot/src/sockets/Socket.h
function class (line 20) | class Socket
FILE: src/robot/src/sockets/SocketException.h
function class (line 9) | class SocketException
FILE: src/robot/src/sockets/SocketMultiThreadWrapper.h
function class (line 10) | class SocketMultiThreadWrapper : public SocketMultiThreadWrapperBase
FILE: src/robot/src/sockets/SocketMultiThreadWrapperBase.h
function class (line 6) | class SocketMultiThreadWrapperBase
FILE: src/robot/src/sockets/SocketMultiThreadWrapperFactory.cpp
function SocketMultiThreadWrapperFactory (line 10) | SocketMultiThreadWrapperFactory* SocketMultiThreadWrapperFactory::getFac...
function SocketMultiThreadWrapperBase (line 55) | SocketMultiThreadWrapperBase* SocketMultiThreadWrapperFactory::get_socket()
FILE: src/robot/src/sockets/SocketMultiThreadWrapperFactory.h
type class (line 8) | enum class
function class (line 14) | class SocketMultiThreadWrapperFactory
FILE: src/robot/src/sockets/SocketTimeoutException.h
function class (line 9) | class SocketTimeoutException
FILE: src/robot/src/utils.cpp
function wrap_angle (line 15) | float wrap_angle(float a)
function angle_diff (line 31) | float angle_diff(float a1, float a2)
function ClockTimePoint (line 145) | ClockTimePoint ClockWrapper::now()
function ClockTimePoint (line 156) | ClockTimePoint MockClockWrapper::now()
function ClockFactory (line 190) | ClockFactory* ClockFactory::getFactoryInstance()
function ClockWrapperBase (line 204) | ClockWrapperBase* ClockFactory::get_clock()
function vectorMean (line 255) | float vectorMean(const std::vector<float>& data)
function vectorStddev (line 267) | float vectorStddev(const std::vector<float>& data, float mean)
function zScore (line 280) | float zScore(float mean, float stddev, float reading)
function parseCommaDelimitedString (line 288) | std::vector<std::string> parseCommaDelimitedString(const std::string& st...
function parseCommaDelimitedStringToFloat (line 301) | std::vector<float> parseCommaDelimitedStringToFloat(const std::string& s...
function reset_last_motion_logger (line 316) | void reset_last_motion_logger()
FILE: src/robot/src/utils.h
function class (line 38) | class ClockWrapperBase
function class (line 44) | class ClockWrapper : public ClockWrapperBase
function class (line 50) | class MockClockWrapper : public ClockWrapperBase
function operator (line 188) | bool operator== (const Velocity& other) const
function class (line 206) | class LatchedBool
function get (line 231) | bool get() {return value_;}
function update (line 232) | bool update(bool value_in)
function insert (line 258) | void insert(T val)
function clear (line 277) | void clear()
function class (line 310) | class PositionController
type LocalizationMetrics (line 335) | struct LocalizationMetrics
type CameraDebug (line 344) | struct CameraDebug
FILE: src/robot/test/CameraPipeline_Test.cpp
type TestImageMetadata (line 43) | struct TestImageMetadata
FILE: src/robot/test/RobotController_Test.cpp
function moveToPositionHelper (line 10) | int moveToPositionHelper(RobotController& r, float x, float y, float a, ...
function coarsePositionTest (line 36) | void coarsePositionTest(float x, float y, float a)
function fakeMotionHelper (line 114) | void fakeMotionHelper(float x, float y, float a, int max_loops, StatusUp...
FILE: src/robot/test/RobotServer_Test.cpp
function testSimpleCommand (line 9) | void testSimpleCommand(RobotServer& r, std::string msg, std::string expe...
FILE: src/robot/test/SmoothTrajectoryGenerator_Test.cpp
type Catch (line 7) | namespace Catch {
type StringMaker<Point> (line 9) | struct StringMaker<Point>
method convert (line 11) | static std::string convert(Point const& p)
type StringMaker<Velocity> (line 17) | struct StringMaker<Velocity>
method convert (line 19) | static std::string convert(Velocity const& p)
FILE: src/robot/test/SocketWrapper_Test.cpp
function SimpleSocketSend (line 10) | void SimpleSocketSend(std::string data_to_send)
FILE: src/robot/test/test-main.cpp
function configure_logger (line 18) | void configure_logger()
function main (line 33) | int main( int argc, char* argv[] )
FILE: src/robot/test/test-utils.h
function MockSocketMultiThreadWrapper (line 12) | inline MockSocketMultiThreadWrapper* build_and_get_mock_socket()
function MockSerialComms (line 21) | inline MockSerialComms* build_and_get_mock_serial(const std::string& por...
function MockClockWrapper (line 30) | inline MockClockWrapper* get_mock_clock()
function MockClockWrapper (line 37) | inline MockClockWrapper* get_mock_clock_and_reset()
function reset_mock_clock (line 44) | inline void reset_mock_clock()
FILE: src/robot_motor_driver/SerialComms.cpp
function String (line 15) | String SerialComms::rcv()
FILE: src/robot_motor_driver/SerialComms.h
function class (line 9) | class SerialComms
FILE: src/tools/camera_calibration.py
function calibrate (line 11) | def calibrate(dirpath, prefix, image_format, square_size, width=9, heigh...
function save_coefficients (line 75) | def save_coefficients(mtx, dist, path):
function load_coefficients (line 83) | def load_coefficients(path):
FILE: src/tools/motor_test_script.py
class SerialClient (line 14) | class SerialClient:
method __init__ (line 16) | def __init__(self, port, baud, timeout):
method send (line 21) | def send(self, msg):
method recieve (line 26) | def recieve(self, timeout=0.1):
function power_on (line 61) | def power_on(s):
function power_off (line 66) | def power_off(s):
function send_vel (line 71) | def send_vel(s, vel):
function check_response (line 79) | def check_response(s):
function move_vel_with_pause (line 85) | def move_vel_with_pause(s, vel, move_time):
function timed_move (line 92) | def timed_move(s, vel, move_time, pause_time):
function signal_handler (line 104) | def signal_handler(sig, frame):
FILE: src/tools/plot_logs.py
function get_value (line 11) | def get_value(line, path):
function get_array (line 28) | def get_array(line):
class LogParser (line 35) | class LogParser:
method __init__ (line 37) | def __init__(self, path):
method parse_logs (line 53) | def parse_logs(self):
method plot_pos (line 151) | def plot_pos(self):
method plot_x_control (line 169) | def plot_x_control(self):
method plot_vel (line 196) | def plot_vel(self):
method plot_motors (line 214) | def plot_motors(self):
method plot_motor_counts (line 232) | def plot_motor_counts(self):
method plot_motor_info (line 244) | def plot_motor_info(self):
method plot_logs (line 264) | def plot_logs(self):
function get_all_log_files (line 271) | def get_all_log_files():
class PlottingGui (line 276) | class PlottingGui:
method __init__ (line 278) | def __init__(self):
method loop (line 297) | def loop(self):
Condensed preview — 159 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (645K chars).
[
{
"path": ".gitignore",
"chars": 131,
"preview": ".VSCodeCounter/\nbeacon_state.txt\nbuild/\nlib/\n.vscode/\nscratch\nlog/\n__pycache__\n*.so\nmarvelmind_SW*\n*previous_plan_state."
},
{
"path": "LICENSE",
"chars": 35149,
"preview": " GNU GENERAL PUBLIC LICENSE\n Version 3, 29 June 2007\n\n Copyright (C) 2007 Free "
},
{
"path": "README.md",
"chars": 1654,
"preview": "# Domino Robot\n\nThis is the code for Mark Rober's Domino Robot project. You can find a bunch more info about the project"
},
{
"path": "doc/DominoRobotControllerFlowchart.drawio",
"chars": 3019,
"preview": "<mxfile host=\"app.diagrams.net\" modified=\"2021-06-06T17:16:56.287Z\" agent=\"5.0 (Windows NT 10.0; Win64; x64) AppleWebKit"
},
{
"path": "doc/DominoRobotSoftwareArchitecture.drawio",
"chars": 3319,
"preview": "<mxfile host=\"app.diagrams.net\" modified=\"2021-06-24T01:50:07.329Z\" agent=\"5.0 (Windows NT 10.0; Win64; x64) AppleWebKit"
},
{
"path": "doc/KukaPsuedoCode.txt",
"chars": 679,
"preview": "KukaInit();\n\nint tile_count = 0;\nint num_tiles = 333;\nint num_rows = 20;\nint num_cols = 15;\n\nwhile (tile_count < num_til"
},
{
"path": "doc/NetworkPrototcols.txt",
"chars": 1695,
"preview": "\nMessage spec\n- All messages are sent as json encoded byte strings\n- All messages will contain a 'type' field that will "
},
{
"path": "doc/SubsystemDiagram.drawio",
"chars": 3058,
"preview": "<mxfile modified=\"2020-01-26T19:53:17.764Z\" host=\"www.draw.io\" agent=\"Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWeb"
},
{
"path": "doc/SwClassHeirarchy.txt",
"chars": 664,
"preview": "- robot\n\t- status updater\n\t\t- status\n\t- robot server\n\t\t- socket multi thread wrapper\n\t- robot controller\n\t\t- serial comm"
},
{
"path": "doc/clearcore_api.txt",
"chars": 1332,
"preview": "Tentative thoughts on api with clearcore for motor control over serial comms\n\nThere's a few options here:\n- Clearcore do"
},
{
"path": "experimental_testing/ImageParser.py",
"chars": 1897,
"preview": "import matplotlib.image as mpimg\nimport matplotlib.pyplot as plt\nimport skimage.transform as sktf\nimport numpy as np\n\n# "
},
{
"path": "experimental_testing/RobotArduinoTesting/StepperDriverMotion_4Motor_Accel/StepperDriverMotion_4Motor_Accel.ino",
"chars": 2488,
"preview": "#include <StepperDriver.h>\n// https://github.com/DIMRobotics/ArduinoStepperDriver\n\n/* Axis identifiers inside the librar"
},
{
"path": "experimental_testing/RobotArduinoTesting/TestMotor/TestMotor.ino",
"chars": 857,
"preview": "#define PIN_ENABLE 52\n#define PIN_BL_PWM 6\n#define PIN_BL_DIR 48\n#define FW 1\n#define BW 0\n#define SPEED_SLOW 50\n#define"
},
{
"path": "experimental_testing/RobotArduinoTesting/WifiServerWithESP/RobotServer.cpp",
"chars": 4841,
"preview": "#include <Arduino.h> // This has to be before ArduinoJson.h to fix compiler issues\n#include \"RobotServer.h\"\n#include <Ar"
},
{
"path": "experimental_testing/RobotArduinoTesting/WifiServerWithESP/RobotServer.h",
"chars": 909,
"preview": "/*\n* Robot server that handles gettings messages from master\n* and responding correctly\n*/\n\n#ifndef RobotServer_h\n#defin"
},
{
"path": "experimental_testing/RobotArduinoTesting/WifiServerWithESP/WifiServerWithESP.ino",
"chars": 362,
"preview": "#include \"RobotServer.h\"\n\n\nRobotServer server = RobotServer(Serial3, Serial);\n\n\nvoid setup() {\n // put your setup code "
},
{
"path": "experimental_testing/RobotArduinoTesting/WifiTesting/WifiTesting.ino",
"chars": 956,
"preview": "\n\n// Arduino pin 15 (RX3) to ESP8266 TX\n// Arduino pin 14 (TX3) to voltage divider then to ESP8266 RX\n// Connect GND "
},
{
"path": "experimental_testing/RobotArduinoTesting/all_motors/all_motors.ino",
"chars": 1753,
"preview": "// Pinouts\n#define PIN_ENABLE 52\n#define PIN_FR_PWM 3\n#define PIN_FR_DIR 49\n#define PIN_FL_PWM 2\n#define PIN_FL_DIR 51\n#"
},
{
"path": "experimental_testing/RobotArduinoTesting/all_motors_speed_control/Motor.cpp",
"chars": 1750,
"preview": "#include \"Motor.h\"\n#include <math.h>\n\nMotor::Motor(int pwmPin, int dirPin, int encPinA, int encPinB, double Kp, double K"
},
{
"path": "experimental_testing/RobotArduinoTesting/all_motors_speed_control/Motor.h",
"chars": 1967,
"preview": "/* Requires libraries:\n * Filters: https://github.com/JonHub/Filters\n * PID: https://playground.arduino.cc/Code/PIDLib"
},
{
"path": "experimental_testing/RobotArduinoTesting/all_motors_speed_control/all_motors_speed_control.ino",
"chars": 3242,
"preview": "/* Requires libraries:\n * Filters: https://github.com/JonHub/Filters\n * PID: https://playground.arduino.cc/Code/PIDLib"
},
{
"path": "experimental_testing/RobotArduinoTesting/kalman_test/KalmanFilter.cpp",
"chars": 863,
"preview": "#include \"KalmanFilter.h\"\n\nKalmanFilter::KalmanFilter(\n double dt,\n const mat& A,\n const mat& B,\n const mat&"
},
{
"path": "experimental_testing/RobotArduinoTesting/kalman_test/KalmanFilter.h",
"chars": 1564,
"preview": "// Loosely based on kalman-cpp: https://github.com/hmartiro/kalman-cpp\n\n#ifndef KalmanFilter_h\n#define KalmanFilter_h\n\n#"
},
{
"path": "experimental_testing/RobotArduinoTesting/kalman_test/kalman_test.ino",
"chars": 688,
"preview": "#include \"KalmanFilter.h\"\n#include \"LinearAlgebra.h\"\n\n\nmat A = mat::identity(6);\nmat B = mat::zeros(6,3);\nmat C = mat::z"
},
{
"path": "experimental_testing/RobotArduinoTesting/lifter_nano_test/lifter_nano_test.ino",
"chars": 952,
"preview": "\n#define LIFTER_PIN_0 1\n#define LIFTER_PIN_1 2\n#define LIFTER_PIN_2 3\n#define FEEDBACK_PIN 4\n\n\nvoid setup ()\n{\n Seria"
},
{
"path": "experimental_testing/RobotArduinoTesting/motor_driver_test_script/motor_driver_test_script.ino",
"chars": 2721,
"preview": "#include \"ClearCore.h\"\n\n\n#define LIFTER_MOTOR ConnectorM3\n#define INCREMENTAL_UP_PIN DI7\n#define INCREMENTAL_DOWN_PIN DI"
},
{
"path": "experimental_testing/RobotArduinoTesting/new_dc_motor_tuning/Motor.cpp",
"chars": 3343,
"preview": "#include \"Motor.h\"\n#include \"constants.h\"\n\nconstexpr double COUNTS_TO_RADS = 2.0 * PI / static_cast<double>(COUNTS_PER_O"
},
{
"path": "experimental_testing/RobotArduinoTesting/new_dc_motor_tuning/Motor.h",
"chars": 1940,
"preview": "#ifndef Motor_h\n#define Motor_h\n\n#include <Encoder.h>\n#include <PID_v1.h>\n#include <Filters.h>\n\nclass Motor\n{\n public:\n"
},
{
"path": "experimental_testing/RobotArduinoTesting/new_dc_motor_tuning/constants.h",
"chars": 3724,
"preview": "#ifndef Constants_h\n#define Constants_h\n\n// Pins\n#define PIN_PWM_0 3 \n#define PIN_PWM_1 4\n#define PIN_PWM_2 5\n#define "
},
{
"path": "experimental_testing/RobotArduinoTesting/new_dc_motor_tuning/new_dc_motor_tuning.ino",
"chars": 2760,
"preview": "#include \"constants.h\"\n#include \"Motor.h\"\n\ndouble Kp = 14;\ndouble Ki = 0.1;\ndouble Kd = 0.08;\nMotor m = Motor(PIN_PWM_2,"
},
{
"path": "experimental_testing/RobotArduinoTesting/single_motor_speed_control/Motor.cpp",
"chars": 2004,
"preview": "#include \"Motor.h\"\n#include <math.h>\n\nMotor::Motor(int pwmPin, int dirPin, int encPinA, int encPinB, double Kp, double K"
},
{
"path": "experimental_testing/RobotArduinoTesting/single_motor_speed_control/Motor.h",
"chars": 2153,
"preview": "/* Requires libraries:\n * Filters: https://github.com/JonHub/Filters\n * PID: https://playground.arduino.cc/Code/PIDLib"
},
{
"path": "experimental_testing/RobotArduinoTesting/single_motor_speed_control/single_motor_speed_control.ino",
"chars": 1798,
"preview": "/* Requires libraries:\n * Filters: https://github.com/JonHub/Filters\n * PID: https://playground.arduino.cc/Code/PIDLib"
},
{
"path": "experimental_testing/RobotArduinoTesting/stepper_nano_test/stepper_nano_test.ino",
"chars": 1113,
"preview": "// Uses stepper driver library from here: https://www.airspayce.com/mikem/arduino/AccelStepper/classAccelStepper.html\n#i"
},
{
"path": "experimental_testing/RobotArduinoTesting/test_encoders/test_encoders.ino",
"chars": 974,
"preview": "// Pins\n#define PIN_ENCA_1 21\n#define PIN_ENCA_2 20\n#define PIN_ENCA_3 19\n#define PIN_ENCA_4 18\n\n#define PIN_ENCB_1 25\n#"
},
{
"path": "experimental_testing/TrajGen.py",
"chars": 2764,
"preview": "# Testing out trajectory generation where I have a gui\n\nimport math\nimport numpy as np\nfrom matplotlib import pyplot as "
},
{
"path": "experimental_testing/TrajGenv2.py",
"chars": 9885,
"preview": "import math\nimport numpy as np\nfrom matplotlib import pyplot as plt\n\np_target = 20.0 # m\nv_max = 1.0 #m/s\na_max = 2.0 "
},
{
"path": "src/master/FieldPlanner.py",
"chars": 42269,
"preview": "import matplotlib.image as mpimg\nimport matplotlib.pyplot as plt\nimport skimage.transform as sktf\nimport numpy as np\nimp"
},
{
"path": "src/master/MarvelMindHandler.py",
"chars": 4534,
"preview": "\"\"\"\nPython wrapper for Marvelmind C API\n\"\"\"\n\nimport ctypes\nimport logging\nfrom collections import defaultdict\nfrom Utils"
},
{
"path": "src/master/MasterMain.py",
"chars": 36441,
"preview": "import time\nimport math\nimport copy\nimport config\nimport os\nimport sys\nimport logging\nimport PySimpleGUI as sg\nimport tr"
},
{
"path": "src/master/README.md",
"chars": 10974,
"preview": "# **Warning! This is probably out of date and might be inaccurate. Use at your own risk.**\n\n# Master\nThis is the top lev"
},
{
"path": "src/master/RobotClient.py",
"chars": 12535,
"preview": "# This is a basic TCP client for a robot that wraps up sending\n# and recieving various commands\n\nimport socket\nimport se"
},
{
"path": "src/master/Runtime.py",
"chars": 29017,
"preview": "import time\nimport logging\nimport enum\nimport pprint\nimport pickle\nimport os\nimport json\nimport copy\nimport math\n\nfrom n"
},
{
"path": "src/master/Utils.py",
"chars": 2575,
"preview": "import time\nimport numpy as np\nimport math\nimport enum\n\nclass ActionTypes(enum.Enum):\n MOVE_COARSE = 0,\n MOVE_FINE"
},
{
"path": "src/master/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "src/master/camera_utils.py",
"chars": 2881,
"preview": "# Helper script to get camera images and debug info from raspi\n\nimport paramiko\nfrom scp import SCPClient\nimport os\nimpo"
},
{
"path": "src/master/config.py",
"chars": 10811,
"preview": "import numpy as np\nimport os\nimport Utils\n\nclass Config:\n\n # Various debug/test flags\n # Set to override config va"
},
{
"path": "src/master/plans/vision_offsets small_testing_area.csv",
"chars": 169,
"preview": "#tile_x, tile_y, offset_x (millimeters), offset_y(millimeters), offset_a(degrees), add to default (True) or exact (False"
},
{
"path": "src/master/plans/vision_offsets_full_plan.csv",
"chars": 93,
"preview": "#tile_x, tile_y, offset_x (millimeters), offset_y(millimeters), offset_a(degrees)\nX,17,0,-4,0"
},
{
"path": "src/master/plot_logs2.py",
"chars": 3212,
"preview": "import paramiko\nfrom scp import SCPClient\nimport os\nimport matplotlib.pyplot as plt\nimport config\nimport csv\nimport nump"
},
{
"path": "src/master/requirements.txt",
"chars": 488,
"preview": "cycler==0.10.0\r\ndecorator==4.4.2\r\nimageio==2.9.0\r\nkiwisolver==1.3.1 #Was originally 1.2.0, but wasn't building with Pyt"
},
{
"path": "src/robot/Makefile",
"chars": 1582,
"preview": "# Started with https://spin.atomicobject.com/2016/08/26/makefile-c-projects/\n# Additional inspiration from https://coder"
},
{
"path": "src/robot/README.md",
"chars": 1208,
"preview": "# **Warning! This is probably out of date and might be inaccurate. Use at your own risk.**\n\n## Libraries needed for inst"
},
{
"path": "src/robot/src/KalmanFilter.cpp",
"chars": 1215,
"preview": "#include \"KalmanFilter.h\"\r\n\r\nKalmanFilter::KalmanFilter(\r\n Eigen::MatrixXf A,\r\n Eigen::MatrixXf B,\r\n Eigen::Mat"
},
{
"path": "src/robot/src/KalmanFilter.h",
"chars": 1365,
"preview": "/**\r\n* Kalman filter loosely based on\r\n* https://github.com/hmartiro/kalman-cpp\r\n* and\r\n* https://github.com/tysik/kalma"
},
{
"path": "src/robot/src/Localization.cpp",
"chars": 7466,
"preview": "#include \"Localization.h\"\r\n\r\n#include <math.h>\r\n#include <algorithm> \r\n#include <plog/Log.h>\r\n#include \"constants.h\"\r\n\r\n"
},
{
"path": "src/robot/src/Localization.h",
"chars": 1697,
"preview": "#ifndef Localization_h\r\n#define Localization_h\r\n\r\n#include \"utils.h\"\r\n#include <Eigen/Dense>\r\n#include \"KalmanFilter.h\"\r"
},
{
"path": "src/robot/src/MarvelmindWrapper.cpp",
"chars": 5763,
"preview": "#include \"MarvelmindWrapper.h\"\n#include \"constants.h\"\n#include \"plog/Log.h\"\n#include <filesystem>\n\n\nMarvelmindWrapper::M"
},
{
"path": "src/robot/src/MarvelmindWrapper.h",
"chars": 328,
"preview": "#ifndef MarvelmindWrapper_h\n#define MarvelmindWrapper_h\n\n#include <vector>\n\nextern \"C\" {\n #include <marvelmind/marvel"
},
{
"path": "src/robot/src/RobotController.cpp",
"chars": 14159,
"preview": "#include \"RobotController.h\"\n\n#include <math.h>\n#include <plog/Log.h>\n#include <Eigen/Dense>\n\n#include \"constants.h\"\n#in"
},
{
"path": "src/robot/src/RobotController.h",
"chars": 3976,
"preview": "#ifndef RobotController_h\n#define RobotController_h\n\n#include \"SmoothTrajectoryGenerator.h\"\n#include \"StatusUpdater.h\"\n#"
},
{
"path": "src/robot/src/RobotServer.cpp",
"chars": 8365,
"preview": "#include \"RobotServer.h\"\n\n#include <ArduinoJson/ArduinoJson.h>\n#include <plog/Log.h>\n\n#include \"sockets/SocketMultiThrea"
},
{
"path": "src/robot/src/RobotServer.h",
"chars": 1364,
"preview": "/*\n* Robot server that handles gettings messages from master\n* and responding correctly\n*/\n\n#ifndef RobotServer_h\n#defin"
},
{
"path": "src/robot/src/SmoothTrajectoryGenerator.cpp",
"chars": 17610,
"preview": "#include \"SmoothTrajectoryGenerator.h\"\n#include <plog/Log.h>\n#include \"constants.h\"\n#include \"utils.h\"\n\nconstexpr float "
},
{
"path": "src/robot/src/SmoothTrajectoryGenerator.h",
"chars": 5348,
"preview": "#ifndef SmoothTrajectoryGenerator_h\n#define SmoothTrajectoryGenerator_h\n\n#include <Eigen/Dense>\n#include \"utils.h\"\n\n\n// "
},
{
"path": "src/robot/src/StatusUpdater.cpp",
"chars": 1728,
"preview": "#include \"StatusUpdater.h\"\n#include \"constants.h\"\n\nStatusUpdater::StatusUpdater() :\n currentStatus_()\n{\n}\n\nstd::string "
},
{
"path": "src/robot/src/StatusUpdater.h",
"chars": 5267,
"preview": "\n#ifndef StatusUpdater_h\n#define StatusUpdater_h\n\n#include <ArduinoJson/ArduinoJson.h>\n#include \"utils.h\"\n\nclass StatusU"
},
{
"path": "src/robot/src/TrayController.cpp",
"chars": 7541,
"preview": "#include \"TrayController.h\"\n#include \"constants.h\"\n#include <plog/Log.h>\n#include \"serial/SerialCommsFactory.h\"\n\nenum cl"
},
{
"path": "src/robot/src/TrayController.h",
"chars": 1188,
"preview": "#ifndef TrayController_h\n#define TrayController_h\n\n#include \"serial/SerialComms.h\"\n#include \"utils.h\"\n\nclass TrayControl"
},
{
"path": "src/robot/src/camera_tracker/CameraPipeline.cpp",
"chars": 15409,
"preview": "#include \"CameraPipeline.h\"\n\n#include <plog/Log.h>\n#include \"constants.h\"\n#include <mutex>\n\nstd::mutex data_mutex;\n\ncv::"
},
{
"path": "src/robot/src/camera_tracker/CameraPipeline.h",
"chars": 1778,
"preview": "#ifndef CameraPipeline_h\n#define CameraPipeline_h\n\n#include \"utils.h\"\n#include <opencv2/opencv.hpp>\n#include <Eigen/Dens"
},
{
"path": "src/robot/src/camera_tracker/CameraTracker.cpp",
"chars": 4491,
"preview": "#include \"CameraTracker.h\"\r\n\r\n#include <plog/Log.h>\r\n#include \"constants.h\"\r\n\r\nconstexpr int num_samples_to_average = 10"
},
{
"path": "src/robot/src/camera_tracker/CameraTracker.h",
"chars": 1398,
"preview": "#ifndef CameraTracker_h\r\n#define CameraTracker_h\r\n\r\n#include \"CameraTrackerBase.h\"\r\n#include \"utils.h\"\r\n#include <Eigen/"
},
{
"path": "src/robot/src/camera_tracker/CameraTrackerBase.h",
"chars": 569,
"preview": "#ifndef CameraTrackerBase_h\r\n#define CameraTrackerBase_h\r\n\r\n#include \"utils.h\"\r\n\r\nstruct CameraTrackerOutput\r\n{\r\n Point"
},
{
"path": "src/robot/src/camera_tracker/CameraTrackerFactory.cpp",
"chars": 1213,
"preview": "#include \"CameraTrackerFactory.h\"\r\n#include \"CameraTracker.h\"\r\n#include \"CameraTrackerMock.h\"\r\n\r\n#include <plog/Log.h> \r"
},
{
"path": "src/robot/src/camera_tracker/CameraTrackerFactory.h",
"chars": 940,
"preview": "#ifndef CameraTrackerFactory_h\r\n#define CameraTrackerFactory_h\r\n\r\n#include <memory>\r\n#include <map>\r\n\r\n#include \"CameraT"
},
{
"path": "src/robot/src/camera_tracker/CameraTrackerMock.h",
"chars": 654,
"preview": "#ifndef CameraTrackerMock_h\r\n#define CameraTrackerMock_h\r\n\r\n#include \"utils.h\"\r\n\r\nclass CameraTrackerMock : public Camer"
},
{
"path": "src/robot/src/constants.cfg",
"chars": 8799,
"preview": "name = \"Runtime constants\";\nlog_level = \"info\"; // verbose, debug, info, warning, error, fatal, none\n\nmotion = \n{\n li"
},
{
"path": "src/robot/src/constants.h",
"chars": 1184,
"preview": "#ifndef Constants_h\n#define Constants_h\n\n#include <libconfig.h++>\nextern libconfig::Config cfg;\n\n#define CONSTANTS_FILE "
},
{
"path": "src/robot/src/main.cpp",
"chars": 3758,
"preview": "#include <plog/Log.h> \n#include <plog/Init.h>\n#include <plog/Formatters/TxtFormatter.h>\n#include <plog/Formatters/Messag"
},
{
"path": "src/robot/src/robot.cpp",
"chars": 9764,
"preview": "#include \"robot.h\"\n\n#include <plog/Log.h> \n#include \"utils.h\"\n#include \"camera_tracker/CameraTrackerFactory.h\"\n\n\nWaitFor"
},
{
"path": "src/robot/src/robot.h",
"chars": 1643,
"preview": "#ifndef Robot_h\n#define Robot_h\n\n#include \"MarvelmindWrapper.h\"\n#include \"RobotController.h\"\n#include \"RobotServer.h\"\n#i"
},
{
"path": "src/robot/src/robot_controller_modes/RobotControllerModeBase.cpp",
"chars": 350,
"preview": "#include \"RobotControllerModeBase.h\"\r\n\r\nRobotControllerModeBase::RobotControllerModeBase(bool fake_perfect_motion)\r\n: mo"
},
{
"path": "src/robot/src/robot_controller_modes/RobotControllerModeBase.h",
"chars": 824,
"preview": "#ifndef RobotControllerModeBase_h\r\n#define RobotControllerModeBase_h\r\n\r\n#include \"SmoothTrajectoryGenerator.h\"\r\n#include"
},
{
"path": "src/robot/src/robot_controller_modes/RobotControllerModePosition.cpp",
"chars": 5483,
"preview": "#include \"RobotControllerModePosition.h\"\r\n#include \"constants.h\"\r\n#include <plog/Log.h>\r\n\r\nRobotControllerModePosition::"
},
{
"path": "src/robot/src/robot_controller_modes/RobotControllerModePosition.h",
"chars": 1059,
"preview": "#ifndef RobotControllerModePosition_h\r\n#define RobotControllerModePosition_h\r\n\r\n#include \"RobotControllerModeBase.h\"\r\n#i"
},
{
"path": "src/robot/src/robot_controller_modes/RobotControllerModeStopFast.cpp",
"chars": 5730,
"preview": "#include \"RobotControllerModeStopFast.h\"\n#include \"constants.h\"\n#include <plog/Log.h>\n\nRobotControllerModeStopFast::Robo"
},
{
"path": "src/robot/src/robot_controller_modes/RobotControllerModeStopFast.h",
"chars": 979,
"preview": "#ifndef RobotControllerModeStopFast_h\n#define RobotControllerModeStopFast_h\n\n#include \"RobotControllerModeBase.h\"\n#inclu"
},
{
"path": "src/robot/src/robot_controller_modes/RobotControllerModeVision.cpp",
"chars": 8134,
"preview": "#include \"RobotControllerModeVision.h\"\r\n#include \"constants.h\"\r\n#include <plog/Log.h>\r\n#include \"camera_tracker/CameraTr"
},
{
"path": "src/robot/src/robot_controller_modes/RobotControllerModeVision.h",
"chars": 1247,
"preview": "#ifndef RobotControllerModeVision_h\r\n#define RobotControllerModeVision_h\r\n\r\n#include \"RobotControllerModeBase.h\"\r\n#inclu"
},
{
"path": "src/robot/src/serial/MockSerialComms.cpp",
"chars": 3054,
"preview": "#include \"MockSerialComms.h\"\n\n#include <plog/Log.h> \n#include <iostream>\n\n\nMockSerialComms::MockSerialComms(std::string "
},
{
"path": "src/robot/src/serial/MockSerialComms.h",
"chars": 943,
"preview": "#ifndef MockSerialComms_h\n#define MockSerialComms_h\n\n#include <string>\n#include <queue>\n\n#include \"SerialCommsBase.h\"\n\nc"
},
{
"path": "src/robot/src/serial/SerialComms.cpp",
"chars": 3134,
"preview": "#include \"SerialComms.h\"\n\n#include <plog/Log.h>\n\nSerialComms::SerialComms(std::string portName)\n: SerialCommsBase(),\n s"
},
{
"path": "src/robot/src/serial/SerialComms.h",
"chars": 809,
"preview": "#ifndef SerialComms_h\n#define SerialComms_h\n\n#include <libserial/SerialPort.h>\n#include <queue>\n\n#include \"SerialCommsBa"
},
{
"path": "src/robot/src/serial/SerialCommsBase.cpp",
"chars": 433,
"preview": "#include \"SerialCommsBase.h\"\n#include <plog/Log.h>\n\nSerialCommsBase::SerialCommsBase() : connected_(false) {}\n\nSerialCom"
},
{
"path": "src/robot/src/serial/SerialCommsBase.h",
"chars": 457,
"preview": "#ifndef SerialCommsBase_h\n#define SerialCommsBase_h\n\n#include <string>\n\n#define START_CHAR '<'\n#define END_CHAR '>'\n\ncla"
},
{
"path": "src/robot/src/serial/SerialCommsFactory.cpp",
"chars": 1551,
"preview": "#include \"SerialCommsFactory.h\"\n#include \"SerialComms.h\"\n#include \"MockSerialComms.h\"\n\n#include <plog/Log.h> \n\nSerialCom"
},
{
"path": "src/robot/src/serial/SerialCommsFactory.h",
"chars": 878,
"preview": "#ifndef SerialCommsFactory_h\n#define SerialCommsFactory_h\n\n#include <memory>\n#include <map>\n\n#include \"SerialCommsBase.h"
},
{
"path": "src/robot/src/sockets/ClientSocket.cpp",
"chars": 789,
"preview": "// Implementation of the ClientSocket class\n\n#include \"ClientSocket.h\"\n#include \"SocketException.h\"\n\n\nClientSocket::Clie"
},
{
"path": "src/robot/src/sockets/ClientSocket.h",
"chars": 377,
"preview": "// Definition of the ClientSocket class\n\n#ifndef ClientSocket_class\n#define ClientSocket_class\n\n#include \"Socket.h\"\n\n\ncl"
},
{
"path": "src/robot/src/sockets/MockSocketMultiThreadWrapper.cpp",
"chars": 1703,
"preview": "#include \"MockSocketMultiThreadWrapper.h\"\n\n#include <plog/Log.h>\n\nMockSocketMultiThreadWrapper::MockSocketMultiThreadWra"
},
{
"path": "src/robot/src/sockets/MockSocketMultiThreadWrapper.h",
"chars": 798,
"preview": "#ifndef MockSocketMultiThreadWrapper_h\n#define MockSocketMultiThreadWrapper_h\n\n#include <string>\n#include <queue>\n\n#incl"
},
{
"path": "src/robot/src/sockets/README.md",
"chars": 536,
"preview": "The low level socket implimentation in this folder comes from Rob Tougher and is published here at https://tldp.org/LDP/"
},
{
"path": "src/robot/src/sockets/ServerSocket.cpp",
"chars": 1323,
"preview": "// Implementation of the ServerSocket class\n\n#include \"ServerSocket.h\"\n#include \"SocketException.h\"\n#include \"SocketTime"
},
{
"path": "src/robot/src/sockets/ServerSocket.h",
"chars": 493,
"preview": "// From https://tldp.org/LDP/LG/issue74/tougher.html\n\n// Definition of the ServerSocket class\n\n#ifndef ServerSocket_clas"
},
{
"path": "src/robot/src/sockets/Socket.cpp",
"chars": 2939,
"preview": "// Implementation of the Socket class.\n\n\n#include \"Socket.h\"\n#include \"string.h\"\n#include <string.h>\n#include <errno.h>\n"
},
{
"path": "src/robot/src/sockets/Socket.h",
"chars": 848,
"preview": "// Definition of the Socket class\n\n#ifndef Socket_class\n#define Socket_class\n\n\n#include <sys/types.h>\n#include <sys/sock"
},
{
"path": "src/robot/src/sockets/SocketException.h",
"chars": 304,
"preview": "// SocketException class\n\n\n#ifndef SocketException_class\n#define SocketException_class\n\n#include <string>\n\nclass SocketE"
},
{
"path": "src/robot/src/sockets/SocketMultiThreadWrapper.cpp",
"chars": 3624,
"preview": "#include \"SocketMultiThreadWrapper.h\"\n\n#include <plog/Log.h>\n#include \"sockets/ServerSocket.h\"\n#include \"sockets/SocketE"
},
{
"path": "src/robot/src/sockets/SocketMultiThreadWrapper.h",
"chars": 536,
"preview": "#ifndef SocketMultiThreadWrapper_h\n#define SocketMultiThreadWrapper_h\n\n#include <queue>\n#include <thread>\n#include \"Sock"
},
{
"path": "src/robot/src/sockets/SocketMultiThreadWrapperBase.h",
"chars": 337,
"preview": "#ifndef SocketMultiThreadWrapperBase_h\n#define SocketMultiThreadWrapperBase_h\n\n#include <string>\n\nclass SocketMultiThrea"
},
{
"path": "src/robot/src/sockets/SocketMultiThreadWrapperFactory.cpp",
"chars": 1791,
"preview": "#include \"SocketMultiThreadWrapperFactory.h\"\n#include \"SocketMultiThreadWrapper.h\"\n#include \"MockSocketMultiThreadWrappe"
},
{
"path": "src/robot/src/sockets/SocketMultiThreadWrapperFactory.h",
"chars": 1001,
"preview": "#ifndef SocketMultiThreadWrapperFactory_h\n#define SocketMultiThreadWrapperFactory_h\n\n#include <memory>\n\n#include \"Socket"
},
{
"path": "src/robot/src/sockets/SocketTimeoutException.h",
"chars": 346,
"preview": "// SocketTimeoutException class\n\n\n#ifndef SocketTimeoutException_class\n#define SocketTimeoutException_class\n\n#include <s"
},
{
"path": "src/robot/src/utils.cpp",
"chars": 6882,
"preview": "#define _USE_MATH_DEFINES\n \n#include \"utils.h\"\n\n#include <cmath>\n#include <chrono>\n#include <plog/Log.h>\n#include <iostr"
},
{
"path": "src/robot/src/utils.h",
"chars": 7943,
"preview": "#ifndef utils_h\n#define utils_h\n\n#include <chrono>\n#include <vector>\n#include <memory>\n#include <math.h>\n#include <plog/"
},
{
"path": "src/robot/test/CameraPipeline_Test.cpp",
"chars": 4576,
"preview": "#include <Catch/catch.hpp>\n\n#include \"camera_tracker/CameraPipeline.h\"\n#include \"test-utils.h\"\n\n\nTEST_CASE(\"Robot coords"
},
{
"path": "src/robot/test/CameraTracker_Test.cpp",
"chars": 4052,
"preview": "#include <Catch/catch.hpp>\r\n\r\n#include \"camera_tracker/CameraTracker.h\"\r\n#include \"test-utils.h\"\r\n#include <unistd.h>\r\n\r"
},
{
"path": "src/robot/test/Localization_Test.cpp",
"chars": 12286,
"preview": "// #include <Catch/catch.hpp>\r\n\r\n// #include <random>\r\n\r\n// #include \"Localization.h\"\r\n// #include \"StatusUpdater.h\"\r\n//"
},
{
"path": "src/robot/test/RobotController_Test.cpp",
"chars": 6692,
"preview": "#include <Catch/catch.hpp>\n\n#include \"RobotController.h\"\n#include \"StatusUpdater.h\"\n#include \"test-utils.h\"\n\n\n// Helper "
},
{
"path": "src/robot/test/RobotServer_Test.cpp",
"chars": 4850,
"preview": "#include <Catch/catch.hpp>\n\n#include \"RobotServer.h\"\n#include \"StatusUpdater.h\"\n#include \"test-utils.h\"\n#include \"socket"
},
{
"path": "src/robot/test/Robot_Test.cpp",
"chars": 1009,
"preview": "#include <Catch/catch.hpp>\n\n#include \"robot.h\"\n\n#include \"test-utils.h\"\n\nTEST_CASE(\"Robot move\", \"[Robot]\")\n{\n std::s"
},
{
"path": "src/robot/test/SanityCheckTest.cpp",
"chars": 433,
"preview": "#include <Catch/catch.hpp>\n\n#include \"constants.h\"\n\n// TEST_CASE(\"Mock socket config disabled\", \"[SanityCheck]\")\n// {\n//"
},
{
"path": "src/robot/test/SmoothTrajectoryGenerator_Test.cpp",
"chars": 25402,
"preview": "#include <Catch/catch.hpp>\n\n#include \"SmoothTrajectoryGenerator.h\"\n#include \"constants.h\"\n\n// This stuff is needed to co"
},
{
"path": "src/robot/test/SocketWrapper_Test.cpp",
"chars": 1225,
"preview": "#include <Catch/catch.hpp>\n#include <unistd.h>\n\n#include \"sockets/ClientSocket.h\"\n#include \"sockets/SocketException.h\"\n#"
},
{
"path": "src/robot/test/StatusUpdater_Test.cpp",
"chars": 2096,
"preview": "#include <Catch/catch.hpp>\n\n#include \"StatusUpdater.h\"\n\nusing Catch::Matchers::StartsWith;\nusing Catch::Matchers::EndsWi"
},
{
"path": "src/robot/test/TrayController_Test.cpp",
"chars": 5248,
"preview": "#include <Catch/catch.hpp>\n\n#include \"TrayController.h\"\n#include \"test-utils.h\"\n\nTEST_CASE(\"Send and waiting\", \"[TrayCon"
},
{
"path": "src/robot/test/test-main.cpp",
"chars": 1636,
"preview": "#define CATCH_CONFIG_RUNNER\n#include <Catch/catch.hpp>\n#include \"constants.h\"\n#include \"serial/SerialCommsFactory.h\"\n#in"
},
{
"path": "src/robot/test/test-utils.h",
"chars": 2006,
"preview": "#ifndef testutils_h\n#define testutils_h\n\n#include \"serial/MockSerialComms.h\"\n#include \"serial/SerialCommsFactory.h\"\n#inc"
},
{
"path": "src/robot/test/test_constants.cfg",
"chars": 8454,
"preview": "name = \"Test constants\";\nlog_level = \"info\"; // verbose, debug, info, warning, error, fatal, none\n\nmotion = \n{\n limit"
},
{
"path": "src/robot/test/utils_Test.cpp",
"chars": 15412,
"preview": "#include <Catch/catch.hpp>\n#include <unistd.h>\n\n#include \"utils.h\"\n#include \"test-utils.h\"\n\nTEST_CASE(\"Sign util test\", "
},
{
"path": "src/robot_motor_driver/SerialComms.cpp",
"chars": 1075,
"preview": "#include \"SerialComms.h\"\n\nSerialComms::SerialComms(HardwareSerial& serial)\n: serial_(serial),\n recvInProgress_(false),\n"
},
{
"path": "src/robot_motor_driver/SerialComms.h",
"chars": 410,
"preview": "#ifndef SerialComms_h\n#define SerialComms_h\n\n#include <HardwareSerial.h>\n\n#define START_CHAR '<'\n#define END_CHAR '>'\n\nc"
},
{
"path": "src/robot_motor_driver/robot_motor_driver.ino",
"chars": 16789,
"preview": "#include \"ClearCore.h\"\n#include \"SerialComms.h\"\n#include <math.h>\n\n// From https://teknic-inc.github.io/ClearCore-librar"
},
{
"path": "src/tools/80-domino-bot.rules",
"chars": 358,
"preview": "SUBSYSTEM==\"tty\", ATTRS{idVendor}==\"2890\", ATTRS{idProduct}==\"8022\", SYMLINK+=\"clearcore\"\nSUBSYSTEM==\"tty\", ATTRS{idVend"
},
{
"path": "src/tools/IR_calibration_1.yml",
"chars": 398,
"preview": "%YAML:1.0\n---\nK: !!opencv-matrix\n rows: 3\n cols: 3\n dt: d\n data: [ 1.2117719731192783e+03, 0., 6.456986793316388"
},
{
"path": "src/tools/IR_calibration_2.yml",
"chars": 397,
"preview": "%YAML:1.0\n---\nK: !!opencv-matrix\n rows: 3\n cols: 3\n dt: d\n data: [ 1.2054254220822804e+03, 0., 6.733098384879414"
},
{
"path": "src/tools/camera_calibration.py",
"chars": 4546,
"preview": "import numpy as np\nimport cv2\nimport glob\nimport argparse\nimport os\n\n# termination criteria\ncriteria = (cv2.TERM_CRITERI"
},
{
"path": "src/tools/motor_test_script.py",
"chars": 3935,
"preview": "import serial\nimport time\nimport signal\nimport sys\n\nSERIAL_PORT = 'COM7'\nSERIAL_BAUD = 115200\nSERIAL_TIMEOUT = 1\n\nSTART_"
},
{
"path": "src/tools/plot_logs.py",
"chars": 13779,
"preview": "import argparse\nimport os\nimport numpy as np\nimport json\nimport matplotlib.pyplot as plt\nimport PySimpleGUI as sg\n\nDEFAU"
}
]
// ... and 19 more files (download for full content)
About this extraction
This page contains the full source code of the alexbaucom17/DominoRobot GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 159 files (596.1 KB), approximately 161.5k tokens, and a symbol index with 411 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.