ee175ab final report humanoid robot -...

SCATY

Dept. of Electrical Engineering, UCR

EE175AB Final Report: Humanoid Robot

March 14, 2016 & Version 1.0

1 of 110

EE175AB Final Report

Humanoid Robot

EE 175AB Final Report

Department of Electrical Engineering, UC Riverside

Project Team

Member(s)

Stanley Chang, Alberto Tam Yong

Date Submitted March 14, 2016

Section

Professor

Dr. Roman Chomko

Revision Revision 1.0

URL of Project

Wiki/Webpage

http://stanleyand.albertotech.com

Permanent Emails

of all team mem-

bers

[email protected]

[email protected]

Summary

This report presents the design and development of a low-cost humanoid

robot. The content within this document gives specific details regarding the

components of the product.

SCATY




2 of 110

Revisions

Version Description of Version Author(s) Date

Completed

Approval

0.1 First draft – not ready to be released Stanley Chang,

Alberto Tam Yong

02/25/2016 Chomko

0.2 Needs formatting Stanley Chang,

Alberto Tam Yong

3/14/2016

1.0 Final Version Stanley Chang,

Alberto Tam Yong

03/14/2016

SCATY




3 of 110

Table of Contents

REVISIONS .....................................................................................................................2

TABLE OF CONTENTS ....................................................................................................3

1 EXECUTIVE SUMMARY ...............................................................................................7

2 INTRODUCTION ...........................................................................................................8

2.1 DESIGN OBJECTIVES AND SYSTEM OVERVIEW ..........................................................8

2.2 BACKGROUNDS AND PRIOR ART ...............................................................................8 2.3 DEVELOPMENT ENVIRONMENT AND TOOLS ..............................................................9 2.4 RELATED DOCUMENTS AND SUPPORTING MATERIALS............................................10 2.5 DEFINITIONS AND ACRONYMS ................................................................................10

3 DESIGN CONSIDERATIONS ........................................................................................12

3.1 ASSUMPTIONS .........................................................................................................12 3.2 REALISTIC CONSTRAINTS ........................................................................................12

3.2.1 Weight/size Constraints ............................................................................................ 12 3.2.2 Technical and Skill Constraints ................................................................................ 12 3.2.3 Project Time and Budget Constraints ...................................................................... 12 3.2.4 End Product Price Constraints ................................................................................ 12

3.3 SYSTEM ENVIRONMENT AND EXTERNAL INTERFACES ............................................12

3.4 INDUSTRY STANDARDS ...........................................................................................13 3.5 KNOWLEDGE AND SKILLS .......................................................................................13 3.6 BUDGET AND COST ANALYSIS ................................................................................14

3.7 SAFETY ...................................................................................................................15 3.8 PERFORMANCE, SECURITY, QUALITY, RELIABILITY, AESTHETICS ETC. ..................15 3.9 DOCUMENTATION ...................................................................................................15 3.10 DESIGN METHODOLOGY .......................................................................................16 3.11 RISKS AND VOLATILE AREAS ................................................................................16

4 EXPERIMENT DESIGN AND FEASIBILITY STUDY ......................................................17

4.1 EXPERIMENT DESIGN ..............................................................................................17 4.1.1 Degrees of Freedoms - Lower Body ......................................................................... 17 4.1.2 Audio Recognition .................................................................................................... 17 4.1.3 Power Enable Module .............................................................................................. 18 4.1.4 Bluetooth Module ..................................................................................................... 18 4.1.5 Object Avoidance ..................................................................................................... 18 4.1.6 Text-to-Speech .......................................................................................................... 19 4.1.7 On-Board Computer for Computer Vision ............................................................... 19

4.2 EXPERIMENT RESULTS AND FEASIBILITY ................................................................19

5 ARCHITECTURE ........................................................................................................20

SCATY




4 of 110

5.1 SYSTEM ARCHITECTURE .........................................................................................20 5.2 RATIONALE AND ALTERNATIVES ............................................................................20

6 HIGH LEVEL DESIGN ................................................................................................21

6.1 CONCEPTUAL VIEW .................................................................................................21 6.2 HARDWARE .............................................................................................................21

6.2.1 3D-Printed Frame: ................................................................................................... 21 6.2.2 Raspberry Pi 2: ........................................................................................................ 21 6.2.3 Dynamixel XL-320 Motors: ...................................................................................... 21 6.2.4 Charging PCB: ......................................................................................................... 22 6.2.5 SG90 Motor Control: ............................................................................................... 22 6.2.6 Teensy 3.2 ................................................................................................................. 22 6.2.7 EasyVR: .................................................................................................................... 22 6.2.8 Bluetooth nRF8001: ................................................................................................. 22 6.2.9 Ultrasonic Distance Sensor HC-SR04: .................................................................... 22 6.2.10 Accelerometer ADXL337: ...................................................................................... 22 6.2.11 Amplifier: ................................................................................................................ 22

6.3 SOFTWARE ..............................................................................................................22 6.3.1 Python on Raspberry Pi 2: ....................................................................................... 22 6.3.2 Arduino IDE: ............................................................................................................ 22 6.3.3 Processing: ............................................................................................................... 23

7 DATA STRUCTURES ...................................................................................................24

7.1 INTERNAL SOFTWARE DATA STRUCTURE .................................................................24 7.2 GLOBAL DATA STRUCTURE .....................................................................................25

7.3 TEMPORARY DATA STRUCTURE ...............................................................................26 7.4 DATABASE DESCRIPTIONS .......................................................................................26

8 LOW LEVEL DESIGN .................................................................................................27

8.1 POWER MANAGEMENT ............................................................................................30 8.1.1 Voltage Regulation ................................................................................................... 30 8.1.2 Battery Charging ...................................................................................................... 30

8.2 CONTROLLERS ........................................................................................................32 8.2.1 Ground Station ......................................................................................................... 32 8.2.2 Mobile Device ........................................................................................................... 32

8.3 CENTRAL PROCESSING ............................................................................................33 8.3.1 Distance Sensing ...................................................................................................... 34 8.3.2 Object Recognition and Text-to-Speech ................................................................... 34 8.3.3 Motor Control ........................................................................................................... 35 8.3.4 Audio Recognition .................................................................................................... 37 8.3.5 Main MCU ................................................................................................................ 38 8.3.6 Wireless Transceiver ................................................................................................ 40

8.4 MECHANICAL ..........................................................................................................40 8.4.1 Lower Body ............................................................................................................... 40 8.4.2 Upper Body ............................................................................................................... 40

8.5 AESTHETIC ..............................................................................................................40 8.5.1 Face .......................................................................................................................... 41

SCATY




5 of 110

9 USER INTERFACE DESIGN ........................................................................................42

9.1 APPLICATION CONTROL ..........................................................................................42

9.2 INTERFACE ..............................................................................................................42 9.2.1 Interface 1: Mobile Bluetooth Application ............................................................... 42 9.2.2 Interface 2: GUI ....................................................................................................... 42 9.2.3 Interface 3: Voice Recognition ................................................................................. 42

9.3 DEVELOPMENT SYSTEM AND COMPONENTS AVAILABLE ..........................................42

10 EXPERIMENT DESIGN AND TEST PLAN ..................................................................43

10.1 DESIGN OF EXPERIMENTS ......................................................................................43 10.1.1 Standing on its own ................................................................................................ 43 10.1.2 Experiment 2: Walking ........................................................................................... 43 10.1.3 Experiment 3: Robot Movements ............................................................................ 43 10.1.4 Experiment 4: Manual Control via Bluetooth ........................................................ 44 10.1.5 Experiment 5: Visual Object Recognition ............................................................. 44 10.1.6 Experiment 6: Object Avoidance ............................................................................ 44 10.1.7 Experiment 7: Text-to-Speech ................................................................................ 44 10.1.8 Experiment 8: Voice Commands ............................................................................ 45 10.1.9 Experiment 9: Face Detection ................................................................................ 45 10.1.10 Experiment 10: Sleep ............................................................................................ 45

10.2 BUG TRACKING .....................................................................................................46 10.3 QUALITY CONTROL ...............................................................................................46

10.4 PERFORMANCE BOUNDS ........................................................................................47 10.5 IDENTIFICATION OF CRITICAL COMPONENTS ..........................................................47 10.6 ITEMS NOT TESTED BY THE EXPERIMENTS ............................................................47

10.6.1 Pressure Sensor ...................................................................................................... 47 10.6.2 Battery Balancer ..................................................................................................... 47

11 EXPERIMENTAL RESULTS AND TEST REPORT .......................................................49

11.1 TEST ITERATION 1: STABILIZATION/STANDING .....................................................49 11.1.1 Test 1 ...................................................................................................................... 49 11.1.2 Test 2 ...................................................................................................................... 49 11.1.3 Test 3 ...................................................................................................................... 49

11.2 TEST ITERATION 2: WALKING ...............................................................................49 11.2.1 Test 1 ...................................................................................................................... 49 11.2.2 Test 2 ...................................................................................................................... 50 11.2.3 Test 3 ...................................................................................................................... 50

11.3 TEST ITERATION 3: ROBOT MOVEMENTS ..............................................................50 11.3.1 Test 1 ...................................................................................................................... 50

11.4 TEST ITERATION 4: MANUEL CONTROL VIA BLUETOOTH ......................................50 11.4.1 Test 1 ...................................................................................................................... 50

11.5 TEST ITERATION 5: VISUAL OBJECT RECOGNITION ...............................................51 11.5.1 Test 1 ...................................................................................................................... 51

11.6 TEST ITERATION 6: TEXT-TO-SPEECH ...................................................................51 11.6.1 Test 1 ...................................................................................................................... 51

11.7 TEST ITERATION 7: VOICE COMMANDS .................................................................51 11.7.1 Test 1 ...................................................................................................................... 51

SCATY




6 of 110

11.7.2 Test 2 ...................................................................................................................... 52 11.8 TEST ITERATION 8: FACE DETECTION ...................................................................52

11.8.1 Test 1 ...................................................................................................................... 52 11.9 TEST ITERATION 9: BATTERY CHARGING ..............................................................52

12 CONCLUSION AND FUTURE WORK .........................................................................55

12.1 CONCLUSION .........................................................................................................55 12.2 FUTURE WORK ......................................................................................................56 12.3 ACKNOWLEDGEMENT ............................................................................................56

13 REFERENCES ...........................................................................................................57

14 APPENDICES ............................................................................................................58

14.1 APPENDIX A: PARTS LIST ......................................................................................58

14.2 APPENDIX B: HARDWARE TOOLS ..........................................................................59 14.3 APPENDIX C: SOFTWARE LIST ...............................................................................59 14.4 APPENDIX D: ARDUINO CODE FILES: ....................................................................60

14.4.1 SCATY_Humanoid_Robot_John_1.ino .................................................................. 60 14.4.2 Accelerometer.h ...................................................................................................... 68 14.4.3 BLE.h ...................................................................................................................... 69 14.4.4 Dynamixel.h ............................................................................................................ 71 14.4.5 EasyVR.h ................................................................................................................ 84 14.4.6 Head_Arms.h .......................................................................................................... 89 14.4.7 RPi.h ....................................................................................................................... 92 14.4.8 TFT.h ...................................................................................................................... 99 14.4.9 Ultrasonic.h .......................................................................................................... 104

14.5 APPENDIX E ........................................................................................................104 14.5.1 Python: picam_face_color_3_notext.py ............................................................... 104

14.6 APPENDIX F ........................................................................................................107 14.6.1 Processing: ee175a_slider_config_gui_1.pde...................................................... 107

SCATY




7 of 110

1 Executive Summary

The goal of our project was to design and develop an open-source low-cost humanoid robot that

would benefit four different fields: education, research/academia, consumer electronics, and extreme en-

vironments. Our open sourced project will allow educators to use it as a tool to teach programing and

robotics, consumers to have an advance and fun electronic machine to play with, and for researchers to

further advance the technology behind robotics. Overall, the complexity of the project can be divided into

two parts: the hardware, which is composed of diverse peripherals, and the software, which has a vast

amount of functions, variables, and complex relations.

The hardware within the system allows the robot to have human-like features and have the capa-

bility to accomplish many tasks that the user can implement. Our Alpha prototype chassis was mainly

fabricated with 3D printed plastics (Figure 1.1) that allows for easy replication while maintaining the goal

of being low-cost. The system is powered by a 7.4V 2-Cell 1000mAh Li-Po battery that allows the robot

to have an operation run time of approximately 30 minutes. Also, with the Raspberry Pi 2 and Teensy 3.2,

many peripherals such as the EasyVR, HC-SR04, TFT Display, Raspberry Pi Camera and multiple actua-

tors allows the user to learn about the integration with the MCU and integrate more peripherals

The software of the system was programmed with C and python on the Teensy 3.2 and Raspberry

Pi 2, respectively. The Raspberry Pi 2 is used to process the input from the camera to detect faces and

colored object, run the text-to-speech software, and, on top of that, is actively communicating with the

Teensy 3.2 through the UART protocol. The Teensy 3.2 is the main MCU that runs the task scheduler on

top of being integrated with the rest of the peripherals of the robot. Throughout the software, many com-

munications protocol are implemented, such as TTL, SPI, UART, Bluetooth, PPM, and I2C. By having

these different protocols we were able to efficiently cover the most common industry protocols, which

can be used to educate advanced users.

By combing the hardware and software together, the humanoid robot was constructed. It has the

capabilities to recognize voice commands, process text-to-speech, have interactive facial expressions,

recognize faces and colored objects, and have 18 degrees of freedom. On top of that, we have a Bluetooth

module integrated with a Bluetooth Android application that allows the user to preset commands and

directly interact with the robot.

Figure 1.1: Alpha Prototype Standing on Right Leg

SCATY




8 of 110

2 Introduction

The design of our robot was inspired by many of the humanoid robots currently available in the market. It

incorporates many branch of electrical engineering such as robotics, power, and embedded systems.

These branches allow the project to be developed in the most efficient method in a complex project.

2.1 Design Objectives and System Overview

The objective of our group is to design a bipedal humanoid that is capable of interacting with humans by

recognizing faces, responding to voice commands, and be able to express itself with speech. However,

our overall goal is to allow the humanoid to be used for educational purposes, research and academia,

consumer electronics, and be applicable in extreme environments.

The robot can be used to teach children and adults C programming along with the hardware and

software aspects of robotics. For research and academia, researchers can use the humanoid robot to test

new algorithms and theories in control systems, kinematics, motion planning, and robotics in general. As

a consumer electronics product, users can use the robot as it is for entertainment or can be combined for

educational and practical purposes, as an assistant.

We designed the robot with the following technical objectives. We planned for the robot to run on

batteries for an operation time of 30 minutes. The range of the wireless communication to be 3.3 meters,

or 10 feet. The structure of the robot to have 18 degrees of freedom, where 10 would be on the lower

body, 6 for the arms, and 2 for the head. For the object detection range, we designed it to detect objects

accurately from the range of 4 to 100 centimeters. On top of detecting obstacles, the robot would be able

to detect three different objects, which could be considered “toys” for the robot. The voice command was

programmed to recognize 25 different commands and would be paired with the text-to-speech, in which

the robot would speak in the English language.

This is a meaningful project for us because we both have strong interest in embedded systems and

robotics. This project incorporates electrical engineering by integrating courses from the Electrical Engi-

neering program at UC Riverside, such as introductory and intermediate embedded systems (EE/CS120B

and CS122A), data acquisition, instrumentation, and process control (EE128), electronic circuits

(EE100A/B), automatic control (EE132), and computer vision (EE146)

2.2 Backgrounds and Prior Art

Several other humanoid robots are available in the market, such as the Nao Robot (see Figure 2.2.1), the

Robotis Mini (see Figure 2.2.2), and the NASA Valkerye (see Figure 2.2.3). The Nao Robot from Al-

deraban Robotics is a 58 cm humanoid robot, popular in academia and commercial industries. Currently,

the Nao robot can be acquired in North America for $7990. This robot has been widely used on the Ro-

bocup Competition - robot soccer competition. The Robotis Mini is a $500 humanoid robot designed for

the consumer electronic industry. By limiting the number of features, the Robotis Mini is able to set a

competitive price to open the humanoid robot market to middle-income families. The NASA Valkerye is

one of the latest humanoid robots designed by NASA to compete on the DARPA Robotics Challenge.

Inspired from previous humanoid robots designed by NASA, Valkerye packs some of the most advance

sensors and software packages developed for humanoid robots.

SCATY




9 of 110

Figure 2.2.1: Nao Robot – a customizable interactive robot

Figure 2.2.2: Robotis Mini – Humanoid robot kit with open source embedded board

Figure 2.2.3: NASA Valkeyrie – a six-foot-tall robot is packed with cameras, sensors and sonar to

help it maneuver with grace

2.3 Development Environment and Tools

The frame for the humanoid robot was printed using the Makerbot Replicator 2.0 from IEEE at UCR. The

CAD file for the frame was provided by an open source forum.

SCATY




10 of 110

3D Printing

The frame for the humanoid robot was 3D printed using the Makerbot Replicator 2 - 3D printer, from

IEEE at UCR. The CAD files were sliced using the MakerBot Desktop software. Custom 3D CAD parts

were designed on SolidWorks.

Electronics Development

For soldering, we used a Weller WES51 - soldering stations, and Weller WLC100 - soldering station. To

debug and troubleshoot signals, we used a Saleae Logic 8 - logic analyzer, and a Agilent DSO3102A -

digital storage oscilloscope. To supply power to initial prototypes and for testing, we used the following

power supplies: the HP E3630A and the MPJA 9615PS. For measuring voltage, resistance, current, and

continuity, we used the following multimeters: the HP 34401A and the Triplett 1101-B. To charge the 2S

LiPo batteries safely, we used the iMAX B6 battery balance charger. To check the battery status, we used

the Hobbyking Cellmaster 7 - a digital battery health checker.

Software Development

To program the Teensy 3.2 - our main MCU, we used the Arduino IDE with the Teensyduino add-on. To

develop the GUI for the ground station controller, we used the Processing IDE. The Raspberry Pi 2 ran

Raspbian Jessie - a Linux operating system, and we used the Python IDE to develop the Python scripts. A

laptop running Windows 8.1 ran the Processing GUI application, allowed for further development on the

Raspberry Pi 2 by creating an SSH or a VNC connection, and compiled the source code on Arduino IDE

for the Teensy 3.2.

For the mobile station controller, we used a OnePlus One Android smartphone with the following apps:

Bluefruit LE and nRF Toolbox.

2.4 Related Documents and Supporting Materials

The Bluetooth core specification can be found on their official website:

https://www.bluetooth.com/specifications

2.5 Definitions and Acronyms

Adafruit - Online electronics store

Arduino IDE - Integrated development environment for Arduino supported microcontrollers

BLE - Bluetooth Low Energy

CAD – Computer-aided design

EasyVR – Voice Recognition Module

EasyVR Commander - Software to train the EasyVR module

GUI - Graphical User Interface

I2C - Inter-Integrated Circuit

IEEE – Institute of Electrical and Electronics Engineers

Makerbot – 3D printer

MCU - Microcontroller Unit

OpenCV – Open Computer Vision

SCATY




11 of 110

PCB - Printed Circuit Board

PLA - Polylactic Acid

Pololu - Online electronics store

PPM - Pulse Position Modulation

Raspberry Pi – ARM Processor

Sparkfun - Online electronics store

SPI - Serial Peripheral Interface

SSH - Secure Shell

Teensy 3.2 – an ARM Cortext-M4 microcontroller

Teensyduino – Add-on for Arduino IDE to allow Teensy to be programmed on the Arduino IDE

TFT LCD - Thin Film Transistor Liquid Crystal Display

