Eurobot 2007

DIIT Team
Universit degli Studi di Catania
13 giugno 2007

Motor Controller
Hardware Design Software Design
What means Motor Controller?
The Motion module changes motors velocity and
controls trajectory, by means of a closed-loop control,
thus allowing the robot to reach the desired object or
position.
Movement is based of two separately driven wheels.The
basic idea is that the robot can change its direction by
varying the relative rate of rotation of its wheels and
hence does not require an additional steering motion.
The general structure:
embedded system,
microcontroller,
h-bridge and motor.
Microchip technology offers a broad product portfolio to provide
solutions for stepper, DC,brushless motor

MCUs 8-bit and 16-bit families to provide on-chip peripherals to
design high-performance and precision motor controller system.
Indeed, these peripherals include module that can generate the
appropriate driving signals for motors.
High-speed 10-bit analog-to-digital converter
Specialized motor control PWM, Capture and Compare
Quadrature encoder (interface or input capture)


Hw DesignThe choice!
The solution.PIC18FXX31
Hardware Design
Hardware Design (1/7)
We realized a solution to manage two independent motors with an
unique microcontroller. The PIC18F2431 was chosen as
microcontroller, to achieve high performance, power control and safety
management.

It is equipped with some special
peripherals as:

Motion Feedback Module (MFM)
with3-channel Input Capture Module
and Quadrature Encoder Interface.

14-bit resolution Power Control
PWM (PCPWM) Module with
programmable dead-time insertion,
complementary outputs.
Hardware Design (2/7)
The following figure represents schematic circuit of motor controller
board.
The MFM Quadrature
Encoder Interface
provides precise rotor
position feedback and/or
velocity measurement.

The 3-channel Input
Capture can be used to
detect the rotor state using
Hall sensor feedback: in
particular, Input Capture
can measure edge-
trigger, period or pulse
signal and it is equipped
with programmable
prescaler to set
periodicity of increase of
the counter.
Hardware Design (3/7)
Hardware Design (4/7)
The PCPWM can generate up to six complementary PWM outputs with dead-
band time insertion. For this reason, we used Locked Anti-Phase
configuration (LAP mode) to control the H-Bridge.
Combination Polarity Effect
A & D Forward The motor
goes forward
B & C Backward
The motor goes
backward
A & B Blocked The motor is
stopped
None Free-running The motor is
out of gear
PWM is square wave with variable duty-cycle (low voltage and costant
frequency signal) used to drive H-Bridge.
Duty cycle 0 : max
rotation speed in a
verse
Duty cycle 50%:
motors are stop
Duty cycle 100%:
max rotation speed
in other verse

We used Locked Anti-Phase mode so that motors arent in Free
Running Mode, when dutycycle is set 50% (to stop motors).

The STMicroelectronics L298N
was selected to deliver power to the
motors: it is a high voltage, high
current dual full-bridge driver
designed to accept standard TTL
logic levels and drive inductive loads
such as relays, solenoids, DC and
stepping motors. Two enable inputs
are provided to enable o disable
independently of the input signals.
Hardware Design (5/7)
H-Bridge outputs is connected to Shottky diodes allowing the chip to drive
inductive load (i.e. DC-brushed gearmotors with Reduction Ratio of 76.84:1 and
No Load Speed of 81 RPM).
Hardware Design (6/7)
The motors have Hall-Effect sensors onboard: each device includes a
voltage regulator, quadratic Hall voltage generator, temperature stability circuit,
signal amplifier, Schmitt trigger and open collector output on a single silicon
chip.

Connecting pull-up restistor (4,7 kOhm) at encoder output, we can read
pulses with microcontroller. In particular, it measures rotation speed with Input
Capture interface.
To communicate with Embedded System using RS-485 standard, we
use MAX485CPA: its a transceiver for communication.
It contains one receiver and one driver and allow to transmit up to
2.5Mbps.

ENABLE_485 signal permits to select a data direction (transmission
or reception)
Hardware Design (7/7)
Software Design

if (motor0.on == MOTOR_ON)
set_power_pwm0_duty(pwm0)
;

else
set_power_pwm0_duty(1024);

