Software Design

1
Eurobot 2007 DIIT Team Università degli Studi di
Catania 13 giugno 2007
2
Motor Controller
Hardware Design
Software Design
3
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.

4
Hw DesignThe choice!

• 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)?

The solution.PIC18FXX31
5
Hardware Design
6
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.

7
Hardware Design (2/7)?

• The following figure represents schematic circuit
  of motor controller board.

8
Hardware Design (3/7)?

• 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.

9
Hardware Design (4/7)?

• PWM is square wave with variable duty-cycle (low
  voltage and costant frequency signal) used to
  drive H-Bridge.

• 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.

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).

10
Hardware Design (5/7)?

• 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.

• 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.841 and No Load Speed of 81 RPM).

11
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.

12
Hardware Design (7/7)?

• 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)

13
Software Design
if (motor0.on MOTOR_ON)
set_power_pwm0_duty(pwm0) else set_power_pwm0_d
uty(1024)
14
Sw Design Flow Diagram

• 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

15
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

- 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 -
16
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.

17
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-gttarget_speed
  motorX-gtcurrent_speed
• 6 if (fabs (Error) lt 1.0)
• 7 Error 0
• 8
• First operation determination of the error
• dead band in order to avoid instability in the
  control
• (Line 6)

18
Software Subsystems

• Next steps integral and derivative terms.
• 9 // Proporzional term
• 10 Control_new motorX-gtControl_old
  (motorX-gtKp Error)
• 11
• 12 // Integral term
• 13 motorX-gtSum_G Error
• 14 Control_new motorX-gtKi/SAMPLE_RATE
  motorX-gtSum_G
• 15
• 16 // Differential term
• 17 Control_new (motorX-gtKd SAMPLE_RATE
  (Error - motorX-gtOld_error_G))
• 18
• 19 // Range Control
• 20 if (Control_new gt PID_MAX)
• 21 Control_new PID_MAX

• Line 10 (motorX-gtControl_old)
• If the error was zero the PID output must only
• remain constant.

• 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).

19
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

20
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 Ts in order to ? and
  ? remain constant, its possible to integrate the
  equations with Eulero method to obtain

Kinematic Model
21
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).

22
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 gt PI)
theta - (2 PI - theta) if (theta lt -PI)
theta 2 PI theta else theta theta
(TICK_DISTANCE (ns-nd))/ WHEEL_DISTANCE endif
x x (TICK_DISTANCE (nsnd)sin(theta))/2
y y (TICK_DISTANCE (nsnd)cos(theta))/2
General plan of the code to permit evaluations
with or without the compass assistance.
23
Limits

• 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)

The bi-directional square path as a benchmark
test
24
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).

25
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