TTL - Transistor-Transistor Logic

TTS - Text-to-Speech

UART - Universal Asynchronous Receiver/Trasmitter

UCR - University of California, Riverside

USB - Universal Serial Bus

VNC - Virtual Network Computing

SCATY




12 of 110

3 Design Considerations

3.1 Assumptions

The end product is designed to be used in controlled environments. The field of bipedal humanoid robots

is still on an early stage, and this project aims to make bipedal humanoid robots more accessible by its

small size, low cost, and open sourced design.

3.2 Realistic Constraints

3.2.1 Weight/size Constraints

The weight and size of the robot need to be kept at the lowest possible because a bipedal humanoid robot

is designed to operate vertically. The actuators controlling the lower joints of the robot will require the

most torque. The robot will be designed to not be taller than 16 inches tall and lighter than 4 pounds. The

large amount of electromechanical and electronic components will need to be as small as possible to min-

imize the overall size of the robot.

3.2.2 Technical and Skill Constraints

This project involves the application of several topics that the Electrical Engineering program at the Uni-

versity of California, Riverside, cover in their curricula; however, this project requires to expand on the

materials covered in the standard curricula. The technical constraints include the implementation of mul-

tiple concurrent state machines, interfacing multiple peripherals with different communication protocols,

PID tuning, interfacing with diverse types of actuators, facial recognition, target tracking with a single

moving camera, and safe charging of 2S Lithium batteries with cell balancing. The mechanical and kine-

matic design of the robot also require a set of expertise our team does not feature.

3.2.3 Project Time and Budget Constraints

The project was given a 9-month design and development period with a 2-person team and a limited

budget of $500. The first 3 months were reserved for design development and the built of a proof-of-

concept while the following 6 months were designated to developing the first full prototype. Our largest

projected expense were the motors required to move the 20 joints in the robot.

3.2.4 End Product Price Constraints

One of the main goals of this project is to keep the end product price low; thus, the parts and components

used on the prototype need to be cost efficient.

3.3 System Environment and External Interfaces

The ground station remote controller code runs using the Processing IDE, which is available on Windows,

Mac OS, and Linux. All the same, the program can be compiled as a standalone software; however, it

would require a system with Java installed, to run.

The mobile station remote controller was tested on Android devices, but there are similar available solu-

tions on the App Store for iOS devices.

SCATY




13 of 110

To update the firmware of the robot, a computer running the Arduino IDE must be set up with the Teen-

syduino Add-On and the other libraries stated on the Reference section of this manual.

3.4 Industry Standards

This project features I2C, SPI, and Bluetooth standards.

Communication between several peripherals within the robot used I2C and SPI standards.

Wireless communication with the robot is established using Bluetooth 4.0 standards, also known as Blue-

tooth Low Energy (BLE). Following the GATT service profiles, we are using the UART service to create

a virtual wireless UART connection between the robot and a remote controller.

3.5 Knowledge and Skills

This project required knowledge on embedded systems, computer vision, control systems, power man-

agement, electronic circuits, and electronic component acquisition.

This project required the following skills: EAGLE CAD PCB design and development, soldering, circuit

design, use of oscilloscopes, multi-meters, and power supplies, programming in C, C++, Python, Pro-

cessing, and Arduino.

Courses and Experience Related to this Project

Alberto

EE/CS 120B, CS 122A - Beginner and Intermediate Embedded systems

EE 1A/B - Engineering Circuit Analysis

CS 13 - Introduction to Computer Science for Engineering Majors

EE 100 A/B - Electronic Circuits

Previous Experience:

o SolidWorks

o Arduino

o Rapid Prototyping Techniques, such as 3D printing

Stanley:

EE 1A/B - Engineering Circuit Analysis

CS 10/12 - C++ Programming I/II

EE100 A/B - Electronic Circuits

EE/CS 120B, CS 122A - Beginner and Intermediate Embedded Systems

EE 128 - Data Acquisition, Instrumentation, and Process Control

Previous Experience:

o Arduino

SCATY




14 of 110

New Knowledge and Skills

Communication and Control of Dynamixel 320 - smart servo motors, using TTL

OpenCV on Raspberry Pi

Python on Raspberry Pi

Target Tracking with Single Moving Camera

Control Systems for Multilink Robots

Text-to-Speech implementation

Voice Recognition implementation

Power Management for 2S (2-cell) Lithium-Ion Polymer Packs

3.6 Budget and Cost Analysis

By aiming to market this robot as a low-cost product, we set our budget to be $500.

Cost Analysis for Single Product

Qty Item Cost

10 Dynamixel XL-320 219.00

1 ROBOTIS Rivet Set 21.9

1 ROBOTIS Screw Set 2.90

1 EasyVR 3.0 38.25

1 Teensy 3.2 19.80

1 Red PLA 1.75 mm 22.98

1 Raspberry Pi 2 Model B 35.00

1 Raspberry Pi Camera 17.88

6 Micro Servo Motors SG90 15.00

2 LiPo 7.4V 2S 1000mAh 8.58

1 TFT 2.2" Display 320x240 8.00

2 6V 2.5A Step-Down Volt. Reg. 19.9

1 5V 1A Steo Down/Up Volt. Reg. 4.95

1 3.3V 1A Step Down/Up Volt. Reg. 5.95

1 PCA9685 LED Driver IC 2.54

1 HC-SR04 Ultrasonic Sensor 0.97

1 Sparkfun Mono Audio Amp 7.95

1 3.5mm Connector Breakout Board 0.99

1 Current Sensor ACS711EX -31A to 31A 3.95

1 Bluefruit LE - nRF8001 19.95

1 3-axis Accelerometer - ADXL337 9.95

SCATY




15 of 110

1 Relay Board - srd-05vdc-sl-c 1.31

Miscellaneous Electronics 20.00

Sub Total $507.70

3.7 Safety

With the use of LiPo batteries, we have to ensure that the user charges the batteries properly. To do so, we

designed an on-board safe charging circuit that allowed the batteries cells to be properly balanced and

charged. We added a distance sensor onto the head to allow the robot to sense any obstruction in its path

to halts its movement to protect itself from getting damage and to keep users from getting harmed. To

safeguard from any potential bug, we have limited the range of motion from the actuators to avoid any

damages to the robot or users.

3.8 Performance, Security, Quality, Reliability, Aesthetics etc.

Performance: We want the overall performance of the robot to be enjoyable and promising. To achieve

this goal, we have to have extensively quality control to ensure that each component will work well to-

gether and have little to no errors while in operation.

Security: The design of our product ensures that users cannot unintentionally open up the body, which

houses the electronics and power system. Also due to the use of Li-Po batteries to power up our product,

we designed a safe charging circuit that will ensure the batteries cells are properly balanced and charged.

Quality Control: To ensure that the quality of our robot is exceptional, we extensively test each compo-

nent to determine whether or not it is fit to be used on our robot. Before we add any new hardware or

software to our design, each part must be tested and verified that it would work perfect with the overall

design.

Reliability: Ideally, the reliability of the robot should be top notch. With the application for research and

extreme environment, the robot should not fail in these environment as results would be inadmissible or

missions would fail if humans cannot retrieve the robot in the extreme environments. To test the reliabil-

ity of our robot, we would meticulously test each actions in multiple scenario and make adjustments until

we receive passing results.

Aesthetic: As our product has an intended use for consumer electronics, we have to create a design that

would catch consumer’s eyes, but at the same time be neutral to all audience. Another application that our

product would be for is educational. The design of the robot should be friendly enough to allow children

to be engaged and learn at the same time. To accomplish an aesthetically pleasing design, we designed the

3D printed body ourselves and designed the robot face on a TFT LCD Screen to look “cute”.

3.9 Documentation

To document the process of our senior design, we created a folder on Google Drive that contains all the

information regarding our project. Within the folder we created sub-folders to neatly organize our infor-

mation, such as: 3D-Printing, Code, Design Files, Final Report, Notes, Photo and Media, and Validation.

When we meet to discuss our progress, we created a Google Document with the corresponding date.

Within the document, we would take notes on what we have done, our goals, and our projected timeline

for each tasks. We have multiple documents containing notes on our trial-and-errors, validation tests, test

data and results, parts lists, and high-level design. For hand written documents and pictures, we scanned

the documents and uploaded them onto the drive. This protocol allowed us to have an electronic copy in

case a document got lost.

SCATY




16 of 110

By having everything uploaded and documented on Google Drive, it gave us easy and fast access to our

past information. This also allowed us to compile everything into the final technical report with ease and

allowed easy access for others to view our files if needed.

3.10 Design Methodology

For our design methodology, we would first prototype and test independent systems to identify and trou-

bleshoot any initial bugs and design issues. After such development phase, the system would be proto-

typed on its final form, such as a prototyping board, PCB, or independently soldered wires. Then, we

would proceed to a partial system installation, which would involve interfacing several independent mod-

ules into a modular section of the project. After resolving any potential issues, a full system implementa-

tion and test would allow for a complete check of the designs.

3.11 Risks and Volatile Areas

Making the humanoid robot is one of the technical challenges that presents the most risk because it not

only involves electrical engineering but also a good understanding on mechanical engineering. For such,

we will attempt to tackle this challenge incrementally to evaluate and try to solve any design issues from

early on. All the same, we can foresee that the most difficult stage will be when all the load is installed on

the robot. Considering the limited time and financial resources, we will allocate a large portion of our

development schedule to solving this technical challenge avoiding to incur in further financial expense for

redesign.

SCATY




17 of 110

4 Experiment Design and Feasibility Study

To analyze and evaluate the degrees of freedom required for the lower body of the humanoid robot, we

designed an early cardboard prototype using inexpensive SG90 servo motors that had 10 degree of free-

doms (see Figure 4.1).

Figure 4.1: Cardboard prototype with SG90 Servo Motors

4.1 Experiment Design

The following showcases the different solutions we explored before selecting a final implementation.

4.1.1 Degrees of Freedoms - Lower Body

The experiment design was to see how complex the project was going to be by first developing only the

lower half of the body. With much research, we decided to test out 10 degrees of freedom for the the low-

er half as it was the most popular and efficient design. The structure was designed with hollowed out

cardboard and a thin strip of wood with the consideration of the motors we were using. The SG90 servo

motors do not provide a lot of torque so the structure was required to be very light weight. After the as-

sembly the final test was to program the Arduino to make the robot walk.

Stanley and Alberto was responsible for this task.

4.1.2 Audio Recognition

The experiment that was executed using an Arduino library called “uSpeech” which only required the use

of a MCU and a microphone with an amplifier. We tested it extensively however the accuracy on the

library was very low. A small vibration would be picked up as a command which would be a problem as

SCATY




18 of 110

the robot would have a lot vibration going through it with multiple actuators running. The main issue that

we decided to go with the EasyVR module and stop using the uSpeech library because the author indicat-

ed that the library should not be used with a real time scheduler.

Stanley and Alberto was responsible for this task.

4.1.3 Power Enable Module

The initial method we used was using the TIP31C NPN Transistor to act as a switch to allow the motors

to turn off and go to sleep mode. However the result of the design did not meet our expectation. The

thermal consideration as it would get really hot and had some difficulty supplying enough current to our

motors so we decided to switch over to a mechanical relay

Stanley was responsible for this task.

4.1.4 Bluetooth Module

The initial design of the product was using the HC-05 Bluetooth 2.0 module as it was cheap and efficient.

However, our MCU ran out of Serial ports so to rectify the problem, we decided to use a slightly more

expensive Bluetooth 4.0 module that communicated through SPI protocol. Also due to the change from

Bluetooth 2.0 to 4.0, the standards were not backward compatible so we had to change our method of

remote connection.


4.1.5 Object Avoidance

We were able to design the algorithm for the object avoidance during the initialize phase in which the

robot would continuously read input data from the HC-SR04. If the object detected an object in front of it

within a 15 centimeter range the object avoidance function would be executed. The robot would stop

walking, move left, move forward until it passes the object, and move right. Lastly, the robot will contin-

ue its previous movements before the object was detected. You can view the code below in Table 4.1.5.1.


void obj_detc(){ digitalWrite(trig, LOW); delayMicroseconds(2); digitalWrite(trig, HIGH); delayMicroseconds(10); digitalWrite(trig, LOW); duration = pulseIn(echo, HIGH); distance = (duration/2) / 29.1; }

void obj_avoid(){ obj_detc(); while(distance < 15){ led_color(0); moveLeft(); obj_count++; obj_detc(); avoid_flag = 1; } if(avoid_flag == 1){ for(int i = 0; i< 7;i++){ moveLeft(); obj_count++; } walk_function(); walk_function(); for(int i = 0; i < obj_count; i++){ moveRight(); } } avoid_flag = 0; led_color(1); }

Table 4.1.5.1: Code for Object Avoidance

SCATY




19 of 110

4.1.6 Text-to-Speech

Our first implementation for text-to-speech aimed to find an embedded solution to obtain low-

power consumption, low-cost, and an independent solution. The Speakjet was a sound synthesizer that

seemed to meet most of our criteria, except low-cost because of its $25 price tag. However, the quality of

the “speech” produced was very low, and it didn’t truly function as text-to-speech; instead, the Speakjet

required the specific pronunciation of words instead of simply converting strings of text to speech sounds.

After such, we evaluated for other solutions, giving higher priority to quality of sound because the

“speech” had to be understandable. We were only able to find a couple other available embedded solu-

tions, off-the-shelve, but one was designed for Chinese language while the other one cost more than $50.

Hence, we decided to explore using the Raspberry Pi 2 that we already had in our design considerations.

Within the Raspberry Pi 2, I tested the following solutions: eSpeak, Cepstral TTS, Pico TTS,

Google TTS, and Festival TTS. The quality of eSpeak was lower than desired. Cepstral TTS required a

licence, which would result troublesome on later development since we wanted to open sourced our de-

signs and files. Pico TTS had less support as it used to have, but I found instructions online to compile it

myself but it might be very time consuming. I couldn’t get Google TTS working as suggested by many

sources, but I noticed that the updated Terms and Conditions of the Google API deemed this solution in

violation of such. Festival TTS required a very simple installation, usage, and decent quality of speech.

Thus, I chose to use Festival TTS.

Alberto was responsible for this task.

4.1.7 On-Board Computer for Computer Vision

I have had experience working with the Raspberry Pi single-board computer and computer vision using

OpenCV, separately, but I had seen this implementation before and knew about its feasibility. We first

selected the Raspberry Pi Model A+ because of its size, lower power consumption, and cheaper price. All

the same, I worked on the development of the Python scripts and learned how to use the OpenCV package

on Python in the Raspberry Pi Model B+. After noticing how slow the system was, I decided to upgrade

our implementation to a Raspberry Pi 2 because of the featured quad core processor, making the computer

vision processing a lot smoother and giving us the option to use the Raspberry Pi 2 for other purposes,

too, such as Text-to-Speech.

Alberto was responsible for this task.

4.2 Experiment Results and Feasibility

The result of the experiment prove that the project was feasible and the project would be approved to the

next stage of prototyping. Our initial design proved that the robot must be light weight, however the SG90

servo motors would not be able to handle the stress and weight of an overall humanoid robot. This al-

lowed us to purchase stronger motors and come up with a more stable frame for the system. The end re-

sult for the next phase ended with the purchase of 10 Dynamixel XL-320 Smart Servo Motors to keep the

initial design of 10 degree of freedoms for the leg and then using the MakerBot to 3D print the frame as it

would be light weight and low cost.

SCATY




20 of 110

5 Architecture

For the overall system architecture, refer to Section 6.1 for the High Level Design System’s Block Dia-

gram. All the same, several of the components are hidden inside the body of the robot to optimize for size

(see Figure 5.1 and Figure 5.2).

Figure 5.1: Arrangement of Physical Layout of Voltage Regulators and Current Sensor

Figure 5.2: From left to right & top to bottom, Bluefruit LE, Custom Servo Board, and Teensy 3.2

5.1 System Architecture

Refer to Section 6.1

5.2 Rationale and Alternatives

Not Applicable.

SCATY




21 of 110

6 High Level Design

6.1 Conceptual View

Our System Block Diagram describes the major blocks of our robot (see Figure 6.1.1).

Figure 6.1.1: System Block Diagram

6.2 Hardware

6.2.1 3D-Printed Frame:

Alberto is responsible for printing out the frame and modeling head and body of the robot.

Stanley is responsible for the assembly of the robot and giving inputs on the design.

6.2.2 Raspberry Pi 2:

Alberto is responsible for the Raspberry Pi in which he installed OpenCV to have facial and object recog-

nition with the Raspberry Pi camera and he also integrated text-to-speech on the system.

6.2.3 Dynamixel XL-320 Motors:

Both Alberto and Stanley are responsible for learning how to operate the smart motor.

SCATY




22 of 110

6.2.4 Charging PCB:

Alberto is responsible for creating the schematic and PCB to charge and balance the cells of the 2-cell Li-

Po battery.

6.2.5 SG90 Motor Control:

Alberto created a motor control using the PCA9685 to control different PWMs for all 8 SG90 motors we

have on the robot through I2C communications to save pins on the microcontroller. Stanley developed the

supporting functions to improve the control of all motors.

6.2.6 Teensy 3.2

Stanley and Alberto are responsible for programming the Teensy. Alberto wrote the code to integrate

communication through serial to send commands while Stanley wrote the code for the actions of the ro-

bot.

6.2.7 EasyVR:

Stanley is responsible for the voice recognition, in which he trained the module to recognize commands

and integrated the code to the Teensy.

6.2.8 Bluetooth nRF8001:

Stanley is responsible for Bluetooth communication with the robot and a smart device.

6.2.9 Ultrasonic Distance Sensor HC-SR04:

Stanley is responsible for the integration of the distance sensor to assist the robot detect objects

6.2.10 Accelerometer ADXL337:

Stanley is responsible for using the accelerometer to help the robot balance and walk.

6.2.11 Amplifier:

Alberto is responsible for the construction of the PCB and

6.3 Software

6.3.1 Python on Raspberry Pi 2:

Alberto is responsible for installing and developing scripts in Python, using, OpenCV for facial and object

recognition, and Festival TTS, for text-to-speech, for the Raspberry Pi 2.

6.3.2 Arduino IDE:

Stanley is responsible for programming the Teensy for the actions of the robot and the voice recognition.

SCATY




23 of 110

6.3.3 Processing:

Alberto designed a GUI to assist the movement of the Dynamixel XL320 motors to help with the algo-

rithm of walking.

SCATY




24 of 110

7 Data Structures

Our main focus for software development involved developing the code for the Teensy 3.2 on the Ar-

duino IDE. We organized the code by a main file and files for the different major peripherals on our sys-

tem, so our file structure is:

SCATY_Humanoid_Robot_John_1.ino

o Main file that contains the inclusion of libraries, objects, global variable declarations, and

the Task Scheduler

Accelerometer.ino

o Contains the core functions to read from the accelerometer

BLE.ino

o Contains the initiation and listening function to receive incoming commands

Dynamixel.ino

o Includes string parsing functions, functions to control the Dynamixel XL-320 smart servo

motors, PID, and the menu interface for manual control

EasyVR.ino

o Contains the initiation and state machine for the EasyVR module

Head_Arms.ino

o Contains the initiation for I2C, helping functions for the SG90 servo motors, and the state

machine for adjusted motor updates

RPi.ino

o Includes the helping functions for the Teensy 3.2 to interact with the Raspberry Pi 2 and

the state machine to receive and transmit data between these two devices

TFT.ino

o Contains the initiation for the TFT display, helping functions to animate the face expres-

sions of the robot, and the state machine to cycle through the different modes

Ultrasonic.ino