Delay
Speed Evaluation
Motor 0
Speed Evaluation
Motor1
PID_0
PID_1
Speed Evaluation Task
Odometry
Embedded
Command
Command
Handler
PID Task
Odometry Task
Handler Command Task
Main tasks:

Speed evaluation and control

Speed Evaluation Task

Pid Task

Calculation of the absolute position of
the robot

Odometry

Interaction with the outside (RS485)

Handler Command Task
Sw Design Flow Diagram
Components
Among all components there are:
t_motor_info - (struct) : a data dictionary which is a central location for storing many
kinds of data.

typedef struct {

float current_speed;
boolean current_direction;
int16 target_speed;



boolean on;
boolean pid;
boolean encoder_enabled;





float Kp; / * 'P' proportional gain */
float Ki; / * 'I' integral gain */
float Kd; / * 'D' derivative gain */

} t_motor_info;
- Current Speed - Calculated speed coming from Encoders
- Current Direction - Actual direction of the motors
- Target Speed - New desired speed that has been applied

Flags used to activate or disable the respective controls
- Boolean on -
- Boolean pid -
- Boolean Encoder enabled -


Constants used in the PID control.
- Proportional gain -
- Integral gain -
- Derivative gain -
Software Subsystems
Speed Evaluation
Through encoder ticks calculates motors speed.
(built-in feature of PIC18F2431, called MOTION
FEEDBACK MODULE)

MOTION FEEDBACK MODULE

- Determines the period between two different pulses
of the motor encoder.

- Uses:
a special timer (Timer5) programmed to
increment at a frequency of Tclock/8.

two input pins (CAP1 & CAP2), connected to the
output of the encoders of motor 0 & 1.

- Timer5 value is stored in a register: CAPx-BUF.

- Encoder Interrupt Service Routine monitors any
counter overflow.
Delay
Speed Evaluation
Motor 0
Speed Evaluation
Motor1
PID_0
PID_1
Speed Evaluation Task
Odometry
Embedded
Command
Command
Handler
PID Task
Odometry Task
Handler Command Task
Software Subsystems
PID Task
Compares the current speed of a motor to the desired
speed to reduce the error through the use of the PID gains.

The output of the PID task is directly a value
representing the PWM Duty Cycle.

1 float PID_Control(t_motor_info * motorX){
2 float Error ;
3 float Control_new ;
4
5 Error =motorX->target_speed motorX->current_speed;
6 if (fabs (Error) <1.0)
7 Error =0;
8

First operation: determination of the error

dead band: in order to avoid instability in the control
(Line 6)
Delay
Speed Evaluation
Motor 0
Speed Evaluation
Motor1
PID_0
PID_1
Speed Evaluation Task
Odometry
Embedded
Command
Command
Handler
PID Task
Odometry Task
Handler Command Task
Software Subsystems


Next steps: integral and derivative terms.


9 // Proporzional term
10 Control_new = motorX->Control_old + (motorX->Kp *Error);
11
12 // Integral term
13 motorX->Sum_G += Error;
14 Control_new += motorX->Ki/SAMPLE_RATE * motorX->Sum_G;
15
16 // Differential term
17 Control_new += (motorX->Kd * SAMPLE_RATE * (Error - motorX->Old_error_G));
18
19 // Range Control
20 if (Control_new > PID_MAX){
21 Control_new = PID_MAX;
22 motorX->Sum_G -= Error;
23 }
24 else if (Control_new < PID_MIN) {
25 Control_new = PID_MIN;
26 motorX->Sum_G -= Error;
27 }
28 // Error
29 motorX->Old_error_G = Error;
30 motorX->Control_old = Control_new;
31 return Control_new; } //Out is already a pwm signal

Lines 19-27: Anti-windup Control

- Values are maintained in an appropriate range to
avoid the saturation condition.

- Otherwise system never settles out, but just slowly
oscillates around the target position (wind up).
Line 10: (motorX->Control_old)

-If the error was zero the PID output must only
remain constant.
Software Subsystems
Odometry Task
Determines the absolute position of the robot, in term of x, y
and .

Uses:
two internal timers of the PIC in counting mode:
- Timer0 (left wheel)
- Timer1 (right wheel).

