Classes - Beard

You are here: start » robot_soccer » 2017 » motion_control



Motion Control

Wiring Diagram

Wiring Diagram for Development

Wiring Diagram for Production

PSoC 4

  1. Install CY8CKIT-049-42xx Kit Setup and PSoC Creator 4. Note: The kit files contain PSoC Creator 3 which is why you have to install PSoC Creator 4 separately.
  2. Follow section 5.3 to 5.6 in Getting Started with PSoC 4 to create and build your first project. When creating the project select the CY8CKIT-049-42xx kit.
    1. Pay specific attention to the section on adding a bootloadable component. Remember to reference the .hex and .elf files in the bootloader project, NOT the ones in your own project!
  3. Download the project, unzip it, open it, and program it to the board using the same instructions you found in step 2.

If you wish to modify the project or create your own:

  1. Learn PSoC basics (PWM, digital output and inputs pins, UART, etc) from the Video Tutorials
  2. Use the PSoC 4 Datasheet to understand voltage specifications and pin connections (for example, some pins have direct connections to UART and PWM modules, others don't). Our PSoC is the CY8C4245AXI-483, 44-TQFP.
  3. Create a PSoC design that reads the motor encoders and controls the motors using PWM and PID control. Use the UART to receive commands from the Raspberry Pi. You are also welcome to use Neil Hinckley's soccer controller as a resource
    1. Keep in mind that there are only 4 TCPWM blocks. These blocks can be used for PWM or quadrature decoders. In interest of resources it is best to use these as quadrature decoders. The remaining PWM blocks can be implemented using UDBs (Universal Digital Block). For comparison, a UDB implementation of a PWM module takes 25% of the UDBs, but implementing a single quadrature encoder takes 75% of the UDBs.

PSoC Project SoccerController2017 Pin Connections


  • First learn how H-bridges work. Learn the difference between phase-magnitude drive and lock anti-phase drive. The PSoC projects above use phase-magnitude drive.
  • Our H-Bridge is the L289N. Use the L298N Datasheet to find out the voltage specifications and how to drive the H-Bridge. You'll notice that the logic voltage source (VSS) must be close to 5V, but the logic signals (IN1, ENA, etc) will work at 3.3V.
  • There are three jumpers. Disconnect all three jumpers unless you have a reason to leave them connected. The two jumpers on the ENA and ENB pins conect them to 5V so that the motors are always on (the main implication of leaving these jumpers connected is that the PSoC disengage command won't work). The other jumper tells the board to generate its own 5V power source by using the regulator on board. If you leave this jumper connected, DO NOT power the H-Bridge on the 5V pin because if the two voltages are slightly different they will short.

New Motors and Encoders


  • Cheap $30 motor from Pololu. There are a ton of specs on the product page
  • The motor is designed to be run at 12V
  • The gear ratio is 9.68:1 and the encoder has 12 lines or pulses per motor rotation (same as 48 counts per rotation since there are four pulse edges seen each pulse period). This means there are 12*9.68 = 116.16 pulses per shaft rotation (464.64 counts per rotation). As it is currently set up, the PSoC quadrature decoder only counts once for each pulse. You are more than welcome to modify the PSoC Creator project to count 4 times per pulse instead of only once per pulse.

Old Motors and Encoders

Motor Datasheet - Note: The motor is a series 2224U012SR by Faulhaber. Link to website

Motor Encoder Datasheet - You'll need this page to know which motor pins are VCC, GND, Channel A, Channel B, Motor +, and Motor -. The encoder is the IE2-512 magnetic encoder. Link to website

20/1 Gearhead datasheet. The exact gearhead ratio (9.679) can be found on page 14 of the gearhead reduction ratio datasheet. If you do some math, you should be able to get something close to the 19820 counts per revolution that Andrew Keller measured. Link to website. Again, as it is currently set up, the PSoC only quadrature decoder only counts once per pulse, so you will actually see 4955 pulses per rotation. You are more than welcome to modify the PSoC Creator project to count 4 times per pulse instead of only once per pulse.

Converting Robot Velocities to Motor Velocities


  • Derives kinematic relationship between commanded world speeds and commanded wheel speeds.
  • Presented in class on 1/15/2014.

Raspberry Pi Serial Communication

  • GPIO pins 14 and 15 (physical pins 8 and 10) are the tx and rx pins on the Raspberry Pi. See Raspberry Pi GPIO and this Interactive Pinout Diagram.
  • By default the serial port /dev/ttyS0 is connected to these pins. Unfortunately, the baud rate of /dev/ttyS0 is derived from a dynamic system clock (slows down to save power), and thus is completely useless. However, if you disable bluetooth by adding the line dtoverlay=pi3-disable-bt to the end /boot/config.txt, the serial port ttyAMA0 (which is normally used for bluetooth) will be rerouted to the serial pins (8 and 10), thus fixing the clock problem. For more information see
  • You'll need to add your user to the dialout group and logout or restart to be able to have permissions to access the serial port. Don't simply run python with sudo or you'll have problems later on when you try to run ROS stuff. See How to add a user to the dialout group
  • To connect the PSoC directly to the Raspberry Pi you will need to snap off the programmer. To ensure a clean break, press the PSoC gently against the edge of a table or hard surface.
  • If you would rather communicate using the USB and avoid snapping off the programmer, you will need to download the USB serial bridge controller drivers. You will need to compile the drivers using make. I have not yet tried it.

Debugging Motor Connections and PID Control

1. Install Python 2

  • Linux:
sudo apt install python
  • Windows: Install Python. (Install Python 2, not Python 3!)
    • To simplify things, check the box “add python to PATH” during installation

2. Make sure pip is installed. Note: Python 2.7.9 and later should come with pip so you do not need to install it.

3. Use pip to install serial and matplotlib.

pip install pyserial
pip install matplotlib

4. Run the script below

import struct
import time
import serial
import matplotlib.pyplot as plt

ser = serial.Serial('COM11', 115200, timeout=None) #windows
#ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=None) #linux
#ser = serial.Serial('/dev/ttyAMA0', 115200, timeout=None) #linux, (read note on webpage about ttyAMA0 first)

# Note: If you would like to port the readFloat and writeFloat functions to C++, simply use the functions provided 
# in the PSoC implementation:
def writeFloat(f):
	ser.write(struct.pack('>i', int(f*1000)))
def readFloat():
	return float(struct.unpack('>i',[0])/1000
def setPower(p1, p2, p3):
def setSpeed(s1, s2, s3):
def setPID(motor, p, i, qpps): #use motor = 0 to set all motors
def setT(period_ms, tau_ms):
def getSpeed():
	return readFloat(), readFloat(), readFloat()
def getEncoderCount():
	return readFloat(), readFloat(), readFloat()
def disengage():
totalTime = 3   #seconds
sampleRate = 50 #samples per second
#pulsePerRotation = 4955 #Old motors
pulsePerRotation = 116.2 #New motors

speedM1 = 2 # rot/s
speedM2 = 2 # rot/s
speedM3 = 2 # rot/s

# Set the PIDQ values for all motors
setPID(0, 1, 1, 800)

# Set tick period (triggers PID control) and velocity filter corner frequency
setT(20, 50)

#initialize arrays
times = []
speedsM1 = []
speedsM2 = []
speedsM3 = []

setSpeed(speedM1*pulsePerRotation, speedM2*pulsePerRotation, speedM3*pulsePerRotation)

for i in range(0,totalTime * sampleRate):
	speed = getSpeed()

plt.plot(times, speedsM1) #blue
plt.plot(times, speedsM2) #green
plt.plot(times, speedsM3) #red
plt.legend(['motor1', 'motor2', 'motor3'], loc='lower right')
plt.plot([0, totalTime], [speedM1, speedM1])

plt.ylabel('Rotations per second for ' + str(len(speedsM1)) + ' samples')
plt.xlabel('Sample Time (s)')

5. Tune the PD control without the integrator

  • If motor graph shows a negative speed, switch the encoder channel A and B wires
  • Try commanding each motor individually to ensure the encoders wires are connected to the correct motor
  • Change each motor QPPS until you get within 10% of commanded velocity for all three motors
  • You may need to set different QPPS for each motor if your robot is unbalanced

6. Add integrator control

  • This should eliminate steady-state error
  • Integrator values that are too large will lead to instability (see graph below)

Path Planning and Ball Prediction


  • Discusses planning paths to push ball into the goal.
  • Assumes the ability to predict the future location of the ball.
  • Updated for three wheel robots.


  • Overview of the Rapidly exploring Random Tree (RRT) algorithm for path planning.
  • From Beard, McLain, Small Unmanned Aircraft, Princeton University Press, 2012.
  • Discussed on 2/10/2014.

  • Matlab code that implements the RRT algorithm.
  • From Beard, McLain, Small Unmanned Aircraft, Princeton University Press, 2012.


  • Old but useful.
  • Path planning using potential field methods.


  • almost identical material to motion_planning.pdf except it contains a section on ball prediction.
  • Need to combine the two files.

(old) velocity_control_three_wheel.pdf

(old) control.pdf

(old) closed_loop_two_wheel.pdf

(old) motion_control_two_wheel.pdf

(old) ball_prediction.pdf

(old) ball_prediction2.pdf

(old) velocity_control_two_wheel.pdf

(old) goal_tending.pdf

(old) goal_scoring_intercept.pdf

(old) path_planning.pdf

(old) Simulink simulation of model reference adaptive control of a DC motor.