o Includes the initiation and reading functions for the HC-SR04 ultrasonic sensor

7.1 Internal software data structure

Inputs:

Accelerometer readings

o Acquired on Accelerometer.ino

BLE incoming commands

o Acquired on BLE.ino

EasyVR

o Acquired on EasyVR.ino

RPi Stream

o Acquired in SM4_Tick_RPi() within RPi.ino

Ultrasonic

o Acquired in Ultrasonic.ino

Processing:

Task Scheduler

o Located in SCATY_Humanoid_Robot_John_1.ino

SCATY




25 of 110

Command Menu Interface

o In the form of command_menu(char) in Dynamixel.ino

Servo State Machine

o In the form of servo_write() in Head_Arms.ino

RPi Parsing State Machine

o In the form of SM4_Tick_RPi() within RPi.ino

TFT State Machine

o In the form of SM2_Tick_TFT() in TFT.ino

Outputs:

Dynamixel XL-320 Motors

SG90 Motors - Arms and Head

RPi Text-to-Speech

TFT Display

7.2 Global data structure

The Task Scheduler runs the following tasks with the following functions:

t1Callback_HeadCameraFollow()

o SM1_Tick_HeadCameraFollow()

This function runs as a state machine for following objects with only the head by

using the input from the Raspberry Pi 2 and the pi camera

t2Callback_RPi()

o SM4_Tick_RPi()

This function runs as a state machine for receiving and transmitting information

to the Raspberry Pi 2

t3Callback_SPI()

o SM2_Tick_TFT()

This function runs as a state machine cycling through the facial expressions, in-

cluding blinking and changes in “moods”

o BLE_loop()

This function handles the Bluetooth 4.0 transmissions and collection of data

t4Callback_EASYVR()

o EasyVR_SM()

State machine handling the communications with the EasyVR 3.0 module

t5Callback_command()

o command_menu_loop()

Function waiting for manual commands from either Bluetooth 4.0 or EasyVR

t6Callback_servo()

o servo_write()

State machine that allows speed control of the SG90 servo motors

t7Callback_PID()

o dynamixel_PID_x()

Function that constantly runs PID to maintain the robot balanced

t8Callback_walkStep()

o walk_left_step()

State machine for the execution of a left step. This function is part of the devel-

opment phase for walking

SCATY




26 of 110

7.3 Temporary data structure

Not applicable.

7.4 Database descriptions

Not applicable.

SCATY




27 of 110

8 Low Level Design

The implementation of our design required the use of several components. The schematics of the Central

Processing describes the connections between the different solutions we selected for the Central Pro-

cessing blocks (see Figure 8.1). The Teensy 3.2 is the main MCU, and the schematics show the purpose

of each GPIO. The HC-SR04 is an ultrasonic distance module that allows us to determine the distance of

different obstacles and objects. The EasyVR 3.0 was our selected solution for voice recognition. The

TFT_DISPLAY allowed us to incorporate a face to our robot and add aesthetics. The Raspberry Pi 2 is a

single-board computer that ran the scripts for image processing and Text-to-Speech. The output of the

Raspberry Pi 2 was amplified by a mono audio amplifier before being played by an 8 ohm 1 Watt speak-

er. The Bluefruit BLE was our selected solution for wireless communication using Bluetooth 4.0. The

Servo Board was a custom design solution for controlling the upper body (see figure 8.3). The Accel-

erometer ADXL337 allowed us to receive feedback on the balance of the robot’s body. A Relay Board

enabled us to power cycle the lower body motors, or disable them entirely if necessary. A voltage divider

was designed to acquire a reading on the voltage of the battery.

Figure 8.1: Schematics of the Central Processing

The voltage regulation was performed by several independent voltage regulator modules (see Figure 8.2).

A switch and current sensor were placed before the voltage regulators to allow for ON/OFF control and

measurement of overall power use. The Dynamixel XL-320 are smart servo motors selected for the lower

body. They were daisy chained to improve on wire management.

SCATY




28 of 110

Figure 8.2: Schematics of the Power Management and Lower Body

The Servo Board required the custom design of a PCB to use the PCA9685 PWM I2C LED controller to

generate the PPM signals to control the several servo motors used on the upper body and to minimize the

wire management required to provide power to such motors (see Figure 8.3).

SCATY




29 of 110

Figure 8.3: Schematics of the Custom Servo Board for Controlling the Upper Body

SCATY




30 of 110

8.1 Power Management

Figure 8.1.1: Voltage Level Distribution

8.1.1 Voltage Regulation

The voltage regulation module includes a current sensors and steps down the battery voltage down to

3.3V, 5V, and 6V. The current sensor is a ACS711EX, with a range of -31A to 31A. The output is given

as an analog voltage based on the following equation: VOUT = (Vcc/2)+(i*(Vcc/73.3A))

The S7V8F3 Pololu 3.3V Step-Up/Step-Down voltage regulator features a TPS63060 buck boost con-

verter from Texas Instruments.

Schematics for S7V8F3: https://www.pololu.com/file/0J791/s7v8x-schematic.pdf

Datasheet for TPS63060: https://www.pololu.com/file/0J793/tps6306x-datasheet.pdf

The S7V7F5 Pololu 5V Step-Up/Step-Down voltage regulator also features a TPS63060 buck boost con-

verter from Texas Instruments.

Schematics for S7V7F5: https://www.pololu.com/file/0J790/s7v7f5-schematic.pdf

The D24V22F6 Pololu 6V 2.5A Step-Down voltage regulator is a switching regulator with integrated

reverse-voltage-protection, over-current protection, over-temperature shutoff, and soft start.

Alberto selected and acquired the voltage regulators and the current sensor. Stanley arranged the regula-

tors and sensor in a single prototyping board and soldered headers and wires.

8.1.2 Battery Charging

Using the MCP73842, the robot includes a custom on-board charging circuit for 2S Lithium batteries. The

schematic was based on the suggested layouts shown on the datasheet (see Figure 8.1.2.1). The resistor

values were chosen according to the formulas provided by the datasheet. R1 is 0.110 ohms to obtain a

SCATY




31 of 110

charging current of 1 Ah. From the schematics, Alberto also designed a PCB layout for manufacturing a

prototype (see Figure 8.1.2.2). Once the PCB and components arrived, we assembled the prototype (see

Figure 8.1.2.3).

Figure 8.1.2.1: Schematic for Custom 2S Battery Charger

Figure 8.1.2.2: Board Layout for Custom 2S Battery Charger

SCATY




32 of 110

Figure 8.1.2.3: Assembled Prototype for Custom 2S Battery Charger

8.2 Controllers

The Controllers allowed the user to remotely operate the robot from either, a ground station or mobile

device.

8.2.1 Ground Station

The Ground Station consisted of a custom GUI written in Processing running on a Windows laptop with

Bluetooth 4.0.

Figure 8.2.1.1: Processing GUI to Control the Robot

8.2.2 Mobile Device

The Mobile Device consisted of an Android smart phone running the Bluefruit LE app, designed by Ada-

fruit (see Figure 8.2.2.1). The user could send commands to the robot through a text-based interface. Fur-

thermore, we used the nRF Toolbox app for Android to create a mobile user interface (see Figure 8.2.2.2).

SCATY




33 of 110

Figure 8.2.2.1: Bluefruit LE app on Android Smartphone

Figure 8.2.2.2: nRF Toolbox UART Interface

8.3 Central Processing

The central processing module is composed of the main MCU and other peripherals: distance sensing,

object recognition, motor control, audio recognition, text-to-speech, and wireless transceiver (see Figure

8.1). Most of these modules were stored on the torso compartment of the robot (see Figure 8.3.2) and

inside the head. Due to the constrained space and large quantity of wires, the components were packed

very tightly within the body.

SCATY




34 of 110

Figure 8.3.2: Torso of Robot Used for Component Storage

8.3.1 Distance Sensing

An HC-SR04 Ultrasonic sensor module was used to measure the distance of objects from the robot loca-

tion.

8.3.2 Object Recognition and Text-to-Speech

A Raspberry Pi 2, with a Pi Camera, ran a Python script, on the background, to process the captured im-

ages with the OpenCV module and transmit processed data to the main MCU via UART with the

py_serial module.

By running the object recognition Python script on the background, the Raspberry Pi 2 was able to exe-

cute TTS commands, using Festival, on the foreground and output the sound through the 3.5 mm port to

the Sparkfun Mono Audio Amp - TPA2005D1, which amplified the signal to be played by an 8 ohm 1

Watt speaker. The Sparkfun Mono Audio Amp was mounted on the side of the robot to avoid receiving

the noise generated from the wires and circuits inside the robot (see Figure 8.3.2.1).

SCATY




35 of 110

Figure 8.3.2.1: Prototype Board with Sparkfun Mono Audio Amp

8.3.3 Motor Control

We designed a custom Servo Board featuring a PCA9685 I2C 16-Channel PWM LED driver to generate

the PPM signals to control the SG90 servo motors.The relay module allowed us to enable or disable the

Dynamixel XL-320 motors by controlling its supplied power. The Dynamixel motors are daisy-chained to

each other and communicates to the MCU through the TTL communication protocol. An ADXL337 3-

axis accelerometer module, placed on the hip of the robot and in between the legs, allowed us to get feed-

back on the inclination of the robot’s body (see Figure 8.3.3.1). The accelerometer was also used to help

with PID control and tuning in which we integrated mode filtering to help reduce the noise that our sys-

tem was picking up.

Figure 8.3.3.1: ADXL337 3-axis Accelerometer Placement

SCATY




36 of 110

The Dynamixel XL-320 required a custom communication protocol as seen in Table 8.3.3.2. By

experimenting with the example code provided by the XL320.h library for Arduino, we used a

Saleae Logic Analyzer to investigate the instruction packet format for setting a motor at position

0 (see Figure 8.3.3.3) and a motor at position 1023 (see Figure 8.3.3.4). By comparing the results

with the format provided, we were able to learn and understand how to control the motor (see

Table 8.3.3.5).

Header Reserved ID Packet Length Instruction Parameter 16 bit CRC

0xFF 0xFF 0xFD 0x00 ID Length

Lower Length

Higher Instruction Parameter

1 ... ... Parameter

N CRC

Lower CRC

Higher

Table 8.3.3.2: Dynamixel XL-320 Instruction Packet Format

Figure 8.3.3.3: Saleae Logic Analyzer. Packet for Motor on 0

Figure 8.3.3.4: Saleae Logic Analyzer. Packet for Motor on 1023

Header Reserved ID Packet Length Instruction Parameter 16 bit CRC

0xFF 0xFF 0xFD 0x00 ID Length

Lower Length

Higher Instruction Parameter

1 ... ... Parameter

N CRC

Lower CRC

Higher

255 255 253 0 1 7 0 3 30 0 0 0 \ E

255 255 253 0 1 7 0 3 30 0 255 3 Y 199

Table 8.3.3.5:Instruction Packet for Motor on 0 and on 1023

SCATY




37 of 110

8.3.4 Audio Recognition

Easy VR 3.0 is a module with built-in voice recognition features and UART communication. Stanley pre-

programmed a set of voice commands to fit the needs of our robot (see Table 8.3.4.1).

Table 8.3.4.1: List of Pre-Programmed Commands

Connect the EasyVR to the computer via a TTL to USB device.

Open EasyVR Commander and connect the EasyVR to the software (see Figure 8.3.4.2)

Figure 8.3.4.2: EasyVR Commander on Windows 7

The EasyVR module and microphone were placed on the head to keep the microphone expose and mini-

mize noise generated from the other wires within the torso of the robot (see Figure 8.3.4.3).

SCATY




38 of 110

Figure 8.3.4.3: EasyVR Inside the Head, on the bottom side of the photo

8.3.5 Main MCU

A Teensy 3.2 was chosen to be the main MCU because of its small size, low cost, high processing power,

and compatibility with the Arduino IDE. We chose to design our embedded system using a task scheduler

to allow for multiple concurrent state machines.

Figure 8.3.5.1: Graph Mapping Out the Teensy 3.2 Pins and Features

SCATY




39 of 110

Teensy Device

Teensy Device Protocol

Teensy Device Protocol

GND GND

0

13 Bluetooth - SCK SCK

Vin

1 Dynamixel TX1

14 Accelerometer - x

AGND

2 Bluetooth - REQ

15 Accelerometer - y

3.3V 3.3V

3 Bluetooth - RDY

16 Accelerometer- z

4 Bluetooth - RST

17 TFT - DC

5 TFT - CS

18 I2C- Servo (SDA) SDA

6 HC-SR04-Echo

19 I2C- Servo (SCL) SCL

7 R-Pi RX3

20 HC-SR04-Trigger

8 R-Pi TX3

21 MOTOR EN

9 EasyVR RX2

22

10 EasyVR TX2

23 Current Status

11 Bluetooth - MOSI MOSI

12 Bluetooth - MISO MISO

Table 8.3.5.2: Pin Connection to Modules

The boot-up process allows for setting up all peripherals and initiate interactions between the

user and the robot (see Figure 8.3.5.3).

Figure 8.3.5.3: Flow Chart for boot-up

SCATY




40 of 110

8.3.6 Wireless Transceiver

The Adafruit Bluefruit LE module featured the Nordic nRF8001 for BLE. The module communicated

with the main MCU via SPI and simulated a wireless UART bridge with one of our controllers.

8.4 Mechanical

The mechanical block was composed by the lower body, using Dynamixel XL-320 smart servo motors,

and the upper body, using SG90 servo motors for the arms and head.

Most of the robot chassist was 3D printed with red PLA plastic.

8.4.1 Lower Body

The lower body chassis was connected to Dynamixel XL-320 smart servo motors. The Dynamixel XL-

320 used a one-wire TTL communication protocol and were able to be daisy-chained. We followed the

following video for the assembly of the lower half of the body: https://youtu.be/iOHySW-_j9Q.

Prior to the assembly of the lower body, the motors had to be configured. We first set the motor ID to a

value between 1-253 and the baud rate of the motors would be configured to 9600. Next step was to pow-

er cycle the motors and then re-configure the baud rate to 115200. (Refer to the library of the Dynamixel

XL-320 motors for precise instructions)

8.4.2 Upper Body

The motorized upper body of the robot consisted of the arms and the head. Inexpensive SG90 servo mo-

tors moved the arms and head.

8.5 Aesthetic

Some non-essential features were implemented to improve the appearance of the robot (see Figure 8.5.1).

Figure 8.5.1: Head of the Robot without Facial Expressions

SCATY




41 of 110

8.5.1 Face

A 2.2” TFT color display represented the face of the robot, being able to output emotions and other data

with facial expressions.

SCATY




42 of 110

9 User Interface Design

The project consists of a test GUI, Voice Commands and Bluetooth Commands that allows the user to

send commands and control the activities of the robot.

9.1 Application Control

The mobile control sends commands to the robot via Bluetooth in which the robot will execute commands

based on what data is sent to it. The GUI allows the user to control the joints which helps with developing

the code for the movements of the robot. The voice command will be processed by the EasyVR in which

the microphone picked up the commands and communicate with the Teesny to execute the command.

9.2 Interface

9.2.1 Interface 1: Mobile Bluetooth Application

After running the Bluetooth the user is prompted to select available Bluetooth devices, in which “John”

will be selected to connect to. After connecting to the application, the user can either select a button to

program a code to be later sent to the robot or press the button to send the pre-set commands to the robot.

9.2.2 Interface 2: GUI

The robot must be connected to a computer via a USB cable prior to running the processing script. Once

connected the GUI can be started and a new window will pop-up and have several slider bars. Each slider

bars is used to control a joint on the robot. Sliding the bar will move the joint in real time and the position

value is displayed on the GUI.

9.2.3 Interface 3: Voice Recognition

The EasyVR module is actively listening for its name “John”, in which the robot would respond by turn-

ing its motor’s LED to blue and then start listening for a command. One it detects a correct command, the

robot will then execute the command change the LED to the corresponding LED to notify that it has suc-

cessfully received and executed the command.

9.3 Development system and components available

The mobile application was a generic application downloaded off the smart phone's application market

which allows the user to pre-program code that can be sent to the robot with a touch of a button. The GUI

interface is programmed using processing which allows the user to control individual joints of the robot

using a slider. The voice commands was developed using the EasyVR module with the EasyVR com-

mander, in which the words were pre-trained to be able to recognize specific commands spoken to it.

SCATY




43 of 110

10 Experiment Design and Test Plan

10.1 Design of Experiments

10.1.1 Standing on its own

1. The objective of the experiment is to test whether the robot can stand upright on its own with no

external help. It tested the Dynamixel XL-320 motors and the design of the frame. This measure

the motor’s torque and its ability to withstand the weight of the robot.

2. To set up the experiment, we programmed the Teensy to send an initial position to all motors and

checked to see if the robot was able to stand on its own.

3. The procedure was to turn on the robot and lay it on its feet and see if it stood on its own.

4. The expected result of this experiment was having the robot stand on its own after the initial

power up.

Stanley and Alberto worked on the experiment

10.1.2 Experiment 2: Walking

1. The object of the experiment was validating the robot’s capability to walk. It tested the design

constraints of the robot’s weight and whether the motors could handle it. This measured the mo-

tor’s torque and its ability to withstand the weight of the robot.

2. The setup was to pre-program the walking movement to the robot

3. The procedure was to start the robot and send a command. If the robot walked successfully, the

experiment will have passed.

4. The expected result of the experiment was allowing the robot to walk successfully without falling

down.


10.1.3 Experiment 3: Robot Movements

1. The purpose of the experiment was to test the degrees of freedom of the robot. It tested both the

Dynamixel XL-320 and SG90 motors and their ability to move the robot’s frame. This measured

the degrees of freedom of the robot in which there were ten for the lower body, six for the arms,

and two for the head.

2. The setup was to move each motors individually to test the degree of freedom of the robot.

3. The procedure of this experiment was to demonstrate each degree of freedom by holding the ro-

bot and moving each individual joint. If they all moved, then the experiment passed.

4. The expected result of the experiment allowed the robot to demonstrate all degree of freedom by

moving each individual joints


SCATY




44 of 110

10.1.4 Experiment 4: Manual Control via Bluetooth

1. The objective of the experiment was to test the manual control of the robot via Bluetooth. It tested

the wireless communication from both the ground control and a smart device to the robot.

2. The set up was to connect a smart device or the ground station to the Bluetooth module and send

a command to it.

3. The procedure of this experiment was to connect the robot to the ground station and a smart de-

vice and send commands to the robot. If the robot received the commands, the experiment is suc-

cessful.

4. The expected result of the experiment allowed communication from a ground station and/or a

smart device to the robot through Bluetooth.


10.1.5 Experiment 5: Visual Object Recognition

1. The objective of this experiment was to test the ability to recognize objects. It tested the Raspber-

ry Pi camera, the Python script, and the robot’s ability to recognize specific colors with no-

predefined backgrounds.

2. The setup was to let the Raspberry Pi camera scan objects in front of the robot.

3. The procedure of this experiment was to place colored object in front of the robot to let it scan it.

When it detects the object’s color, the motor’s LED will change to the detected color.

4. The expected result of the experiment would be that the robot can detect specific colored objects.

Alberto worked on the experiment

10.1.6 Experiment 6: Object Avoidance

1. The objective of this experiment was to test the ability to avoid objects. It tested the HC-SR04,

our algorithm, and the robot’s ability to detect the distance of any object in front of it between 4

cm to 100 cm.

2. The setup was to place an obstruction in front of the robot and see if the robot would maneuver

around it.

3. The procedure of this experiment was to let the robot walk and have an obstruction in front of it.

If the robot stopped and moved out of the way, the experiment passed.