Some terms with hypothesis that speed value
remain constant in every iteration :

- L = distance between contact points of
wheels and ground .
- N = tick number for every complete wheel
rotation
- W = wheel diameter
- D = linear distance covered by a wheel in
a tick.
- ns = tick number of left wheel
- nd = tick number of right wheel
Delay
Speed Evaluation
Motor 0
Speed Evaluation
Motor1
PID_0
PID_1
Speed Evaluation Task
Odometry
Embedded
Command
Command
Handler
PID Task
Odometry Task
Handler Command Task
) ( ) (
) (
) ( ) (
) (
) (
cos ) 2 / sin(
sin ) 2 / cos(
k k
k
k k
k
k
dt
dy
dt
dx
dt
d

Software Subsystems

General method (Kinematic equations) to evaluate the correct robots position and orientation.

Supposing that the robot goes in a:
rectilinear motion with linear speed
circular motion with angular speed











Considering a sampling time T
s
in order to and remain constant, its possible to integrate the equations
with Eulero method to obtain:
Kinematic Model
) ( ) 1 ( ) 1 ( ) (
) ( ) 1 ( ) 1 ( ) (
) 1 ( ) 1 ( ) (
cos
sin
k s k k k
k s k k k
s k k k
T y y
T x x
T









Software Subsystems
Given that velocity and are evaluated from encoder ticks with equations:





The previous formulas become:







Important Hardware Problem:
Encoder based on Hall sensors.

Signals affect by a time shift (due to a perturbation provoked by the magnetic
field of the motors).
L
n n D
T
n n D
T
d s
s
d s
s
) ( 1
2
) ( 1

) ( ) 1 ( ) (
) ( ) 1 ( ) (
) 1 ( ) (
cos
2
) (
sin
2
) (
) (
k
d s
k k
k
d s
k k
d s
k k
n n D
y y
n n D
x x
L
n n D

Solution: C mpass
Aim: to produce a precise number to represent the direction the robot is facing.

The compass uses two magnetic field sensors, sensitive to detect the Earths
magnetic field, mounted at right angles to each other.
compass = read_compass ();

#ifdef USE_COMPASS
theta = TO_RADIANTS(ceil(compass -compass_zero))+ theta_zero;
if (theta > PI) theta = - (2 * PI - theta);
if (theta < -PI) theta = 2 * PI + theta;
#else
theta = theta + (TICK_DISTANCE * (ns-nd))/ WHEEL_DISTANCE;
#endif
x = x + (TICK_DISTANCE * (ns+nd)*sin(theta))/2;
y = y + (TICK_DISTANCE * (ns+nd)*cos(theta))/2;
General plan of the code to permit
evaluations with or without the
compass assistance.
Odometry Limits
Requires initialization
Subject to cumulative errors (drift)
Systematic
Unequal wheel diameters
Actual diameter different from nominal value
Actual wheelbase different from nominal value
Misaligned wheels
Finite encoder resolution
Finite encoder sampling rate
Non-Systematic
Travel over uneven floor
Travel over unexpected objects on floor
Wheel slippage
Slippery floor
Overacceleration
Fast turning
Interaction with external bodies
Internal forces(castor wheel)
Non-point wheel contact with floor

Compass Limits
Earths magnetic field moves itself in time ( temporal declination);
It isnt direct towards Geographic North ( area declination);
Its deviate by fixed and mobile magnetic materials ( local declination and accidental declination);
Its deviate by robot itself (compass deviation).
Limits
The bi-directional
square path
as a benchmark test
Possible Solutions
Reduction of odometry Errors
No vehicles with a small wheelbase (more prone to orientation
errors).
No castor wheels (which bear significant portion of weight and
are likely to induce slippage).
Synchro-drive design provides better odometric accuracy.
The wheels should be knife-edge thin and not compressible
Auxiliary passive Wheels to reduce encoder errors.
Better robot structure.

Compass
external magnetic fields isolation.
Gyroscope uses (difficult for device size).
Performance Evaluations
Parameter setting for system performance and PID promptness
evaluation:
target speed =80 [tick/time]
initial speed =0 [tick/time]
Kp =6
Ki =0
Kd=0

As figure shows, using only proportional term we have a good time
response: