Eurobot 2007 DIIT Team Università degli Studi di Catania 13 giugno 2007

Eurobot 2007DIIT Team

Università degli Studi di Catania13 giugno 2007

Motor Controller

Hardware Design Software Design

What means Motor Controller?What means Motor Controller?

•The Motion module changes motor’s 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 Design…The choice!Hw Design…The choice!

The solution….PIC18FXX31

Hardware Design

Hardware Design (1/7)Hardware Design (1/7)

• We realized a solution to manage two independent motors with an We realized a solution to manage two independent motors with an uniqueunique microcontroller 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)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.



•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 aren’t 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.


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


•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: it’s 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)


Software Design

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

elseset_power_pwm0_duty(1024);

Delay

Speed Evaluation Motor 0

Speed Evaluation Motor1

PID_0

PID_1

Speed Evaluation Task

Odometry

EmbeddedCommand

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

ComponentsAmong 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



PID_0

PID_1


Odometry

EmbeddedCommand

Command Handler

PID Task

Odometry 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



PID_0

PID_1


Odometry

EmbeddedCommand

Command Handler

PID Task

Odometry Task


Software Subsystems– Next steps: integral and derivative terms.

9 // Proporzional term10 Control_new = motorX->Control_old + (motorX->Kp *Error);11 12 // Integral term13 motorX->Sum_G += Error;14 Control_new += motorX->Ki/SAMPLE_RATE * motorX->Sum_G;15 16 // Differential term17 Control_new += (motorX->Kd * SAMPLE_RATE * (Error - motorX->Old_error_G));1819 // Range Control20 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 // Error29 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



PID_0

PID_1


Odometry

EmbeddedCommand

Command Handler

PID Task

Odometry Task


)()()(

)()()(

)(

cos)2/sin(

sin)2/cos(

kkk

kkk

k

dt

dydt

dxdt

d

Software Subsystems• General method (Kinematic equations) to evaluate the correct robot’s 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, it’s possible to integrate the equations with Eulero method to obtain:

Kinematic Model

)()1()1()(

)()1()1()(

)1()1()(

cos

sin

kskkk

kskkk

skkk

Tyy

Txx

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

nnD

T

nnD

T

ds

s

ds

s

)(1

2

)(1

)()1()(

)()1()(

)1()(

cos2

)(

sin2

)(

)(

kds

kk

kds

kk

dskk

nnDyy

nnDxx

L

nnD

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_COMPASStheta = TO_RADIANTS(ceil(compass -compass_zero))+ theta_zero;

if (theta > PI) theta = - (2 * PI - theta); if (theta < -PI) theta = 2 * PI + theta;

#elsetheta = 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– Earth’s magnetic field moves itself in time ( “temporal” declination);– It isn’t direct towards Geographic North ( “area” declination);– It’s deviate by fixed and mobile magnetic materials ( “local” declination and “accidental” declination);– It’s 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:

Documents

Eurobot 2007 DIIT Team Università degli Studi di Catania 13 giugno 2007