4. The expected result of the experiment would be that the robot can maneuver around any objects

that is detected by the HC-SR04

Stanley worked on the experiment

10.1.7 Experiment 7: Text-to-Speech

1. The objective of this experiment was to test the ability of the robot to speak base on certain given

text. It tested the speakers of the robot, the Festival TTS software installation, the Bluetooth

communication between a controller and the main MCU, and the wired UART communication

between the Raspberry Pi 2 and the main MCU.

SCATY




45 of 110

2. The setup was to have the robot connected to either the ground station or smart device and re-

ceive messages from it.

3. The procedure was to have the Bluetooth receive messages from a smart device or ground station

and then the robot will say the received message. If it correctly said the message, the experiment

passed.

4. The expected result of the experiment would be that the robot can receive text and speak the

phrase it received.


10.1.8 Experiment 8: Voice Commands

1. The objective of this experiment was to test the ability to recognize voice commands. It tested the

EasyVR module and how it recognizes which command it is spoken.

2. The setup was to have the EasyVR initiated and listening to incoming commands in a quiet room.

3. The procedure was to say the command to the microphone and if the robot does the command

stated, the experiment passed.

4. The expected result of the experiment would be that the robot successfully recognizes voice

commands and executes the actions.

Stanley worked on the experiment

10.1.9 Experiment 9: Face Detection

1. The objective of this experiment was to test the ability to detect faces. It tested the Raspberry Pi

camera, the Python script, and the robot’s capability to detect faces.

2. The setup was to have the Raspberry Pi camera scan for faces and to place a face in front of the

camera.

3. The procedure was to place a face in the field of vision of the Raspberry Pi camera. If the camera

detected a face, the expression on the TFT will change to “happy” and the experiment will pass.

4. The expected result of the experiment would be that the robot successfully detects faces.


10.1.10 Experiment 10: Sleep

1. The objective of this experiment was to test the ability of the robot to go to sleep mode. It tested

the capability of the relay to shut off the power to the Dynamixel XL-320 Motors and reduce the

power consumption of the robot.

2. The setup of this experiment was to test the relay that is connected to the motors

3. The procedure was to say or send “sleep” to robot and the robot will go to a resting position and

the relay will shut off the motors. If the motors are off and the current reading is lower, the exper-

iment is successful.

4. The expected result would be that the robot successfully shuts down its motors to conserve power

and that the current reading shows lower power consumption.


SCATY




46 of 110

10.2 Bug Tracking

Logs from Lab Notebook for 02/23/16:

Saturday (02/20/16)

System’s Test

Fails:

BLE: SPI issues

Servo Board

EASY VR

Sunday (02/21/16)

Servo Board works, but the 6V voltage regulator is only rated to up to 2.5 A. Each servo motor stall cur-

rent is around 0.6A, so it can only handle 4 motors = 2.4 A

Solution: Only connect shoulder (2) and head (2) servo motors

Monday (02/22/16)

BLE SPI issue solved after initiating the TFT SPI pins.

EASY VR. After attempting to troubleshoot it separately, once we connected it back to the system, it

worked normally.

Issues

Dynamixel XL-320 motors. Loss of communication after a few commands were sent.

Analyzed power, cables, and signals.

Diagnosis: The MOSFET Enable setup is not providing enough current. Results of extended study:

1K resistor, yields less than 0.36 A

180 resistor, yields less than 0.6 A



Suggested solution: Using a relay

10.3 Quality Control

We only experienced issues with the EasyVR after full system implementation:.

Log of Failures of EasyVR on 02/26/16::

EasyVR stops functioning after 15 seconds

SCATY




47 of 110





Diagnosis: Exited state machine on unexpected state

Solution 02/28/16: Increase period for state machine accounting for Worst Case Execution time

10.4 Performance bounds

Initializing the Raspberry Pi 2 requires special timing considerations since it’s an Operating System that

requires loading several drivers and files.

10.5 Identification of critical components

The Raspberry Pi 2 is a critical component because, during testing, if the system is shutdown suddenly, it

can corrupt all the files on the system. Therefore, the Raspberry Pi 2 needs to be properly turned on and

shutdown to avoid any corruption.

10.6 Items Not Tested by the Experiments

10.6.1 Pressure Sensor

The original objective of the pressure sensor was to gather data on the weight distribution of the robot.

The sensor would have been placed on the bottom of the feet to sense the shift in weight when standing

and walking. However due to time constraint and the inability to walk, we never had the chance imple-

ment the sensor.

10.6.2 Battery Balancer

Untested design for a 2S Lithium Battery Cell Balancer using the BQ29209 (see Figure 10.6.2.1 and Fig-

ure 10.6.2.2).

SCATY




48 of 110

Figure 10.6.2.1: Schematic for 2S Lithium Battery Balancer

Figure 10.6.2.2: Board Layout for 2S Lithium Battery Balancer

SCATY




49 of 110

11 Experimental Results and Test Report

11.1 Test Iteration 1: Stabilization/Standing

11.1.1 Test 1

Stanley and Alberto performed the experiment.

1. The result of the test showed that the lower half of the robot was able to stand on its own

without any power.

2. The actual result of the rest exceeded the expected result.

3. We expected the robot to stand on its own with the motors on and our result exceeded our

expectation by supporting its own weight with the motors off, however more test was

needed to determine if the robot would have been able to stand with the upper half as-

sembled.

4. The robot was able to stand on its own with only the lower half of the body assembled, so

the next test was to fully assemble the robot and retest the capability to stand on its own

again

11.1.2 Test 2

1. The result of this test showed us that the robot was capable of standing with the upper

half of the body assembled.

2. We expected the robot to stand on its own and the actual result proved that the robot was

able to do so.

3. Although the test showed that the robot was able to stand on its own, it was not stable so

corrective actions needed to be taken.

4. To make the standing more stable, we decided to implement PID control and tune it for

more stability.

11.1.3 Test 3

1. The result of the test showed that the robot had more stability while standing.

2. The actual result of the test is what was expected.

3. After the implementing PID and tuning it, the robot was able to stand with more stability.

4. No further corrective actions was needed as the result was what was expected.

11.2 Test Iteration 2: Walking

Stanley and Alberto performed the experiment

11.2.1 Test 1

1. The result of the test showed that the lower half of the robot was capable of walking for-

ward and scooting left and right.

2. The tested result matched up with the expected result of the robot’s capability to walk

SCATY




50 of 110

3. The robot was able to walk, however this was only with the lower half. We need to as-

semble the top half of the robot and retest.

4. As stated in the result analysis, the robot’s walking needed to be retested with the assem-

bly of the upper half of the body.

11.2.2 Test 2

1. The result of the fully assembled robot’s walking was a failure.

2. The tested result did not match with the expected result as the robot was unable to walk.

3. We believe that the robot was unable to walk because of the uneven weight distribution

where the top half was significant heavier than the lower half.

4. For a corrective action, PID control and tuning was implemented.

11.2.3 Test 3

1. The result of the test showed that the robot was still unable to walk even with the imple-

mentation of PID.

2. The tested result did not match with the expected result as the robot was unable to walk.

3. We believed the robot was unable to walk because of the big margin of error the plastic

frame had. The plastic was capable of moving a couple millimeter while the motors were

not moving, which threw off the entire balance of the entire system.

4. A corrective actions that we considered was to redesign the entire system in which we

would first create a dynamic model of the robot.

11.3 Test Iteration 3: Robot Movements


11.3.1 Test 1

1. The result of the test showed the robot capability of moving the head, arms, and legs, and

show the 18 degrees of freedom.

2. The actual result showed that the robot was capable of moving each joints, but only two

joints on the arms were capable to move simultaneously.

3. We were able to determine that the joints on the arm was only capable of moving only

two out of the six joints, so corrective actions needed to be taken.

4. Given the result, we plan to add an additional 6v regulator to see if this would rectify our

problem.

11.4 Test Iteration 4: Manuel Control via Bluetooth


11.4.1 Test 1

1. The result of the result showed the robot capability to communicate with a smart device, in

which the Bluetooth module was able to communicate to the Teensy through SPI communication.

SCATY




51 of 110

2. The actual result showed the robot’s ability to receive data from a smart device through Bluetooth

which matched with our expected result.

3. We were able to communicate to the robot within a ten feet radius and send commands to it with

little to no delay.

4. Given the result, no corrective actions were needed to be taken.

11.5 Test Iteration 5: Visual Object Recognition

11.5.1 Test 1


1. The result of the test was that the robot was capable of detecting green and red objects and

changed eyes colors based on which color was detected.

2. Our tested result of detecting colored object was as described in our expected result.

3. The robot was able to detect the green and red makers we placed in front of it and changed its

eyes based on the color, however, the robot will change its eye color on any detection of green

and red in the background.

4. The robot was capable of detecting the colored objects of our choice so no corrective actions was

needed.

11.6 Test Iteration 6: Text-to-Speech

Alberto performed the experiment

11.6.1 Test 1

1. The result of the test showed that the robot was capable of “speaking” the English test we

sent to it.

2. The tested result matched up with the expected result as the robot was capability of play-

ing the text we sent it through the speaker.

3. The robot voice is a bit soft, however that can be fixed with a stronger speaker, but the

text-to-speech software worked well with no issues.

4. The end result was successful as the robot was able to speak English text we sent it, so no

need to take any corrective actions

11.7 Test Iteration 7: Voice Commands

Stanley performed the experiment

11.7.1 Test 1

1. The test resulted to the EasyVR detecting the voice and run the function corresponding to

the command received. The robot was able to successfully recognize some words, but

had troubles with others.

SCATY




52 of 110

2. We expected the robot to recognize the commands spoken to it and it was able to recog-

nize most words with no problem.

3. We noticed that the more complex phrases gave the recognition software some trouble

due to the fact that the EasyVR Commander limits the training to two per word.

4. To further develop the recognition, we decided to change the name from “Scaty” to

“John” for the simplicity and add more words to the database.

11.7.2 Test 2

1. The resulted test showed the EasyVR working and responding to the name “John” and

the new phrases that were added to the data set.

2. The tested result matched well with our expected result.

3. After retraining the robot to “John”, the success rate of the robot’s response was greatly

increased.

4. With the robot capability of recognizing speech, no corrective actions was further neces-

sary.

11.8 Test Iteration 8: Face Detection

Alberto performed the experiment

11.8.1 Test 1

1. The test was to determine if the Raspberry Pi camera, Python script, and the robot’s ca-

pability to detect any faces and if it did, the TFT display would change the robot’s reac-

tion.

2. The tested result matched up with our expected result with the capability of detecting a

face in front of the camera.

3. When a face was in the vicinity of the camera, the robot’s expression on the TFT display

changed.

4. Since the camera software successfully detected faces, no further testing was required.

11.9 Test Iteration 9: Battery Charging


Data from Lab Notebook:

Initial testing for the 2S Lithium Charger custom board yielded negative results. The custom circuit was not charging the battery,

and there seemed to be some irregularities on voltages on the output side. Hence, I decided to set that part of the project aside

and focused on the other components of the project.

The setup of the test included a 9V 1A wall adapter power supply, a 2.1 mm jack adapter, wires, the 2S Lithium Charger custom

board - assembled, a JST connector, and a 1000 mAH 2S Lithium battery. I used a regular multimeter to measure input and

output voltages.

Assuming all settings were correct, I set the battery to charge for 10 minutes. At the end, I measured the status of the battery and

compared it with its initial status, and I saw no changes.

SCATY




53 of 110

Debugging and New Test

I checked the polarity of the 2.1 mm jack adapter, the wire connections, and the 2S Lithium Charger custom board circuitry and

soldered components. They all seemed to be correct. The 9V 1A power supply was supplying specifically 9.25 V, which is within

the input voltage operating range of the MCP73842. To verify that the charger system was supplying current to the battery, I

connected a multimeter in-line to the output of the 2S Lithium Charger and the battery to measure the charging current. When I

plugged in the 9V power supply, the current readings were around 0.09 A and then would spike to around 0.5 A. It seemed like a

very slow PWM, so I double-checked with the datasheet to look for this kind of behavior, but found nothing related to it. Hence, I

assumed it was an irregular behavior. The expected behavior should have been a constant current of 1 A or a slowly decreasing

current, depending on the charging stage.

I assumed that the issue was the power supply because the 9V 1A wall adapter power supply was barely within specifications for

this task. I switched it for a 12V 1.5A wall adapter power supply. Once I changed it and plugged it in, I immediately got a solid

0.81 A reading on the multimeter, meaning that the charger system was indeed charging the battery. I stopped the process and

took some measurements before restarting the test.

Test

Time = 0 min

Vinput = 12.12 V

Voutput = 8.36 V

Battery Status:

Vdd = 7.683 V (47%)

V1s = 3.837 V

V2s = 3.846 V

Vdelta = 0.009 V

Procedure: Connect system

Iout = 0.81 A

Vinput = 12.09 V

Vout = 8.36 V

Time = 5 min

Iout = 0.60 A

Procedure: Unplug system

Battery Status:

Vdd = 7.773 V (58%)

V1s = 3.882 V

V2s = 3.891 V

Vdelta = 0.009 V

SCATY




54 of 110

Observations:

The 2S Lithium Charger custom board was very hot. At first, I observed some fumes from the board but further investigation

yielded that it might have been caused by flux residue. By studying the datasheet for the MCP73842, the Thermal Considerations

sections on page 16 explains that the power dissipation = (Vddmax - Vpthmin) x Iregmax. In this test, power dissipation could be

approximated to (12 - 7.7) x (0.8) = 3.44 W. The P-MOSFET NDS8434 component package is rated to dissipate up to 2.5 W,

when the PCB layout aids with heat dissipation. However, the current design of the 2S Lithium Charger custom board should

allow for only up to 1 W of safe heat dissipation.

Time = 7 min = 5 min (charging) + 2 min (rest)

Battery Status:

Vdd = 7.751 V (56%)

V1s = 3.869 V

V2s = 3.882 V

Vdelta = 0.013 V

Observations:

The 2S Lithium Charger system was set to charge the battery at 1 Ah, so it would take 1 hour to fully charge the 1000 mAh 2S

Lithium battery used in this test. After 5 min, the battery was charged 9%, which approximately matches the expected result.

Conclusion

This test successfully shows that the 2S Lithium Charger custom board can charge a 2S Lithium battery in approximately 1-hour;

however, due to thermal considerations, this iteration of the prototype would not provide a safe solution for an on-board charg-

ing system.

SCATY




55 of 110

12 Conclusion and Future Work

12.1 Conclusion

The conclusion of our product was satisfying, but there are still several features that can be added

onto the robot. Overall, we met the majority of goals set for verification and validation. Some technical

objectives we had included combining many branches of electrical engineering to create our final project.

We implemented state machines, a task scheduler, power management, computer vision, text-to-speech,

voice recognition, and many communication protocols that allowed many peripherals to work together, as

well as Bluetooth to allow control through a smart device.

Despite completing the majority of our goals, we were unable to get the walking to work. The

robot was very top heavy and did not have a stable center of mass. On top of that, the 3D printed frame

had a flexibility that added instability to the body that caused the frame to behave in an unpredictable

manner. We found a possible solution towards the end of the project, which was to create a dynamic

model for our robot so we would know how heavy each area was and by having the model, it would be

easy to determine the center of mass. Another possible solution to the failure of walking is to have the

frame of the body professionally manufactured. Our frame was printed by the local IEEE chapter with

their Makerbot 3D printer which did not provide a high resolution print. Since we were unable to get

walking to function, we were not able to test the object avoidance algorithm that we had wrote. Once the

walking feature is available, testing and implementing the object avoidance feature would be very easy.

Although we did not have the feature of walking, we had a lot of peripherals and protocols im-

plemented on the robot. Our robot was still capable of moving its joints, recognize voices, colors, and

faces, have an interactive screen that displayed multiple different emotions for the robot, text-to-speech,

and have Bluetooth communication. We were able to speak to the robot to have it execute the commands

or we were able to connect to it via Bluetooth and send commands for it execute them.

The tasks Stanley was responsible for was the assembly of the robot, PID control and tuning,

voice recognition, power management, and Bluetooth. Alberto was responsible for the 3D printing of the

robot, the GUI, nRF Toolbox App, computer vision, text-to-speech, and battery management. Lastly the

both of us were responsible for the upper body control on top of the wire management and overall coding.

Stanley: This project I learned about many different branches of electrical engineering. I was able

to further develop my embedded system knowledge and skill with the implementation of all the different

communication protocols on top of the integration of multiple peripherals. Most of the tasks I was respon-

sible were either taught in theory or not implemented hands on in lab or was not taught in school. This

project taught me the importance of research and the importance of pre-planning everything before exe-

cuting it. For example, I only learned about PID control and tuning through theory in my courses, howev-

er it was my first time implementing on a project. It showed me how big of a difference from learning

about a theory and then actually applying it which led to many complications and trail-and-errors. On top

of learning new things, I also learned when to stop and find an alternative solution. For the goal of creat-

ing a low budget project, I initially tried to use a software library for voice recognition with a small ampli-

fier and microphone however it prove to be really inconsistent. Alberto and I then tried to work together

to get it to function, however with all the inconsistent result we decided to halt the process and look for a

better solution which led to the purchase of the EasyVR module. For professional growth, this project

allowed me a small glimpse as to what I would be doing as a real engineering in the industry. This project

taught me many technical skills and showed me that I knowledge is never ending and that I always have

something new to improve or learn.

SCATY




56 of 110

Alberto: During this project, I learned many skills and knowledge from tackling a wide variety of

technical challenges, understood the design and development process, and improved my soft skills while

working on a team. This robot introduced several technical challenges that I hadn’t encountered before.

Interfacing several peripherals together showed me the advantages of concurrent state machine design.

While exploring the possible implementations for the features we wanted on the robot, I came up with

several solutions that seemed very simple or complex in appearance but, in reality, required comparing

several factors, such as price, size, power, quality, technical complexity, development time, and others.

For example, when implementing Text-to-Speech, I came up with several solutions that produced very

low-quality results. Hence, I had to discard these solutions in spite of how much time I spent on them

because they did not meet our project criteria. Even though we wanted good quality, I had to set ourselves

to a medium quality solution with the Raspberry Pi 2 and the Festival TTS solution because better quality

solutions would require much more on price, size, and development time.

I have worked in several projects before starting this one, but this project has been one of the

most well planned projects I have worked in because of the design and development planning we set since

the beginning of the project. Even though we had some challenges on the timeline, we were expecting

them because they are part of the nature of research and development. All the same, setting good specifi-

cations at the beginning of the project allowed us to have a clear goal and improved our ability to plan our

timeline appropriately.

Even though we were only a team of 2 members, I improved my communication skills, documen-

tation writing, meeting planning, and teamwork ability. I managed to work and coordinate working ses-

sions with my partner despite our busy schedules. Since we had limited team working sessions, our doc-

umentation writing and coordination were essential to make sure we completed the project, so I feel we

were able to work together very well due to our great soft skills and teamwork.

12.2 Future Work

For our future work, the first step is to accomplish the goals that we weren’t able to complete. We

believe that in order to accomplish walking, a dynamical model would first need to be created before

physically building the robot. After getting the walking to work, the next step would be implementing the

object avoidance algorithm with the HSR-04 Ultra Sonic Distance senor.

After accomplishing all our goals, the next step would be to redesign the wire management. Cur-

rently our robot has a vast amount of wires running inside it which makes it very hard to disconnect and

connect components. A solution to that would be designing a printed circuit board for the entire system.

This would not only minimize the amount of wires in the robot, but it would lessen the weight of the en-

tire robot which would help with the balance and walking of the robot.

12.3 Acknowledgement

Dr Roman Chomko

o Faculty advisor

Dr. Anastasios Mourikis

o Dynamic Modeling

Joshua Yuen, UCR CS Ph.D. Graduate Candidate

o 1-wire TTL TX/RX Signal Isolation for Dynamixel XL-320 motors

Pavle Kirilov

o PCB Manufacturing

Rex Lu, UCLA ME Master Graduate Candidate

o Walking trajectory for bipedal robots

SCATY




57 of 110

13 References

Arduino Libraries:

Teensyduino Add-On https://www.pjrc.com/teensy/td_download.html

Adafruit BLE https://github.com/adafruit/Adafruit_nRF8001

TFT Display https://github.com/adafruit/Adafruit-GFX-Library

https://github.com/adafruit/Adafruit_ILI9341/blob/master/Adafruit_ILI9341.h

Adafruit PWM Motor

Driver https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library

PID https://github.com/br3ttb/Arduino-PID-Library/

Task Scheduler https://github.com/arkhipenko/TaskScheduler

Dynamixel Motor https://github.com/hackerspace-adelaide/XL320

EasyVR https://github.com/RoboTech-srl/EasyVR-Arduino

Raspberry Pi 2. Python 2.7 Packages:

OpenCV 3.0.0 http://www.pyimagesearch.com/2015/10/26/how-to-install-opencv-3-on-raspbian-

jessie/

Python Pi Cam-

era https://www.raspberrypi.org/documentation/usage/camera/python/README.md

Numpy https://github.com/numpy/numpy

Processing 2.2.1 Libraries:

Serial https://processing.org/reference/libraries/serial/index.html

G4P http://www.lagers.org.uk/g4p/

https://www.pjrc.com/teensy/td_download.html

https://github.com/adafruit/Adafruit_nRF8001

https://github.com/adafruit/Adafruit-GFX-Library

https://github.com/adafruit/Adafruit_ILI9341/blob/master/Adafruit_ILI9341.h

https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library

https://github.com/br3ttb/Arduino-PID-Library/

https://github.com/arkhipenko/TaskScheduler

https://github.com/hackerspace-adelaide/XL320

https://github.com/RoboTech-srl/EasyVR-Arduino

http://www.pyimagesearch.com/2015/10/26/how-to-install-opencv-3-on-raspbian-jessie/

http://www.pyimagesearch.com/2015/10/26/how-to-install-opencv-3-on-raspbian-jessie/

https://www.raspberrypi.org/documentation/usage/camera/python/README.md

https://github.com/numpy/numpy

https://processing.org/reference/libraries/serial/index.html

http://www.lagers.org.uk/g4p/

SCATY




58 of 110

14 Appendices

14.1 Appendix A: Parts List

Qty Item Link

10 Dynamixel XL-

320

http://www.robotis.us/dynamixel-xl-320/

1 ROBOTIS Rivet

Set

http://www.robotis.us/robotis-mini-rivet-set/

1 ROBOTIS Screw

Set

http://www.robotis.us/robotis-mini-screw-set/

1 EasyVR 3.0 http://www.robotshop.com/en/easyvr-speech-recognition-module-30.html

1 Teensy 3.2 http://pjrc.com/store/teensy32.html

1 Red PLA 1.75 mm http://smile.amazon.com/gp/product/B00J0GO8I0?psc=1&redirect=true&ref_=oh_aui_detailpage_o06_s00

1 Raspberry Pi 2

Model B

http://www.mcmelectronics.com/product/83-16530

1 Raspberry Pi

Camera

http://www.ebay.com/itm/SainSmart-Camera-Module-5MP-Webcam-Video-1080p-720p-for-Raspberry-Pi-US-

/301809460370?hash=item46453ee492:g:znEAAOSw2XFUiWr1

6 Micro Servo Mo-

tors SG90

http://www.ebay.com/itm/10-pcs-x-Genuine-SG90-Mini-Micro-9g-Servo-For-RC-Heicopter-Airplane-Car-Boat-USA-

/301802571470?hash=item4644d5c6ce:g:HwYAAOSwnLdWsGhI

2 LiPo 7.4V 2S

1000mAh

http://www.hobbyking.com/hobbyking/store/__20841__Turnigy_1000mAh_2S_20C_Lipo_Pack_US_Warehouse_.html

1 TFT 2.2" Display

320x240

http://pjrc.com/store/display_ili9341.html

2 6V 2.5A Step-

Down Volt. Reg.

https://www.pololu.com/product/2859

1 5V 1A Steo

Down/Up Volt.

Reg.


1 3.3V 1A Step

Down/Up Volt.

Reg.


1 PCA9685 LED

Driver IC

http://www.digikey.com/product-detail/en/PCA9685PW%2FQ900,118/568-5931-1-ND/2531218

1 HC-SR04 Ultra-

sonic Sensor

http://www.ebay.com/itm/New-Arduino-Ultrasonic-Module-HC-SR04-Distance-Sensor-Measuring-Transducer-5Y-

/281787551911?hash=item419bd8d0a7:g:u3YAAOSwoydWoF4m

1 Sparkfun Mono

Audio Amp

https://www.sparkfun.com/products/11044

1 3.5mm Connector

Breakout Board

http://www.ebay.com/itm/1Pc-TRRS-3-5mm-Jack-Breakout-Audio-Stereo-Headphon-Microphone-Interface-Module-

/281842952347?hash=item419f26289b:g:cnoAAOSwo0JWMxCs

1 Current Sensor

ACS711EX -31A

to 31A


http://www.robotis.us/dynamixel-xl-320/

http://www.robotis.us/robotis-mini-rivet-set/

http://www.robotis.us/robotis-mini-screw-set/

http://www.robotshop.com/en/easyvr-speech-recognition-module-30.html

http://pjrc.com/store/teensy32.html

http://smile.amazon.com/gp/product/B00J0GO8I0?psc=1&redirect=true&ref_=oh_aui_detailpage_o06_s00

http://www.mcmelectronics.com/product/83-16530

http://www.ebay.com/itm/SainSmart-Camera-Module-5MP-Webcam-Video-1080p-720p-for-Raspberry-Pi-US-/301809460370?hash=item46453ee492:g:znEAAOSw2XFUiWr1

http://www.ebay.com/itm/SainSmart-Camera-Module-5MP-Webcam-Video-1080p-720p-for-Raspberry-Pi-US-/301809460370?hash=item46453ee492:g:znEAAOSw2XFUiWr1

http://www.ebay.com/itm/10-pcs-x-Genuine-SG90-Mini-Micro-9g-Servo-For-RC-Heicopter-Airplane-Car-Boat-USA-/301802571470?hash=item4644d5c6ce:g:HwYAAOSwnLdWsGhI

http://www.ebay.com/itm/10-pcs-x-Genuine-SG90-Mini-Micro-9g-Servo-For-RC-Heicopter-Airplane-Car-Boat-USA-/301802571470?hash=item4644d5c6ce:g:HwYAAOSwnLdWsGhI

http://www.hobbyking.com/hobbyking/store/__20841__Turnigy_1000mAh_2S_20C_Lipo_Pack_US_Warehouse_.html

http://pjrc.com/store/display_ili9341.html




http://www.digikey.com/product-detail/en/PCA9685PW%2FQ900,118/568-5931-1-ND/2531218

http://www.ebay.com/itm/New-Arduino-Ultrasonic-Module-HC-SR04-Distance-Sensor-Measuring-Transducer-5Y-/281787551911?hash=item419bd8d0a7:g:u3YAAOSwoydWoF4m

http://www.ebay.com/itm/New-Arduino-Ultrasonic-Module-HC-SR04-Distance-Sensor-Measuring-Transducer-5Y-/281787551911?hash=item419bd8d0a7:g:u3YAAOSwoydWoF4m


http://www.ebay.com/itm/1Pc-TRRS-3-5mm-Jack-Breakout-Audio-Stereo-Headphon-Microphone-Interface-Module-/281842952347?hash=item419f26289b:g:cnoAAOSwo0JWMxCs

http://www.ebay.com/itm/1Pc-TRRS-3-5mm-Jack-Breakout-Audio-Stereo-Headphon-Microphone-Interface-Module-/281842952347?hash=item419f26289b:g:cnoAAOSwo0JWMxCs


SCATY




59 of 110

1 Bluefruit LE -

nRF8001

https://www.adafruit.com/products/1697

1 3-axis Accelerom-

eter - ADXL337


1 Relay Board - srd-

05vdc-sl-c

http://www.ebay.com/itm/5V-One-1-Channel-Relay-Module-Board-Shield-For-PIC-AVR-DSP-ARM-MCU-Arduino-

/310566336050?hash=item484f323632:g:iNkAAMXQHU1RyAzD

Miscellaneous

Electronics

http://www.digikey.com/

https://docs.google.com/spreadsheets/d/1tMJ0hoYyYGIp8_53aR1vJRralnWVMpR6S2_LSVPcbkI/edit#g

id=0

14.2 Appendix B: Hardware Tools

3D Printer: Makerbot Replicator 2

Soldering Station: Weller WES51 and Weller WLC100

Logic Analyzer: Saleae Logic 8

Oscilloscope: Agilent DSO3102A

Power Supply: HP E3630A, MPJA 9615PS

Multimeter: HP 34401A, Triplett 1101-B

Laptop: Lenovo Thinkpad T450s (Windows 8.1)

Battery Charger: iMAX B6 - Battery Balance Charger

Battery Checker: Hobbyking Cellmaster 7

14.3 Appendix C: Software List

Slicer: MakerBot Desktop

3D CAD Modeling Software: SolidWorks

Software Development: Arduino IDE, Processing IDE, Python IDE

Android App: Bluefruit LE, nRF Toolbox

https://www.adafruit.com/products/1697


http://www.ebay.com/itm/5V-One-1-Channel-Relay-Module-Board-Shield-For-PIC-AVR-DSP-ARM-MCU-Arduino-/310566336050?hash=item484f323632:g:iNkAAMXQHU1RyAzD

http://www.ebay.com/itm/5V-One-1-Channel-Relay-Module-Board-Shield-For-PIC-AVR-DSP-ARM-MCU-Arduino-/310566336050?hash=item484f323632:g:iNkAAMXQHU1RyAzD

http://www.digikey.com/

https://docs.google.com/spreadsheets/d/1tMJ0hoYyYGIp8_53aR1vJRralnWVMpR6S2_LSVPcbkI/edit#gid=0

https://docs.google.com/spreadsheets/d/1tMJ0hoYyYGIp8_53aR1vJRralnWVMpR6S2_LSVPcbkI/edit#gid=0

SCATY




60 of 110

14.4 Appendix D: Arduino Code Files:

14.4.1 SCATY_Humanoid_Robot_John_1.ino

/**

* Humanoid Robot

* UCR. EE 175 - Senior Design

* Authors: Alberto Tam Yong and Stanley Chang

*

* First Release

* version 1.0

* Date:02/29/16

* Time: 11:30 PM

*/

//**** Library, Objects, and Global Variable Declarations *****

/*

* Accelerometer

* BLE

* Ultrasonic

* TFT Display

* I2C Servo Board

* EasyVR

* Dynamixel XL-320

* PID

* Face

* RPi

* Function Prototypes

* Task Scheduler

*/

//******************* Accelerometer ********************

int scale = 3; // 3 (Â±3g) for ADXL337

int rawX, rawY, rawZ;

//********************** BLE ***************************

#include <SPI.h>

#include "Adafruit_BLE_UART.h"

#define ADAFRUITBLE_REQ 2

#define ADAFRUITBLE_RDY 3

#define ADAFRUITBLE_RST 4

Adafruit_BLE_UART BTLEserial = Adafruit_BLE_UART(ADAFRUITBLE_REQ, ADAFRUITBLE_RDY,

ADAFRUITBLE_RST);

boolean BLE_command_in = 0;

//******************** UltraSonic **********************

#define trigPin 20

#define echoPin 6

long distance = 0;

SCATY




61 of 110

//******************* TFT Display ***********************

#include "Adafruit_GFX.h"

#include "Adafruit_ILI9341.h"

//SPI already included

#define TFT_DC 17

#define TFT_CS 5

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

//******************* I2C Servo Board ************************

#include <Wire.h>

#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x5C); //address set to 0x5C

#define SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096)

#define SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)

char R_Shoulder = 0;

char R_Elbow = 1;

char R_Wrist = 2;

char L_Shoulder = 3;

char L_Elbow = 4;

char L_Wrist = 5;

char Neck_LR = 6;

char Neck_UD = 7;

int init_rest_arms[10]= {50,160,65,131,150,60,65,80};

int curr_arms_pos[10] = {50,160,65,131,150,60,65,80};

int target_arms_pos[10] = {50,160,65,131,150,60,65,80};

int target_arms_time[10] = {0,0,0,0,0,0,0,0,0,0};

boolean servo_sm_update_en = 0;

char motor_joint[10] = {R_Shoulder, R_Elbow, R_Wrist, L_Shoulder, L_Elbow, L_Wrist, Neck_LR, Neck_UD};

//******************** EasyVR ************************

#include "Arduino.h"

#if defined(SERIAL_PORT_USBVIRTUAL)

#define port SERIAL_PORT_HARDWARE

#define pcSerial SERIAL_PORT_USBVIRTUAL

#else

#define pcSerial SERIAL_PORT_MONITOR

#endif

#include "EasyVR.h"

EasyVR easyvr(Serial2);

//Groups and Commands

enum Groups

{

GROUP_0 = 0,

SCATY




62 of 110

GROUP_1 = 1,

};

enum Group0

{

G0_JOHN = 0,

};

enum Group1

{

G1_BOW = 0,

G1_STOP = 1,

G1_WALK = 2,

G1_RIGHT = 3,

G1_LEFT = 4,

G1_LIGHT_RED = 5,

G1_LIGHT_GREEN = 6,

G1_LIGHT_WHITE = 7,

G1_LIGHT_YELLOW = 8,

G1_LIGHT_OFF = 9,

G1_HELLO = 10,

G1_GOODBYE = 11,

G1_GOOD_NIGHT = 12,

G1_SLEEP = 13,

G1_IP = 14,

G1_DANCE = 15,

G1_MUSIC = 16,

G1_CAMERA = 17,

G1_KICK = 18,

G1_HANDS_UP = 19,

G1_RIGHT_HAND = 20,

G1_LEFT_HAND = 21,

G1_RED = 22,

G1_GREEN = 23,

G1_FIND_ME = 24,

};

int8_t group, idx;

boolean EASYVR_SM_EN = 1;

//******************** Dynamixel XL-320 ************************

#include "XL320.h"

XL320 motor;

//Used to initialize all motor speeds

//Add any motors being used and change the quantity

int servoID[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; //servoIDs

int servoID_size = 12; //manually write the quantity of servoIDs

//Initial standing position

unsigned int init_standing_pos[] = {0, 200, 550, 562, 430, 512, 512, 588, 462, 462, 512, 0, 0, 0, 0, 0};

//Current standing position // balance on right foot

unsigned int curr_standing_pos[] = {0, 200, 550, 562, 430, 512, 512, 588, 462, 462, 512, 0, 0, 0, 0, 0};

SCATY




63 of 110

//Motor limits

unsigned int xl320_lower_limit[] = {0, 0, 500, 512, 200, 200, 470, 250, 200, 200, 200};

unsigned int xl320_upper_limit[] = {0, 500, 800, 800, 750, 550, 800, 800, 512, 512, 800};

//Movement

unsigned int walk_motor_id[] = {1, 10, 1, 8, 7, 2, 1, 10};

signed int walk_move_pos[] = {0, 140, 70, -112, 188, 50, -50, -12};

unsigned int walk_delay[] = {1, 1, 25, 1, 15, 1, 1, 1};

unsigned int walk_array_size = 8;

unsigned int walkDelayVal = 40;

int mirror_prop[] = {0, -1, -1, -1, -1, 1, 1, -1, -1, -1, -1};

char incomingCommand[128]; //array to store user input on Serial Terminal

//int command_size = 3; //default command size

char rgb[] = "rgbypcwo"; //array to store the colors

char command_list[][20] = {"help", "motor", "led", "bow", "walk", "stand", "speed", "right", "left","init_rpi", "log-

in_rpi", "stop_rpi", "ip",

"shutdown_rpi", "cam_rpi","toggle_servo","debug","stop_init","head_follow", "arms_up","arms_down",

"easyvr_en",

"arm_right","arm_left","arm_rdown","arm_ldown","m", '0'};

int command_size[] = {4, 5, 3, 3, 4, 5, 5, 5, 4, 8, 9, 8, 2, 12, 7, 12, 5, 9, 11, 7, 9, 9, 9, 8, 9, 9,1, 1};

unsigned short delay_writeAll = 20;

unsigned char flag = 0;

//******** PID *********

#include <PID_v1.h>

//For x-axis at standing at the waist level

double Setpoint, Input, Output, Output2;

//For y-axis at standing at the waist level

double Setpoint_y, Input_y, Output3, Output4;

//For x-axis at standing at the feet level

double Output_sole_l, Output_sole_r;

double Kp = 8.0, Ki = 1.0, Kd = 150.0 ;

double Kp_y = 0.8, Ki_y = 0, Kd_y = 3 ;

double Kp_sole = 4.0, Ki_sole = 0.0, Kd_sole = 200.0 ;

PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

PID myPID2(&Input,&Output2, &Setpoint, Kp, Ki, Kd, REVERSE);

PID myPID3(&Input_y,&Output3, &Setpoint_y, Kp_y, Ki_y, Kd_y, DIRECT);

PID PID_sole_l(&Input, &Output_sole_l, &Setpoint, Kp_sole, Ki_sole, Kd_sole, REVERSE);

PID PID_sole_r(&Input, &Output_sole_r, &Setpoint, Kp_sole, Ki_sole, Kd_sole, DIRECT);

int xl320_PID_delta_x = 10;

boolean PID_X_EN = 1;

boolean PID_SOLE_EN = 1;

int y_PID_disable = 1;

SCATY




64 of 110

//**********************

void stand_in_right_leg();

void right_leg_PID();

boolean walk_left_sm_en = 0;

//******* Face ***************

#define FACE_NORMAL 0

#define HAPPY 1

#define SAD 2

#define EXCITED 3

#define SLEEP 4

int servo_base_pos = 65;

int servo_tilt_pos = 60;

int prevServo_base_pos = 65;

int prevServo_tilt_pos = 60;

int servo_base_pos_upper = 110;

int servo_base_pos_lower = 50;

int servo_tilt_pos_upper = 90;

int servo_tilt_pos_lower = 45;

int video_width = 320;

int video_height = 240;

int camera_x = 512;

int camera_y = 512;

int camera_h = 0;

int camera_t = 0;

int prev_camera_x = 512;

int prev_camera_y = 512;

int delta_camera_x = 0;

int delta_camera_y = 0;

int x_threshold = 5;

int y_threshold = 5;

boolean follow_camera = 0;

boolean camera_flag = 1;

boolean servo_flag = 1;

boolean debug_mode = 0;

boolean first_reading_taken = 0;

boolean activity = 1; //if activity init at 0, robot starts on sleep mode

int activity_counter = 0;

boolean change_of_eyes = 0;

boolean change_of_mouth = 0;

int mood = FACE_NORMAL;

int prev_mood = FACE_NORMAL;

int activity_interval = 200/2; // 10 seconds

int blink_counter = 0;

int blink_interval = 50/2;

SCATY




65 of 110

unsigned int count_last_time_seen = 0;

unsigned int x_proportion = video_width/20;

unsigned int y_proportion = video_height/10;

int color = 0;

int serial3_size;

char serial3_incoming[128];

boolean head_follow_camera_en = 0;

//****************************

//*********** RPi ************

boolean IMAGE_TRACKING_EN = 1;

boolean skip_rpi_init = 1;

unsigned int x, y, h, t;

unsigned int prev_t = 0;

//****************************

//********* Function Prototypes *******

void EasyVR_SM();

void BLE_loop();

void command_menu_loop();

void SM1_Tick_HeadCameraFollow();

void SM2_Tick_TFT();

void SM4_Tick_RPi();

void servo_write();

void dynamixel_PID_x();

void walk_left_step();

//****************************

//****************** Task Scheduler *********************

#include <TaskScheduler.h>

// Callback methods prototypes

void t1Callback_HeadCameraFollow();

void t2Callback_RPi();

void t3Callback_SPI();

void t4Callback_EASYVR();

void t5Callback_command();

void t6Callback_servo();

void t7Callback_PID();

void t8Callback_walkStep();

int SM1_Period = 50;



int Servo_SM_Period = 10;

//Tasks

Task t1_HeadCameraFollow(SM1_Period, TASK_FOREVER, &t1Callback_HeadCameraFollow);

Task t2_RPi(SM4_Period, TASK_FOREVER, &t2Callback_RPi);

Task t3_SPI(SM2_Period, TASK_FOREVER, &t3Callback_SPI);

SCATY




66 of 110

Task t4_EASYVR(200, TASK_FOREVER, &t4Callback_EASYVR);

Task t5_command(50, TASK_FOREVER, &t5Callback_command);

Task t6_servo(Servo_SM_Period, TASK_FOREVER, &t6Callback_servo);

Task t7_PID(50, TASK_FOREVER, &t7Callback_PID);

Task t8_walkStep(600, TASK_FOREVER, &t8Callback_walkStep);

Scheduler runner;

void t1Callback_HeadCameraFollow() {

//State Machine only runs when head_follow_camera_en is 1

SM1_Tick_HeadCameraFollow();

}

void t2Callback_RPi() {

SM4_Tick_RPi();

}

void t3Callback_SPI() {

//Cannot communicate with TFT Display and BLE Module with one hardware SPI

//Execute one at a time

SM2_Tick_TFT();

BLE_loop();

}

void t4Callback_EASYVR() {

//The EasyVR_SM() doesn't have to update very frequently

EasyVR_SM();

}

void t5Callback_command() {

command_menu_loop();

}

void t6Callback_servo() {

servo_write();

}

void t7Callback_PID() {

//PID

dynamixel_PID_x();

}

void t8Callback_walkStep() {

walk_left_step();

}

//******************************************************

void setup () {

Serial.begin(115200);

Serial.println("John - by SCATY");

//Initiate all modules

BLE_setup();

UltraS_setup();

SCATY




67 of 110

I2C_setup();

Dynamixel_setup();

RPi_setup();

TFT_setup();

EasyVR_setup();

runner.init();

if(debug_mode) Serial.println("Initialized scheduler");

runner.addTask(t1_HeadCameraFollow);

if(debug_mode) Serial.println("added t1_HeadCameraFollow");

runner.addTask(t2_RPi);

if(debug_mode) Serial.println("added t2_RPi");

runner.addTask(t3_SPI);

if(debug_mode) Serial.println("added t3_SPI");

runner.addTask(t4_EASYVR);

if(debug_mode) Serial.println("added t4_EASYVR");

runner.addTask(t5_command);

if(debug_mode) Serial.println("added t5_command");

runner.addTask(t6_servo);

if(debug_mode) Serial.println("added t6_servo");

runner.addTask(t7_PID);

if(debug_mode) Serial.println("added t7_PID");

runner.addTask(t8_walkStep);

if(debug_mode) Serial.println("added t8_walkStep");

t1_HeadCameraFollow.enable();

if(debug_mode) Serial.println("Enabled t1_HeadCameraFollow");

t2_RPi.enable();

if(debug_mode) Serial.println("Enabled t2_RPi");

t3_SPI.enable();

if(debug_mode) Serial.println("Enabled t3_SPI");

t4_EASYVR.enable();

if(debug_mode) Serial.println("Enabled t4_EASYVR");

t5_command.enable();

if(debug_mode) Serial.println("Enabled t5_command");

t6_servo.enable();

if(debug_mode) Serial.println("Enabled t6_servo");

t7_PID.enable();

if(debug_mode) Serial.println("Enabled t7_PID");

t8_walkStep.enable();

if(debug_mode) Serial.println("Enabled t8_walkStep");

}

void loop () {

runner.execute();

}

SCATY




68 of 110

14.4.2 Accelerometer.h

// Read, scale, and print accelerometer data

void accelerometer()

{

// Get raw accelerometer data for each axis

rawX = analogRead(A0);

rawY = analogRead(A1);

rawZ = analogRead(A2);

// Scale accelerometer ADC readings into common units

float scaledX, scaledY, scaledZ; // Scaled values for each axis

scaledX = mapf(rawX, 0, 1023, -scale, scale);

scaledY = mapf(rawY, 0, 1023, -scale, scale);

scaledZ = mapf(rawZ, 0, 1023, -scale, scale);

// Print out raw X,Y,Z accelerometer readings

if (debug_mode)

{

Serial.print("X: "); Serial.println(rawX);

Serial.print("Y: "); Serial.println(rawY);

Serial.print("Z: "); Serial.println(rawZ);

Serial.println();

}

/*

// Print out scaled X,Y,Z accelerometer readings

Serial.print("X: "); Serial.print(scaledX); Serial.println(" g");

Serial.print("Y: "); Serial.print(scaledY); Serial.println(" g");

Serial.print("Z: "); Serial.print(scaledZ); Serial.println(" g");

Serial.println();

delay(2); // Minimum delay of 2 milliseconds between sensor reads (500 Hz)

*/

}

// Same functionality as Arduino's standard map function, except using floats

float mapf(float x, float in_min, float in_max, float out_min, float out_max)

{

return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;

}

void curr_sensor() {

int curr_sensor_read = analogRead(23);

//Serial.println(curr_sensor_read);

}

void accelerometer_no_print() {

rawX = analogRead(A0);

rawY = analogRead(A1);

rawZ = analogRead(A2);

}

/*

SCATY




69 of 110

// deprecated function

void stand_w_feedback() {

Serial.println("Falling");

if (rawX < 520 && curr_standing_pos[4] < (init_standing_pos[4] + 30)

&& curr_standing_pos[7] > (init_standing_pos[7] - 30)) {

curr_standing_pos[4] -= 5;

curr_standing_pos[7] += 5;

move_xl320(4, curr_standing_pos[4]);


Serial.print("Falling Back ");

//Serial.println(curr_standing_pos[2]);

}

else if (rawX > 535 && curr_standing_pos[4] > (init_standing_pos[4] - 30)

&& curr_standing_pos[7] < (init_standing_pos[7] + 30)) {

curr_standing_pos[4] += 5;

curr_standing_pos[7] -= 5;



Serial.print("Falling Forward ");

//Serial.println(curr_standing_pos[4]);

}

Serial.print("4: ");

Serial.print(curr_standing_pos[4]);

Serial.print("\t");

Serial.print("7: ");

Serial.println(curr_standing_pos[7]);

Serial.println(rawX);

delay(100);

}

*/

14.4.3 BLE.h

// This implementation uses the internal data queing, so it behaves like Serial

void BLE_setup(void) {

BTLEserial.setDeviceName("John");

BTLEserial.begin();

}

aci_evt_opcode_t laststatus = ACI_EVT_DISCONNECTED;

char incomingBLE_command[128];

unsigned int incomingBLE_command_count = 0;

boolean incomingBLE_command_in = 0;

void BLE_loop() {

//NOTE: Current implementation only allows for one-way communication (receiving), from Controller to Robot

// Tell the nRF8001 to do whatever it should be working on.

BTLEserial.pollACI();

// Ask what is our current status

aci_evt_opcode_t BLE_status = BTLEserial.getState();

// If the status changed....

SCATY




70 of 110

if (BLE_status != laststatus) {

if (BLE_status == ACI_EVT_DEVICE_STARTED && debug_mode) {

Serial.println(F("* Advertising started"));

}

if (BLE_status == ACI_EVT_CONNECTED && debug_mode) {

Serial.println(F("* Connected!"));

}

if (BLE_status == ACI_EVT_DISCONNECTED && debug_mode) {

Serial.println(F("* Disconnected or advertising timed out"));

}

// OK set the last status change to this one

laststatus = BLE_status;

}

if (BLE_status == ACI_EVT_CONNECTED ) {

// Lets see if there's any data for us!

if (BTLEserial.available()) {

if(debug_mode) {

Serial.print("* "); Serial.print(BTLEserial.available()); Serial.println(F(" bytes available from BTLE"));

}

}

// OK while we still have something to read, get a character and print it out

incomingBLE_command_count = 0;

while (BTLEserial.available()) {

char c = BTLEserial.read();

incomingBLE_command[incomingBLE_command_count] = c;

if(debug_mode) {

Serial.print(c);

}

incomingBLE_command_count++;

incomingBLE_command_in = 1;

}

if(incomingBLE_command_in == 1)

{

command_menu(incomingBLE_command);

incomingBLE_command_in = 0;

}

// Evaluate the following code to enable two-way communication

/*

// Next up, see if we have any data to get from the Serial console

if (Serial.available()) {

// Read a line from Serial

Serial.setTimeout(100); // 100 millisecond timeout

String s = Serial.readString();

// We need to convert the line to bytes, no more than 20 at this time

uint8_t sendbuffer[20];

s.getBytes(sendbuffer, 20);

char sendbuffersize = min(20, s.length());

Serial.print(F("\n* Sending -> \"")); Serial.print((char *)sendbuffer); Serial.println("\"");

// write the data

BTLEserial.write(sendbuffer, sendbuffersize);

SCATY




71 of 110

}

*/

}

}

14.4.4 Dynamixel.h

//Dynamixel Control and Command Menu

void clear_array(char data[128])

{

for (int i = 0; i < 128; i++)

{

data[i] = 0;

}

}

void print_array(char data[128])

{

char i = 0;

while (data[i])

{

Serial.print(char(data[i]));

i++;

}

Serial.println();

}

int command_check(char data[128])

{

unsigned int i = 0;

while (command_list[0][i] != '0')

{

for (int j = 0; j < command_size[i] ; j++)

{

if (data[j] == command_list[i][j])

{

if (j == command_size[i] - 1)

{

return i;

}

}

else

{

break;

}

}

i++;

}

}

void move_xl320(unsigned int servoID, unsigned int pos)

{

if (xl320_lower_limit[servoID] <= pos && xl320_upper_limit[servoID] >= pos) {

motor.moveJoint(servoID, pos);

SCATY




72 of 110

}

else if(debug_mode){

Serial.println("ERROR: OUTPUT OF BOUND ");

Serial.print(servoID);

Serial.print("\t");

Serial.println(pos);

}

delay(10);

}

void setSpeed_xl320(int servo_speed)

{

//Initialize motor joint speed

for (int i = 0; i < servoID_size; i++)

{

motor.setJointSpeed(servoID[i], servo_speed);

delay(delay_writeAll);

}

if(debug_mode)

{

Serial.print("Speed: ");

Serial.println(servo_speed);

delay(5);

}

}

void help_function(char data[128])

{

Serial.println(data);

Serial.println("--------------------------------------------------");

Serial.println("Current Program: EE175_Multiple_2");

Serial.println("--------------------------------------------------");

Serial.println("motor(<servoID>,<pos>)");

Serial.println("ServoID: 1 to 10. 254 controls all motors.");

Serial.println("Pos: 0 to 1023.");

Serial.println("--------------------------------------------------");

Serial.println("led(<servoID>,<color>)");

Serial.println("ServoID: 1 to 10. 254 controls all motors.");

Serial.println("Color: 0 to 7. 7 is 'no-color'");

Serial.println("--------------------------------------------------");

Serial.println("debug");

Serial.println("led(<r>,<g>,<b>)");

Serial.println("lcd");

Serial.println("servo(<servo number>,<pos>");

Serial.println("cam_rpi: enable/disable camera readings");

Serial.println("toggle_servo: enable/disable servos");

Serial.println("RASPBERRY PI COMMANDS:");

Serial.println("login_rpi: login to rpi");

Serial.println("init_rpi: initiate facial recognition");

Serial.println("face_rpi: run facial recognition (only use after stop_rpi)");

Serial.println("stop_rpi: stop facial recognition");

Serial.println("shutdown_rpi");

Serial.println("--------------------------------------------------");

}

SCATY




73 of 110

/*already declared

int search_char_on_array(char data[128], char char_goal)

{

unsigned int i = 0;

while (data[i] != char_goal)

{

i++;

}

return i;

}*/

int parse_parameter(char data[128], int parenthesis)

{

unsigned int number_param;

int i = parenthesis;

while (data[i] <= '9' && data[i] >= '0')

{

//Designed to be 2-digits max

if (i == parenthesis)

{

//first digit

number_param = (data[i] - 48); //store first digit

}

else if (i > parenthesis)

{

number_param *= 10; //first digit, multiplied by 10

number_param += (data[i] - 48); //add second digit to ServoID

}

i++;

}

return number_param;

}

int parse_parameter(unsigned int number_param, char data[128], int parenthesis)

{

int i = parenthesis;

while (data[i] <= '9' && data[i] >= '0')

{

//Designed to be 2-digits max

if (i == parenthesis)

{

//first digit

number_param = (data[i] - 48); //store first digit

}

else if (i > parenthesis)

{

number_param *= 10; //first digit, multiplied by 10

number_param += (data[i] - 48); //add second digit to ServoID

}

i++;

}

return number_param;

}

int count_digits(int number)

SCATY




74 of 110

{

int i = 0;

while (number > 0)

{

number /= 10;

i++;

}

return i;

}

void motor_function(char data[128])

{

//disregard first 6 char: "motor("

unsigned int ServoID = 0; //stores target ServoID value

unsigned int Servo_move = 0; //stores target final position value

unsigned int array_pos = 0; //keeps track of position in array being parsed

int parenthesis = search_char_on_array(data, '(');

array_pos = parenthesis;

ServoID = parse_parameter(ServoID, data, parenthesis + 1);

array_pos += count_digits(ServoID);

//Parse first parameter

//loop as long as it is a number

array_pos += 2; //separator from first and second parameter

//could be any character as long as it's not a number

//Second parameter

//designed to fit up to 4 digits

//range of motor up to 1023

Servo_move = parse_parameter(Servo_move, data, array_pos);

if(debug_mode)

{

print_array(data); //display command entered

Serial.print("ServoID: ");

Serial.print(ServoID); //ServoID: first parameter

Serial.print("\t");

Serial.print("Move: ");

Serial.print(Servo_move); //Position: second parameter

Serial.println();

}

//Actually send the command to move the Dynamixel XL-320

//motor.moveJoint(ServoID,Servo_move); //DO NOT USE THIS ANYMORE

move_xl320(ServoID, Servo_move); //includes safety checks


}

void motor_all_function(unsigned int data[16])

{

SCATY




75 of 110

//Actually send the command to move the Dynamixel XL-320

if(debug_mode) Serial.println("Writing to all motors");

for (unsigned int i = 0; i < 16; i++)

{

if (i == 11)

break;

//motor.moveJoint(i,data[i]); //DO NOT USE THIS ANYMORE

move_xl320(i, data[i]);

if(debug_mode)

{

Serial.print(i);

Serial.print("\t");

Serial.println(data[i]);


}

}

}

void stand_function() {

setSpeed_xl320(200);

motor_all_function(init_standing_pos);

}

void led_function(char data[128])

{

//code

unsigned int ServoID = 0;

unsigned int array_pos = 0;

unsigned int Servo_color = 0;



ServoID = parse_parameter(ServoID, data, parenthesis + 1);

array_pos += count_digits(ServoID);

array_pos += 2;

Servo_color = parse_parameter(Servo_color, data, array_pos);

print_array(data);

if(debug_mode)

{

Serial.print("ServoID: ");

Serial.print(ServoID); //ServoID: first parameter

Serial.print("\t");

Serial.print("Color: ");

Serial.print(Servo_color); //Position: second parameter

Serial.println();

}

motor.LED(ServoID, &rgb[7]);

delay(5); //wait for first instruction to finish sending. Timer overrun

motor.LED(ServoID, &rgb[Servo_color]);

delay(5);

SCATY




76 of 110

}

void setspeed_function(char data[128])

{

//code

unsigned int ServoSpeed = 0;

unsigned int array_pos = 0;



ServoSpeed = parse_parameter(ServoSpeed, data, parenthesis + 1);

if(debug_mode)

{

Serial.print("Servo Speed: ");

Serial.print(ServoSpeed); //ServoID: first parameter

Serial.println();

}

setSpeed_xl320(ServoSpeed);

}

void reset_switch() {

digitalWrite(21, LOW);

delay(1000);

digitalWrite(21, HIGH);

}

void right_leg_PID()

{

Input = (analogRead(A0));

if (Input > (Setpoint - 10) && Input < (Setpoint + 10))

Input = Setpoint;

Input_y = (analogRead(A1));

if (Input_y > (Setpoint_y - 10) && Input_y < (Setpoint_y + 10))

Input_y = Setpoint_y;

myPID.Compute();

myPID3.Compute();

move_xl320(4, (int)Output);

if (!y_PID_disable)

{

move_xl320(5, (int)Output3);

}

}

void Dynamixel_setup()

{

Serial1.begin(115200); //XL-320 baud rate

pinMode(23, INPUT);

pinMode(21, OUTPUT);

digitalWrite(21, LOW);

delay(1000);

digitalWrite(21, HIGH);

delay(1000);

Serial.println("Initializing");

SCATY




77 of 110

motor.begin(Serial1); //Link XL320 motor object to Software Serial motor_serial


delay(5);

//Set motors to standing position

motor_all_function(init_standing_pos);

//turn the PID on

myPID.SetMode(AUTOMATIC);

myPID2.SetMode(AUTOMATIC);

myPID3.SetMode(AUTOMATIC);

myPID.SetOutputLimits(init_standing_pos[4]-xl320_PID_delta_x, init_standing_pos[4]+xl320_PID_delta_x);

myPID2.SetOutputLimits(init_standing_pos[7]-xl320_PID_delta_x, init_standing_pos[7]+xl320_PID_delta_x);

myPID3.SetOutputLimits(450, 550);

PID_sole_l.SetMode(AUTOMATIC);

PID_sole_r.SetMode(AUTOMATIC);

PID_sole_l.SetOutputLimits(init_standing_pos[9]-5,init_standing_pos[9]+5);

PID_sole_r.SetOutputLimits(init_standing_pos[2]-5,init_standing_pos[2]+5);

Setpoint = 525;

Setpoint_y = 498;

y_PID_disable = 1;

myPID.SetSampleTime(50);

myPID2.SetSampleTime(50);

PID_sole_l.SetSampleTime(50);

PID_sole_r.SetSampleTime(50);

}

#define NUM_READS 20

float readAnalog(int sensorpin){

// read multiple values and sort them to take the mode

int sortedValues[NUM_READS];

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

int value = analogRead(sensorpin);

int j;

if(value<sortedValues[0] || i==0){

j=0; //insert at first position

}

else{

for(j=1;j<i;j++){

if(sortedValues[j-1]<=value && sortedValues[j]>=value){

// j is insert position

break;

}

}

}

for(int k=i;k>j;k--){

SCATY




78 of 110

// move all values higher than current reading up one position

sortedValues[k]=sortedValues[k-1];

}

sortedValues[j]=value; //insert current reading

}

//return scaled mode of 10 values

float returnval = 0;

for(int i=NUM_READS/2-5;i<(NUM_READS/2+5);i++){

returnval +=sortedValues[i];

}

returnval = returnval/10;

return returnval;

}

