Hardware ImplementationSelf Balancing Robot

Design Objective

› Implement base to develop code

› Define pins to use for code

› Use material discussed

› Balance Robot using – Gyroscope– Accelerometers– PID loops– Kalman Filter

System Block Diagram

STM32F4ARM-4

uController

Accelerometer Gyro

Stepper Driver

Stepper

Battery11.1V

5V regulator

MS1

STEPDIR

SWITCHFUSE

I2C

SPI

UltrasoundSensor

MS2MS3

Stepper Driver

StepperSTEPDIR

PB9, PB6

PE3,PA5, PA6, PA7

PD0PD1

PD2

PD12

PD13

PD14

PD15

ENABLEPD3

Accelerometer - change lateral velocity

› If robot falls forward– The accelerometer measures inclination angle– Ax = reading on x-axis– Ay = reading on y-axis– g = gravity (9.8 m/s2 or 32.17 ft/s2)

– Ø= Angle of Fall (want to keep very small)

› For very small angles, approximate by∅ � ��/� (note small error but good approx.)

› Accelerometer must be placed at the center of gravity

› Accelerometer at rest measures g= 9.81g= 9.81g= 9.81g= 9.81– They are very sensitive to noise– It needs another gyroscope to offset error

Gyroscope – rotational acceleration

› If robot falls forward– Gyro measures Angular Velocity (degrees/sec)

› How fast is the sensor rotating

– Integrating between a time will yield degrees.› Fancy way of saying: “multiply measured value based on small time frame”

› 3 axis Gyro measurement of board being rotated

Offset : Offset : Offset : Offset : At rest board is almost zero but not exactly. Compensate by subtracting a measured offset

Drift: Drift: Drift: Drift: Offset is not constant, it “drifts” with time, input voltage or temperature.

Combining the Sensors› Accel= accurate angle calculation at no acceleration

› Gyroscope = accurate short-time angle measurements

› Use the model:

∅��� � � ∅��� � �∆ � ��∅��

∅������ : Current angle being estimated (Current Error)

∅������ : Prior angle estimation

�∆�: Gyro measurement (multiplication based on time elapsed)

∅��: Accelerometer measurement (Ax/g)

�: Gyro meas. weight ( >0.9)

: Accelerometer meas. weight (< 0.1)

= Average over time

� � = 1

Motor Control (PID Loop)› Rotate wheels in the falling direction– PID: Move faster the larger the angle or slower the smaller angle– PPPProportional = Present (Present Error)– IIIIntegral = Past (sum accumulated Errors)– DDDDerivative = Future (expected future Error)

!"�#$ � %&'(')�*## ×,- � .//*## × ,0 � 12�2&'�## ×,3

› Kp, Ki and Kd are the PID tuning factors

› .//*## should near zero if properly tuned

› Since we are using stepper motors we will not use a PID loop– Rather we will use the ability of controlling degrees of movement to interface to the angle of error we want to correct

Stepper Motor Detail Connections

› MS1-3: Steps resolution

› RESET: Disable Motor

› STEP:

› DIR:

Source: https://www.pololu.com/product/1182

Ideas to add on

› Wheel hub with set screw, with brass going into hub

› Oversized perf board to hold controller and add mounting holes