void dynamixel_PID_x()

{

if(PID_X_EN || PID_SOLE_EN)

{

Input = readAnalog(A0);

if(debug_mode)

Serial.println(Input);

}

if(PID_X_EN)

{

myPID.Compute();

myPID2.Compute();

move_xl320(4,(int)Output);

move_xl320(7,(int)Output2);

}

if(PID_SOLE_EN)

{

PID_sole_l.Compute();

PID_sole_r.Compute();

move_xl320(9,(int)Output_sole_l);

move_xl320(2,(int)Output_sole_r);

}

}

enum walk_left_sm_Status

{walk_left_sm_init,walk_left_sm_weight,walk_left_sm_lift,walk_left_sm_forward,walk_left_sm_stable,walk_left_

sm_stop} walk_left_sm_status;

void walk_left_step()

{

switch(walk_left_sm_status)

{

case walk_left_sm_init:

break;

case walk_left_sm_weight:

PID_X_EN = 0;

PID_SOLE_EN = 0;

setSpeed_xl320(80);

move_xl320(4,395);

SCATY




79 of 110

move_xl320(1,218);

move_xl320(5,530);

//move_xl320(6,530);

move_xl320(4,420);

move_xl320(7,605);


move_xl320(4,390);

break;

case walk_left_sm_lift:

//move_xl320(4,380);


move_xl320(4,375);

move_xl320(8,200);

move_xl320(7,800);

move_xl320(9,380);

move_xl320(1,200);

move_xl320(2,590);

break;

case walk_left_sm_forward:

move_xl320(8,400);

move_xl320(9,480);

move_xl320(7,670);

move_xl320(1,180);

move_xl320(6,545);

move_xl320(2,580);

move_xl320(10,550);

move_xl320(5,510);

break;

case walk_left_sm_stable:


move_xl320(4,440);

move_xl320(7,680);

move_xl320(2,580);

move_xl320(9,490);

Setpoint = 530;

myPID.SetOutputLimits(440-10, 440+10);

myPID2.SetOutputLimits(680-10, 680+10);

PID_sole_l.SetOutputLimits(490-10,490+10);

PID_sole_r.SetOutputLimits(580-10,580+10);

PID_X_EN = 1;

PID_SOLE_EN = 1;

break;

default:

break;

}

switch(walk_left_sm_status)

{

case walk_left_sm_init:

if(walk_left_sm_en)

SCATY




80 of 110

{

UltraS_loop();

if(distance >= 10)

{

walk_left_sm_status = walk_left_sm_weight;

}

}

else

walk_left_sm_status = walk_left_sm_init;

break;

case walk_left_sm_weight:

walk_left_sm_status = walk_left_sm_lift;

break;

case walk_left_sm_lift:

walk_left_sm_status = walk_left_sm_forward;

break;

case walk_left_sm_forward:

walk_left_sm_status = walk_left_sm_stable;

break;

case walk_left_sm_stable:

walk_left_sm_status = walk_left_sm_stop;

case walk_left_sm_stop:

if(walk_left_sm_en)

walk_left_sm_status = walk_left_sm_stop;

else

walk_left_sm_status = walk_left_sm_init;

break;

default:

break;

}

}

unsigned int right_leg_counter = 0;

void stand_in_right_leg()

{

right_leg_counter++;

if (right_leg_counter == 4800) {

y_PID_disable = 0;

setSpeed_xl320(50);

Setpoint = 510;

Setpoint_y = 498;

}

else if (right_leg_counter == 5600) {

move_xl320(10, 520);

move_xl320(1, 205);

}


Setpoint_y = 495;

//move_xl320(4, 480);

move_xl320(10, 530);

move_xl320(1, 210);

}


Setpoint_y = 490;

SCATY




81 of 110

//move_xl320(4, 485);

move_xl320(10, 540);

move_xl320(1, 220);

}


Setpoint_y = 486;

//move_xl320(4, 490);

move_xl320(10, 550);

move_xl320(1, 228);

move_xl320(5, 550);

move_xl320(6, 575);

move_xl320(7, 771);

move_xl320(8, 272);

}

}

//*************************** Menu to Execute Commands

******************************************

void command_menu(char incomingCommand_stream[128])

{

switch (command_check(incomingCommand_stream))

{

case 0:

help_function(incomingCommand_stream);

break;

case 1:

motor_function(incomingCommand_stream);

break;

case 2:

led_function(incomingCommand_stream);

break;

case 3:

//bow_function();

curr_sensor();

break;

case 4:

//walk_function();

//servo_map(0, 200);

//servo_map(3, 0);

walk_left_sm_en = walk_left_sm_en? 0:1;

Serial.print("Walk: ");

Serial.println(walk_left_sm_en);

break;

case 5:

stand_function();

servo_map(0, init_rest_arms[0]);

servo_map(3, init_rest_arms[3]);

break;

case 6:

setspeed_function(incomingCommand_stream);

SCATY




82 of 110

break;

case 7:

//moveRight();

//rightLeg();

//Serial.println("Moving right leg");

accelerometer();

break;

case 8:

//leftLeg();

//moveLeft();

//Serial.println("Moving left leg");

reset_switch();

break;

case 9:

init_rpi();

break;

case 10:

login_rpi();

break;

case 11:

stop_rpi();

break;

case 12:

ip();

break;

case 13:

shutdown_rpi();

break;

case 14:

//cam_rpi

IMAGE_TRACKING_EN = ~(IMAGE_TRACKING_EN);

break;

case 15:

//toggle_servo

newline_rpi();

break;

case 16:

//debug

debug_mode = ~(debug_mode);

Serial.print("debug: ");

Serial.println(debug_mode);

break;

case 17:

skip_rpi_init = 1;

Serial.print("Skipping RPi: ");

Serial.println(skip_rpi_init);

break;

case 18:

head_follow_camera_en = ~head_follow_camera_en;

break;

case 19:

//arms_up

servo_update(0,200,4000);

servo_update(3,0,4000);

break;

SCATY




83 of 110

case 20:

//arms_down

servo_update(0, init_rest_arms[0],4000);

servo_update(3, init_rest_arms[3],4000);

break;

case 21:

//easyvr_en

EASYVR_SM_EN = EASYVR_SM_EN? 0:1;

Serial.print("EASYVR_EN = ");

Serial.println(EASYVR_SM_EN);

break;

case 22:

//right_hand

servo_map(0,200);

Serial.println("Right Hand");

break;

case 23:

//left_hand

servo_map(3,0);

Serial.println("Left Hand");

break;

case 24:

servo_map(0,init_rest_arms[0]);

Serial.println("Right Hand Down");

break;

case 25:


Serial.println("Left Hand Down");

break;

case 26:

motor_function(incomingCommand_stream);

break;

default:

ERROR_NOT_FOUND(incomingCommand_stream);

break;

}

clear_array(incomingCommand); //clear array storing command buffer

}

void command_menu_loop() {

unsigned char incomingCommand_size = 0;

flag = 0;

//Wait for incoming user input on Serial Terminal

if (Serial.available())

{

incomingCommand_size = Serial.available(); //store the size of the input on a variable

//Read and store all incoming data on UART on incomingCommand array

for (int i = 0; i < incomingCommand_size; i++)

{

incomingCommand[i] = Serial.read(); //store the data on the buffer on an array

}

command_menu(incomingCommand);

SCATY




84 of 110

}

}

14.4.5 EasyVR.h

// EasyVR - Voice Recognition module

// Preprogrammed behaviors for voice commands

void EasyVR_setup() {

Serial2.begin(9600);

while(!easyvr.detect())

{

if(debug_mode) Serial.println("EasyVR not detected!");

}

easyvr.setPinOutput(EasyVR::IO1, LOW);

if(debug_mode) Serial.println("EasyVR detected!");

easyvr.setTimeout(5);

easyvr.setLanguage(0);

group = EasyVR::TRIGGER; //<-- start group (customize)

}

void action();

enum EasyVR_SM_Status {EasyVR_SM_init, EasyVR_SM_1, EasyVR_SM_2, EasyVR_SM_3, EasyVR_SM_4}

EasyVR_SM_status;

void EasyVR_SM()

{

switch (EasyVR_SM_status)

{

case EasyVR_SM_1:

easyvr.setPinOutput(EasyVR::IO1, HIGH); // LED on (listening)

if(debug_mode)

{

Serial.print("Say a command in Group ");

Serial.println(group);

}

easyvr.recognizeCommand(group);

break;

case EasyVR_SM_2:

break;

case EasyVR_SM_3:

easyvr.setPinOutput(EasyVR::IO1, LOW); // LED off

idx = easyvr.getWord();

if (idx >= 0)

{

// built-in trigger (ROBOT)

// group = GROUP_X; <-- jump to another group X

return;

}

idx = easyvr.getCommand();

SCATY




85 of 110

if (idx >= 0)

{

// print debug message

uint8_t train = 0;

char name[32];

if(debug_mode)

{

Serial.print("Command: ");

Serial.print(idx);

}

if (easyvr.dumpCommand(group, idx, name, train))

{

if(debug_mode)

{

Serial.print(" = ");

Serial.println(name);

}

}

else

{

if(debug_mode) Serial.println();

}

easyvr.playSound(0, EasyVR::VOL_FULL);

// perform some action

action();

}

else // errors or timeout

{

if (easyvr.isTimeout())

{

if(debug_mode) Serial.println("Timed out, try again...");

}

int16_t err = easyvr.getError();

if (err >= 0)

{

if(debug_mode)

{

Serial.print("Error ");

Serial.println(err, HEX);

}

}

}

break;

default:

break;

}

switch (EasyVR_SM_status)

{

case EasyVR_SM_init:

if(EASYVR_SM_EN)

EasyVR_SM_status = EasyVR_SM_1;

else

EasyVR_SM_status = EasyVR_SM_init;

break;

case EasyVR_SM_1:

SCATY




86 of 110

if(EASYVR_SM_EN)


else


break;

case EasyVR_SM_2:

if(EASYVR_SM_EN == 0)

{


}

else if (!easyvr.hasFinished())

{


}

else

{


}

break;

case EasyVR_SM_3:

if(EASYVR_SM_EN)

{


}

else

{


}

break;

case EasyVR_SM_4:


break;

default:

break;

}

//Serial.println(EasyVR_SM_status);

}

void action()

{

switch (group)

{

case GROUP_0:

switch (idx)

{

case G0_JOHN:

group = GROUP_1;

//mood = HAPPY;

command_menu("led(254,2)");

activity = 1;

break;

}

break;

case GROUP_1:

switch (idx)

SCATY




87 of 110

{

case G1_BOW:

group = GROUP_0;

break;

case G1_STOP:

group = GROUP_0;

command_menu("stop_init");

delay(100);


break;

case G1_WALK:

group = GROUP_0;

break;

case G1_RIGHT:

group = GROUP_0;

break;

case G1_LEFT:

group = GROUP_0;

break;

case G1_LIGHT_RED:

group = GROUP_0;


break;

case G1_LIGHT_GREEN:

group = GROUP_0;


break;

case G1_LIGHT_WHITE:

group = GROUP_0;


break;

case G1_LIGHT_YELLOW:

group = GROUP_0;


break;

case G1_LIGHT_OFF:

group = GROUP_0;


break;

case G1_HELLO:

group = GROUP_0;


Serial3.println("echo \"Hello. How are you doing?\" | festival --tts");

Serial.println("hello");

break;

case G1_GOODBYE:

group = GROUP_0;




//mood = SAD;

break;

case G1_GOOD_NIGHT:

group = GROUP_0;


SCATY




88 of 110

break;

case G1_SLEEP:

group = GROUP_0;


activity = 0;

command_menu("stop_rpi");

delay(1000);

command_menu("shutdown_rpi");

break;

case G1_IP:

group = GROUP_0;


command_menu("ip");

break;

case G1_DANCE:

group = GROUP_0;


Serial3.println("echo \"I don't want to dance\" | festival --tts");

delay(500);

servo_map(6,init_rest_arms[6]-15);

delay(400);

servo_map(6,init_rest_arms[6]+15);

delay(400);

servo_map(6,init_rest_arms[6]-15);

delay(400);


break;

case G1_MUSIC:

group = GROUP_0;


break;

case G1_CAMERA:

group = GROUP_0;


IMAGE_TRACKING_EN = ~IMAGE_TRACKING_EN;

break;

case G1_KICK:

group = GROUP_0;


break;

case G1_HANDS_UP:

group = GROUP_0;


servo_map(0,200);

servo_map(3,0);

//mood = EXCITED;

break;

case G1_RIGHT_HAND:

group = GROUP_0;


servo_map(0,200);

break;

case G1_LEFT_HAND:

group = GROUP_0;


SCATY




89 of 110

servo_map(3,0);

break;

case G1_RED:

group = GROUP_0;


break;

case G1_GREEN:

group = GROUP_0;


break;

case G1_FIND_ME:

group = GROUP_0;


break;

default:

break;

}

break;

}

}

14.4.6 Head_Arms.h

// Servo Board, communication via I2C

// Upper Body = Head and Arms

void I2C_setup(){

pwm.begin();

pwm.setPWMFreq(60); //this frequency seems to work just fine

// save I2C bitrate

uint8_t twbrbackup = TWBR;

// must be changed after calling Wire.begin() (inside pwm.begin())

TWBR = 12; // upgrade to 400KHz

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

servo_map(motor_joint[i],init_rest_arms[i]);

delay(20);

}

}

void servo_map(int servo_id, int angle){

int pulse = 0;

if(angle > 180)

angle = 180;

else if(angle < 0)

angle = 0;

pulse = map(angle,0,180,SERVOMIN,SERVOMAX);

pwm.setPWM(servo_id,0,pulse); //updates the PCA9685 PWM pulse

}

void servo_update(int servo_number, int servo_pos, int servo_duration)

{

servo_sm_update_en = 1;

target_arms_pos[servo_number] = servo_pos;

SCATY




90 of 110

target_arms_time[servo_number] = servo_duration;

}

enum Servo_SM_Status {Servo_SM_init,Servo_SM_wait,Servo_SM_move} Servo_SM_status;

void servo_write()

{

switch(Servo_SM_status)

{

case Servo_SM_init:

break;

case Servo_SM_wait:

break;

case Servo_SM_move:

for(int i = 0; i < 10; i++)

{

if(target_arms_pos[i] != curr_arms_pos[i] && target_arms_time[i] >= 0)

{

if(target_arms_time[i] == 0 || target_arms_time[i] < Servo_SM_Period)

target_arms_time[i] = Servo_SM_Period;

curr_arms_pos[i] += (target_arms_pos[i] - curr_arms_pos[i])/(target_arms_time[i]/Servo_SM_Period);

servo_map(i,target_arms_pos[i]);

target_arms_time[i] -= Servo_SM_Period;

if(target_arms_time <= 0)

{

target_arms_time[i] = -1;


}

else


}

}

break;

default:

break;

}

switch(Servo_SM_status)

{

case Servo_SM_init:

Servo_SM_status = Servo_SM_wait;

break;

case Servo_SM_wait:

if(servo_sm_update_en)

Servo_SM_status = Servo_SM_move;

else


break;

case Servo_SM_move:


break;

default:

break;

}

}

SCATY




91 of 110

//--------- Head Camera Follow ------

//SM1_Tick_HeadCameraFollow() updates servo position

//Based on camera input

//Based on other factors too

unsigned long prevMillis1 = 0;


enum SM1_States {SM1_init, SM1_wait, SM1_follow} SM1_state;

void SM1_Tick_HeadCameraFollow()

{

delta_camera_x = prev_camera_x - camera_x;

delta_camera_y = prev_camera_y - camera_y;

//follow_camera = (abs(delta_camera_x)>x_threshold||abs(delta_camera_y)>y_threshold)?1:0;

switch (SM1_state)

{

case SM1_init:

break;

case SM1_follow:

if (camera_x < 20)

servo_base_pos += 1;

else if (camera_x > 60)

servo_base_pos -= 1;

if (camera_y < 15)

servo_tilt_pos -= 1;

else if (camera_y > 45)

servo_tilt_pos += 1;

if (servo_base_pos >= servo_base_pos_upper)

servo_base_pos = servo_base_pos_upper;

else if (servo_base_pos <= servo_base_pos_lower)

servo_base_pos = servo_base_pos_lower;

if (servo_tilt_pos >= servo_tilt_pos_upper)

servo_tilt_pos = servo_tilt_pos_upper;

else if (servo_tilt_pos <= servo_tilt_pos_lower)

servo_tilt_pos = servo_tilt_pos_lower;

if(servo_base_pos != prevServo_base_pos)

{

servo_map(motor_joint[6],servo_base_pos);

prevServo_base_pos = servo_base_pos;

}

if(servo_tilt_pos != prevServo_tilt_pos)

{

servo_map(motor_joint[7],servo_tilt_pos);

prevServo_tilt_pos = servo_tilt_pos;

}

break;

default:

break;

SCATY




92 of 110

}

switch (SM1_state)

{

case SM1_init:

SM1_state = SM1_follow;

break;

case SM1_wait:

if(head_follow_camera_en == 1)


else

SM1_state = SM1_wait;

case SM1_follow:

if (follow_camera && head_follow_camera_en)


else

SM1_state = SM1_wait;

default:

break;

}

}

//----- end of State Machine ------

14.4.7 RPi.h

//Raspberry Pi 2

//Computer Vision and Text to Speech

void RPi_setup()

{

Serial3.begin(115200);

}

void login_rpi()

{

Serial3.println("pi");

Serial.println("pi");

delay(100);

Serial3.println("raspberry");

Serial.println("password: *********");

while (Serial3.available()) {

char c = Serial3.read();

Serial.print(c);

}

}

void init_rpi()

{

Serial.println("Initiating Facial Recognition");

Serial3.println("python /home/pi/Documents/python27/picam_face_color_3_textonly.py &");

Serial.println("picam_face_color_3.py");

}

char ip_address_stream[50];

SCATY




93 of 110

void ip()

{

Serial3.println("hostname -I");

if(Serial3.available())

{

short incomingStream = Serial3.available();

for(int i = 0; i < incomingStream; i++)

{

ip_address_stream[i] = Serial3.read();

}

Serial.println(ip_address_stream);

}

delay(100);

Serial3.println("cd /home/pi/Documents/custom");

delay(100);

Serial3.println("./ip_echo.sh");

Serial.print("IP ADDRESS: ");

Serial.println(ip_address_stream);

Serial3.flush();

}

void stop_rpi()

{

Serial3.write('\x03');

Serial.write("^C");

Serial3.flush();

Serial3.println("kill %");

Serial.println("stopping tasks");

Serial3.flush();

}

void shutdown_rpi()

{

Serial3.write('\x03');

delay(1000);

Serial3.println("sudo halt");

Serial.println("sudo halt");

delay(1000);



Serial.print(c);

}

}

void cam_rpi()

{

camera_flag = !camera_flag;

Serial.print("Camera Status: ");

Serial.println(camera_flag);

}

void ERROR_NOT_FOUND(char data[128])

{

//code

Serial.print("ERROR: ");

SCATY




94 of 110

Serial.println(data);

}

void newline_rpi()

{



Serial.print(c);

}

Serial3.println();

Serial.println("NEWLINE");



Serial.print(c);

}

}

void debug_mode_init()

{

debug_mode = !debug_mode;

Serial.print("Debug mode: ");

Serial.println(debug_mode);

}

void init_array(char array[], int array_size)

{

for (int i = 0; i < array_size; i++)

{

array[i] = 0;

}

}

bool input_cam()

{

if (Serial3.available())

{

init_array(serial3_incoming, 128);

serial3_size = Serial3.available();

for (int i = 0; i < serial3_size; i++)

{

serial3_incoming[i] = Serial3.read();

}

//delay(20);

//Serial.print(serial3_incoming);

//delay(20);

return true;

}

else

return false;

}

int search_int_on_array(char data[128], unsigned int start_search)

{

SCATY




95 of 110

unsigned int i = start_search;

while (data[i] < '0' || data[i] > '9')

{

i++;

}

return i;

}

int search_char_on_array(char data[128], char char_goal)

{

unsigned int i = 0;

while (data[i] != char_goal)

{

i++;

}

return i;

}

//----------- State Machine for Raspberry Pi --------------


enum SM4_Status {SM4_init, SM4_pre_login, SM4_login, SM4_login_wait, SM4_initRPi, SM4_initRPi_wait,

SM4_wait, SM4_read, SM4_update_cam} SM4_status;

unsigned int x_size = 0, y_size = 0, h_size = 0, t_size = 0;

unsigned int int_start = 0;

unsigned int temp_int_start;

unsigned int RPi_init_count = 0;

void SM4_Tick_RPi()

{

switch (SM4_status)

{

case SM4_pre_login:

break;

case SM4_login:

login_rpi();

break;

case SM4_login_wait:

break;

case SM4_initRPi:

init_rpi();

break;

case SM4_initRPi_wait:

break;

case SM4_wait:

break;

case SM4_read:

count_last_time_seen++;

break;

case SM4_update_cam:

if(mood != EXCITED)

mood = HAPPY;

activity = 1;

activity_counter = 0;

SCATY




96 of 110

first_reading_taken = 1;

input_cam();

int_start = search_int_on_array(serial3_incoming, 0);

x = parse_parameter(serial3_incoming, int_start);

x_size = count_digits(x);

int_start = search_int_on_array(serial3_incoming, int_start + x_size + 1);

y = parse_parameter(serial3_incoming, int_start);

y_size = count_digits(y);

int_start = search_int_on_array(serial3_incoming, int_start + y_size + 1);

h = parse_parameter(serial3_incoming, int_start);

h_size = count_digits(h);

int_start = search_int_on_array(serial3_incoming, int_start + h_size + 1);

t = parse_parameter(serial3_incoming, int_start);

t_size = count_digits(t);

camera_x = x;

camera_y = y;

camera_h = h;

camera_t = t;

/*

//t = 1; red

if(camera_t == 1)

{


}

else if(camera_t == 2)

{

//t = 2; green


}


{

Serial3.println("echo \"I see you\" | festival --tts");

}

*/

if(t == 0)

{

mood = EXCITED;

}

else if(t == 1 || t == 2)

{

mood = HAPPY;

}

else

{

mood = FACE_NORMAL;

}

if(debug_mode == 1)

{

Serial.print(x);

Serial.print("\t");

Serial.print(y);

SCATY




97 of 110

Serial.print("\t");

Serial.print(h);

Serial.print("\t");

Serial.println(t);

if(prev_t != t)

{

if(t == 1)

{

Serial3.println("echo \"Red\" | festival --tts");

}

else if(t == 2)

{

Serial3.println("echo \"Green\" | festival --tts");

}

else if(t == 0)

{

Serial3.println("echo \"I see you\" | festival --tts");

}

}

prev_t = t;

/*

for(int i=0; i < 10; i++)

{

for(int j = 0; j < 20; j++)

{

if(i==0 || i == 9)

Serial.print("-");

//else if(j <= (20 - (x/x_proportion) - (h/x_proportion)) && j >= (20 - (x/x_proportion)) && i >=

(y/y_proportion) && i <= ((y/y_proportion) + (h/y_proportion)))

else if(j == (20 - (x/x_proportion) - (h/x_proportion)/2) && i == ((y/y_proportion) + (h/y_proportion)/2))

Serial.print("*");

else

Serial.print(" ");

}

Serial.println();

}*/

}

count_last_time_seen = 0;

follow_camera = 1;

break;

default:

break;

}

switch (SM4_status)

{

case SM4_init:

SM4_status = SM4_pre_login;

break;

case SM4_pre_login:

SCATY




98 of 110

if(RPi_init_count > (25000/SM4_Period))

{

SM4_status = SM4_login;

RPi_init_count = 0;

}

else if(skip_rpi_init == 1)

{

SM4_status = SM4_wait;

}

else

SM4_status = SM4_pre_login;

break;

case SM4_login:

SM4_status = SM4_login_wait;

break;

case SM4_login_wait:


{

SM4_status = SM4_initRPi;

RPi_init_count = 0;

}


{


}

else

SM4_status = SM4_login_wait;

break;

case SM4_initRPi:

SM4_status = SM4_initRPi_wait;

break;

case SM4_initRPi_wait:


{


RPi_init_count = 0;

}


{


}

else

SM4_status = SM4_initRPi_wait;

break;

case SM4_wait:

if(IMAGE_TRACKING_EN == 0)


else

SM4_status = SM4_read;

break;

case SM4_read:

if(input_cam() && camera_flag == 1 && IMAGE_TRACKING_EN == 1)

{

SM4_status = SM4_update_cam;

follow_camera = 0;

SCATY




99 of 110

}

else if(IMAGE_TRACKING_EN == 0)

{


stop_rpi();

}

else


break;

case SM4_update_cam:


break;

default:

break;

}

//Serial.println(RPi_init_count);

RPi_init_count++;

}

//-------- end of state machine -------

14.4.8 TFT.h

//TFT Display

void TFT_setup()

{

tft.begin();

tft.fillScreen(ILI9341_BLACK);

}

//------- TFT Display Functions -----------------

int mouth_h = 50;

int eyelit_h = (240/2) + 50;

int eyelit_l = (320/2)-80;

int eyelit_r = (320/2)+80;

void drawArc(int x1, int y1, int x2, int y, int r, int pixel_color)

{

int y_temp = 0;

int x_temp = 0;

x_temp = ((x2-x1)/2)+x1;

if(r >= sqrt((y1-y)*(y1-y)+(x_temp-x1)*(x_temp-x1)))

{

for(int i = x1; i < x_temp; i++)

{

y_temp = sqrt((r)*(r)-((x_temp-i)*(x_temp-i)));

tft.drawPixel(y-y_temp,i,pixel_color);

}

for(int i = x_temp; i < x2; i++)

{

y_temp = sqrt((r)*(r)-((i-x_temp)*(i-x_temp)));

tft.drawPixel(y-y_temp,i,pixel_color);

}

SCATY




100 of 110

}

}

void drawArc_up(int x1, int y1, int x2, int y, int r, int pixel_color)

{

int y_temp = 0;

int x_temp = 0;

x_temp = ((x2-x1)/2)+x1;

if(r >= sqrt((y-y1)*(y-y1)+(x_temp-x1)*(x_temp-x1)))

{

for(int i = x1; i < x_temp; i++)

{

y_temp = sqrt((r)*(r)-((x_temp-i)*(x_temp-i)));

tft.drawPixel(y+y_temp,i,pixel_color);

}

for(int i = x_temp; i < x2; i++)

{

y_temp = sqrt((r)*(r)-((i-x_temp)*(i-x_temp)));

tft.drawPixel(y+y_temp,i,pixel_color);

}

}

}

void mouth()

{

tft.drawLine(mouth_h,(320/2)-50,mouth_h,(320/2)+50,ILI9341_WHITE);

}

void blinking()

{

//tft.drawCircle((240/2),(320/2)+80, 40, ILI9341_BLACK);

//tft.drawCircle((240/2),(320/2)-80, 40, ILI9341_BLACK);

tft.drawLine(eyelit_h,(320/2)+80-40, eyelit_h,(320/2)+80+40, ILI9341_WHITE);

tft.drawLine(eyelit_h,(320/2)-80-40, eyelit_h,(320/2)-80+40, ILI9341_WHITE);

}

void blinking_undo()

{

tft.drawLine(eyelit_h,(320/2)+80-40, eyelit_h,(320/2)+80+40, ILI9341_BLACK);

tft.drawLine(eyelit_h,(320/2)-80-40, eyelit_h,(320/2)-80+40, ILI9341_BLACK);

}

void face_sleeping()

{

tft.drawCircle(mouth_h,(320/2), 10, ILI9341_WHITE);

drawArc(eyelit_r-40,eyelit_h,eyelit_r+40,eyelit_h+20,60,ILI9341_WHITE);

drawArc(eyelit_l-40,eyelit_h,eyelit_l+40,eyelit_h+20,60,ILI9341_WHITE);

}

void face_sleeping_undo()

{

tft.drawCircle(mouth_h,(320/2), 10, ILI9341_BLACK);

drawArc(eyelit_r-40,eyelit_h,eyelit_r+40,eyelit_h+20,60,ILI9341_BLACK);

drawArc(eyelit_l-40,eyelit_h,eyelit_l+40,eyelit_h+20,60,ILI9341_BLACK);

}

SCATY




101 of 110

void eyelit()

{

if(camera_t == 2)

{

tft.drawCircle(eyelit_h,eyelit_r, 40, ILI9341_RED);

tft.drawCircle(eyelit_h,eyelit_l, 40, ILI9341_RED);

tft.fillCircle(eyelit_h,eyelit_r, 40, ILI9341_RED);

tft.fillCircle(eyelit_h,eyelit_l, 40, ILI9341_RED);

}


{

tft.drawCircle(eyelit_h,eyelit_r, 40, ILI9341_GREEN);

tft.drawCircle(eyelit_h,eyelit_l, 40, ILI9341_GREEN);

tft.fillCircle(eyelit_h,eyelit_r, 40, ILI9341_GREEN);

tft.fillCircle(eyelit_h,eyelit_l, 40, ILI9341_GREEN);

}

else

{

tft.drawCircle(eyelit_h,eyelit_r, 40, ILI9341_WHITE);

tft.drawCircle(eyelit_h,eyelit_l, 40, ILI9341_WHITE);

}

}

void eyelit_undo()

{

tft.drawCircle(eyelit_h,eyelit_r, 40, ILI9341_BLACK);

tft.drawCircle(eyelit_h,eyelit_l, 40, ILI9341_BLACK);

}

void face_stand_by()

{

eyelit();

mouth();

}

void face_smile()

{

//mouth

drawArc((320/2)-50,mouth_h+20,(320/2)+50,mouth_h+50,60,ILI9341_WHITE);

eyelit();

}

void face_sad()

{

//mouth

drawArc_up((320/2)-50,mouth_h+20,(320/2)+50,mouth_h-10,60,ILI9341_WHITE);

eyelit();

}

void face_excited()

{

//mouth

tft.drawLine(mouth_h+30,(320/2)-60,mouth_h+30,(320/2)+60,ILI9341_WHITE);

drawArc((320/2)-60,mouth_h+40,(320/2)+60,mouth_h+40,60,ILI9341_WHITE);

SCATY




102 of 110

eyelit();

}

void clear_eyes()

{

tft.fillCircle(eyelit_h,eyelit_r, 50, ILI9341_BLACK);

tft.fillCircle(eyelit_h,eyelit_l, 50, ILI9341_BLACK);

}

void clear_mouth()

{

tft.fillRect(mouth_h-30,(320/2)-100,80,200, ILI9341_BLACK);

}

//---------- State Machine for the Face ------


enum SM2_Status {SM2_init,SM2_sleep,SM2_blink,SM2_expression} SM2_status;

void SM2_Tick_TFT()

{

switch(SM2_status)

{

case SM2_init:

break;

case SM2_sleep:

if(change_of_eyes)

{

clear_eyes();

change_of_eyes = 0;

}

if(change_of_mouth)

{

clear_mouth();

change_of_mouth = 0;

}

face_sleeping();

servo_flag = 0;

break;

case SM2_blink:

clear_eyes();

blink_counter = 0;

blinking();

change_of_eyes = 1;


break;

case SM2_expression:

if(mood != prev_mood)


if(change_of_eyes)

{

clear_eyes();

change_of_eyes = 0;

}

if(change_of_mouth)

SCATY




103 of 110

{

clear_mouth();


}

if(activity_counter > activity_interval)

//activity = 0;

servo_flag = 1;

if(mood == FACE_NORMAL)

face_stand_by();

else if(mood == HAPPY)

face_smile();

else if(mood == SAD)

face_sad();

else if(mood == EXCITED)

face_excited();

else if(mood == SLEEP)

face_sleeping();

prev_mood = mood;

break;

default:

break;

}

switch(SM2_status)

{

case SM2_init:

SM2_status = SM2_sleep;

break;

case SM2_sleep:

if(activity == 0)


else

{

SM2_status = SM2_expression;

change_of_eyes = 1;


}

break;

case SM2_blink:


break;

case SM2_expression:

if(activity == 0)

{


change_of_eyes = 1;


}

else if(blink_counter > blink_interval)

SM2_status = SM2_blink;

else


break;

default:

break;

SCATY




104 of 110

}

blink_counter++;

activity_counter++;

}

//----- end of State Machine ------

14.4.9 Ultrasonic.h

//HC-SR04 Ultrasonic Sensor

void UltraS_setup() {

pinMode(trigPin, OUTPUT);

pinMode(echoPin, INPUT);

}

//Function only called when the robot is gonna walk

//Check for obstacles on the pathway

void UltraS_loop() {

long duration;

digitalWrite(trigPin, LOW);

delayMicroseconds(2);

digitalWrite(trigPin, HIGH);

delayMicroseconds(10);

digitalWrite(trigPin, LOW);

duration = pulseIn(echoPin, HIGH);

distance = (duration/2) / 29.1;

if(debug_mode)

{

if (distance >= 100 || distance <= 0){

Serial.println("Out of range");

}

else {

Serial.print(distance);

Serial.println(" cm");

}

}

}

14.5 Appendix E

14.5.1 Python: picam_face_color_3_notext.py

#rpi code

from picamera.array import PiRGBArray

from picamera import PiCamera

import cv2

import time

import numpy as np

SCATY




105 of 110

#cv2.namedWindow("mask_red")

#cv2.namedWindow("mask_green")

prevTime_sec = time.localtime()[5]

img = np.zeros((512,512),np.uint8)

face_cascade = cv2.CascadeClassifier('/home/pi/Documents/python27/haarcascade_frontalface_default.xml')

camera = PiCamera()

camera.resolution = (320,240)

camera.framerate = 30

rawCapture = PiRGBArray(camera, size = (320,240))

#cap = cv2.VideoCapture(0)

time.sleep(0.2)

#cap.set(3,160)

#cap.set(4,120)

print('Executing: picam_face_color_3_textonly.py')

print('start')

def scan_image():

total = 0

for i in range(0,160):

for j in range(0,120):

#mask_green[100+i,100+i] = 100

if mask_green[i,j] >= 200:

total += 1

if total >= 20:

print total

#while(cap.isOpened()):

for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port = True):

# Take each frame

#_, frame = cap.read()

image = frame.array

frame = image

live = image

# Convert BGR to HSV

hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

# define range of blue color in HSV

#lower_blue = np.array([110,50,50])

#upper_blue = np.array([130,255,255])

#green

lower_green = np.array([50,100,100])

upper_green = np.array([70,255,255])

SCATY




106 of 110

#orange

#hsv(25,100,100)

#lower_orange = np.array([30,80,80])

#upper_orange = np.array([50,100,100])

#red

lower_red = np.array([0,100,100])

upper_red = np.array([4,255,255])

# Threshold the HSV image to get only blue colors

#mask = cv2.inRange(hsv, lower_blue, upper_blue)

#green

#mask = cv2.inRange(hsv,lower_green,upper_green)

#orange

#mask = cv2.inRange(hsv,lower_orange,upper_orange)

mask_green = cv2.inRange(hsv,lower_green,upper_green)

mask_red = cv2.inRange(hsv, lower_red, upper_red)

moments_green = cv2.moments(mask_green)

area_green = moments_green['m00']

if area_green > 10000:

x_green = int(moments_green['m10']/area_green)

y_green = int(moments_green['m01']/area_green)

print 'x: ' + repr(x_green) + ' y: ' + repr(y_green) + ' h: 10 t: 1'

moments_red = cv2.moments(mask_red)

area_red = moments_red['m00']

if area_red > 50000:

x_red = int(moments_red['m10']/area_red)

y_red = int(moments_red['m01']/area_red)

print 'x: ' +repr(x_red) + ' y: ' + repr(y_red) + ' h: 10 t: 2'

gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)

faces = face_cascade.detectMultiScale(gray,1.3,5)

for(x,y,w,h) in faces:

cv2.rectangle(live,(x,y),(x+w,y+h),(255,0,0),2)

print('x: '+repr(x)+' y: '+repr(y)+ ' h: '+ repr(h)) + ' t: 0'

# Bitwise-AND mask and original image

#res = cv2.bitwise_and(frame,frame, mask= mask)

#uncomment to visualize output on screen

#cv2.imshow('live',live)

#cv2.imshow('mask_red',mask_red)

#cv2.imshow('mask_green',mask_green)

rawCapture.truncate(0)

cv2.destroyAllWindows()

SCATY




107 of 110

14.6 Appendix F

14.6.1 Processing: ee175a_slider_config_gui_1.pde

//Processing 2.2.1

//Install G4P

import processing.serial.*;

import g4p_controls.*;

GSlider slider1,slider2,slider3,slider4,slider5,slider6,slider7,slider8,slider9,slider10;

//GSlider slider_obj[] = {slider1,slider2,slider3,slider4,slider5,slider6,slider7,slider8,slider9,slider10};

Serial myPort;

int val;

int init_pos[] = {200,500,512,512,512,512,512,512,512,512};

int slider_pos[] = {200,500,512,512,512,512,512,512,512,512};

int lower[] = {0,500,512,200,200,512,250,200,200,200};

int higher[] = {500,800,800,750,512,800,800,512,512,800};

/*

void create_slider(GSlider slider,int init_pos,int lower,int higher,int x, int y)

{

slider = new GSlider (this,x,y,260,50,10);

slider.setShowDecor(false,true,true,true);

slider.setNumberFormat(G4P.DECIMAL,3);

//slider.setNbrTicks(5);

slider.setLimits(init_pos,lower,higher);

}

*/

void setup()

{

size(640,640);

String portName = Serial.list()[0];

myPort = new Serial(this,portName, 115200);

//slider1 = new GCustomSlider(this,20,20,260,50,null);

/*

for(int i = 0; i < 10; i++)

{

create_slider(slider_obj[i],slider_pos[i],lower_limit[i],upper_limit[i],20,20+((i+1)*50));

}

*/

slider1 = new GSlider (this,20,20,260,50,10);

slider1.setShowDecor(false,true,true,true);

slider1.setNumberFormat(G4P.DECIMAL,3);


slider1.setLimits(init_pos[0],lower[0],higher[0]);


SCATY




108 of 110













































}

SCATY




109 of 110

void draw()

{

background(255);

if(keyPressed)

{

}

else

fill(0);

rect(500,300,100,100);

}

void keyPressed()

{

if(key == '1')

{

//fill(slider1_pos);

//myPort.write("motor(10," + slider1_pos + ")");

//myPort.write(slider1_pos);

//myPort.write(100);

//myPort.write(")");

//myPort.write("bow");

}

}

void handleSliderEvents(GValueControl slider,GEvent event)

{

//println();

if(slider == slider1)

{

println(slider.getValueI());

slider_pos[0] = slider.getValueI();

fill(slider_pos[0]);

myPort.write("motor(1," + slider_pos[0] + ")");

}

else if(slider == slider2)

{





}


{





}


{




SCATY




110 of 110


}


{





}


{





}


{





}


{





}


{





}


{





}

}