FeedBack Control
for your FIRST robot’s drivetrain
David Giandomenico
Team mentor for
Lynbrook Robotics – Team #846
<DGiandomenico@LynbrookRobotics.com>
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #1
Driving Smart
Make your robot easy to drive
• Using brakes
More control when turning
• Closed Loop (Feedback) control
Precision driving
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #2
Turning without Brakes
When the joystick is at neutral position,
the robot coasts.
•
When turning, the inboard motor coasts,
gradually slowing, causing the robot to spiral!
•
If reverse power is applied during a turn,
the robot easily spins out.
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #3
Normalized Quantities
Speed Nˆ  N / N noLoad
Torque Tˆ  T / TStall
Voltage Vˆ  V / 12
Current Iˆ  I / I Stall
The reference quantities must be measured on the Robot
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #4
Ideal Torque vs Joystick (PWM)Input
Torque vs Input
Ideal case:
• Robot not moving
50%
• no drag
100%
50%
-50%
0%
0%
-100%
Effective Torque (% Tstall)
100%
• Jaguar (not IFI Victor884)
-50%
-100%
Applied Duty Cycle
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #5
Robot is Non Linear when Moving
PWM Speed Controller can’t “sink”
current.
Torque
Robot Moving 50% speed
“ESC” model
50%
-50%
100%
50%
0%
-50%
0%
-100%
Effective Torque (% Tstall)
100%
+
+12V
M
-100%
-150%
Applied Duty Cycle
EMF < 12V
−
Robot coasts when input is less than robot speed!
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #6
Use Brakes to Fix the Curves
Torque w/Brakes
Robot Moving 50% speed
The Fix:
1. Measure the robot speed.
50%
2. Calculate:
-50%
100%
50%
0%
-50%
0%
-100%
Effective Torque (% Tstall)
100%
(Robot_Speed – Joystick_Input)
3. Apply brakes proportional to this
amount.
-100%
-150%
Applied Duty Cycle
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #7
Dynamic Braking
+
1. Rotation generates voltage
2. Short allows current flow
−
David Giandomenico
3. Current creates magnetic
field that resists rotation
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #8
H-Bridge: Inside your Motor Controllers
12 V
S-1
S-3
+
S-2
David Giandomenico
−
S-4
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #9
H-Bridge:
Inside the IFI SPIKE Relay Module
12 V
½ Tyco 2VR-1001
½ Tyco 2VR-1001
Fwd Relay
M+
−
+
M
Rev Relay
M−
INPUTS OUTPUTS
Motor is “Braked”
Even when unpowered!
David Giandomenico
Fwd
Rev
M+
M–
MOTOR
FUNCTION
0
0
GND
GND
Off (Braked)
0
1
GND
12V
Reverse
1
0
12V
GND
Forward
1
1
12V
12V
Off (Braked)
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #10
Dither Braking
1 2 3 4 5 6 7
8 9 0 …
Packet or loop_count
const int brakebit[8][7]= {
{0,0,0,0,0,0,0},
{1,0,0,0,0,0,0},
{1,0,0,1,0,0,0},
. . . };
int dither =
brakebit[ brake_amt ][ packet % 7];
ApplyBrake( dither );
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #11
Dynamic Braking Increases with Speed
Common “caliper brake”
applies constant drag.
Fbrake    Fclamp
Bike brake
Car brake
Dynamic brakes apply
drag proportional to
speed.
Tbrake  Tstall  %Speed
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #12
Dynamic Braking
+
M
+
V

KV
−
−
1. Rotation generates voltage proportional to speed
2. Short allows current flow, I = V/R
3. Creates magnetic field that resists rotation, T = Kt I
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #13
Faster → Brakes Harder
Motor voltage:
% EMF  % Speed
With no load, and 12V applied,
EMF  12V
When shorted at full speed, Motor delivers most of Istall,
and develops Tstall
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #14
Quick Observations
• At 100% speed, when the motor is shorted, the motor
resists with about 100% of its stall torque.
• At 40% speed, we get ~40% of the stall torque.
How can we deliver
a known amount of braking?
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #15
Calculate duty cycle, DC, to apply the brakes.
When shorted, and the motor is running at speed N,
Tbrake  Tstall  ( N / N no _ load )
Or, with normalized quantities, when the motor is shorted,
Tˆbrake  Nˆ
To achieve a normalized braking torque, T,
Pulse brakes with a duty cycle, DC:
Tˆbrake  Nˆ  DC
DC  Tˆbrake Nˆ
David Giandomenico
Nˆ  Tˆbrake  0
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #16
Adjusting Brakes for Speed
Torque w/Brakes
Robot Moving 50% speed
The Fix:
1. Measure the robot speed.
50%
2. Calculate:
B = (Robot_Speed – Joystick_Input)
-50%
100%
50%
0%
-50%
0%
-100%
Effective Torque (% Tstall)
100%
3. To brake at rate ‘B”, calculate
Duty Cycle:
if (N>B>0) then DC = B/N;
else DC = 100%;
-100%
-150%
Applied Duty Cycle
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #17
Effects of Friction
Torque w/ 10% Drag
Robot Stopped
Torque w/ 10% Drag
Robot Moving 50% speed
100%
-50%
50%
-50%
100%
50%
0%
-50%
0%
-100%
100%
50%
-50%
0%
0%
Effective Torque (% Tstall).
50%
-100%
Effective Torque (% Tstall).
100%
-100%
-100%
Applied Duty Cycle
-150%
Applied Duty Cycle
Friction adds braking
Requires tweaking the Brake algorithm
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #18
Benefits of Dynamic Braking
• Turn without spinning out
• Come to a straight dead stop
• Linearized response for your
FeedBack Control Systems
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #19
Closed Loop Control
for your FIRST robot’s drivetrain
•
•
•
•
What is Closed Loop Control?
Pros & Cons – Open Loop vs Closed Loop
Basic Velocity Control – demo & C++ code
Tuning the parameters
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #20
Control Systems
Open Loop System
Controller
Input
System
Output
Closed Loop System
Input +
Measured
error
n
Controller
System
Output
−
Measured output
David Giandomenico
Sensor
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #21
Open Loop
Pros
Closed Loop
Pros
+ Simple
+ Precision maneuvering
+ Easier to drive
+ The Future
• Reliable
+ Fast to build
• More time to practice
Cons
– Harder to drive
– Less Capable
David Giandomenico
Cons
– More parts
– Less reliable
– More time to build
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #22
Closed-Loop Control System
Input Speed
or Position
+
+
(joystick,
preset button,
autonomous, etc.)
G
E.S.C.
−
Out
−
Sensor
Potentiometer, Encoder,
Gear Tooth Sensor,
Hall effect Sensor, …
David Giandomenico
M
Feedback Control for your FIRST Robot’s Drivetrain
Yet more sensors:
Accelerometers & Gyros
Optical Distance Meas.
Cameras,
Ultrasonic
:
Dec 2010 WRRF Workshops #23
Speed Controller
cRio
Input
+
+
ESC
n
M
Output
(speed)
+
−
G
Measured
error
n
+
David Giandomenico
−
Measured
output
Speed Sensor
(Encoder)
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #24
Code Implementation:
Forward Motion
DriveOutput
ClosedLoopDriveTrain::ComputeArcadeDrive(float rawFwd, float rawTurnRateRPS)
{
:
:
Code for turning robot was here
:
float robotSpeed = m_encoders.GetNormalizedForwardSpeed();
float fwdError = rawFwd - robotSpeed;
Input
cRio
ESC
n
+
Output
+
+
M
(speed)
−
float fwdCorrection = fwdError * m_pGainFwd;
G
Measured
error
float newFwd = rawFwd + fwdCorrection;
n
+
−
Measured
output
Speed Sensor
(Encoder)
return m_dbsDrive.ComputeArcadeDrive(newFwd, newTurn);
}
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #25
Code Implementation:
Turn at Rate
DriveOutput
ClosedLoopDriveTrain::ComputeArcadeDrive(float rawFwd, float rawTurnRateRPS)
{
float turningRate = m_encoders.GetNormalizedTurningSpeed();
float turningError = rawTurnRateRPS - turningRate;
float turningCorrection = turningError * m_pGainTurn;
float newTurn = rawTurnRateRPS + turningCorrection;
:
: Code for driving robot straight was here
:
return m_dbsDrive.ComputeArcadeDrive( newFwd, newTurn );
}
How do we set the gain, m_pGainTurn ?
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #26
Calibration and Tuning
1. Calibrate: Normalize your Robot’s Speed
a.
b.
Measure no load speed on test stand
Measure fastest rate robot can turn on floor
2. Tune the Gains
a.
b.
Increase gain with robot on floor until oscillations start.
Back-off gain.
TIP: Use a fast adjustment method, such as:
a potentiometer,
FTP to a data file on the cRio,
or, a driver-station control.
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #27
Feedback System
“Closed Loop”
In
+
G
Out
−
H
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #28
Feedback System
“Closed Loop”
In(f)
+
Out(f)
G
−
H
Out  G  In  H  Out 
Out  G  In  G  H  Out
Out ( f )
G( f )

In( f ) 1  G ( f )  H ( f )
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #29
Moving Forward…
Where we started:
Basic Functionality
Where we are going:
Performance!
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #30
FeedBack Control
for your FIRST robot’s drivetrain
David Giandomenico
Team mentor for
Lynbrook Robotics – Team #846
<DGiandomenico@LynbrookRobotics.com>
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #31
Robot Shakes: Filter the Error
DriveOutput ClosedLoopDriveTrain::ComputeArcadeDrive(float rawFwd, float rawTurnRateRPS)
{
float turningRate = m_encoders.GetNormalizedTurningSpeed();
// update the running sum with the error
float turningError = rawTurnRateRPS - turningRate;
turningError = m_turnRunningError.UpdateSum( turningError );
float turningCorrection = turningError * m_pGainTurn;
float newTurn = rawTurnRateRPS + turningCorrection;
:
: Code for driving robot straight was here
:
return m_dbsDrive.ComputeArcadeDrive( newFwd, newTurn );
}
Concept of integrating error:
Faster changing errors are not weighted
as heavily as slow changing errors.
Consider:
David Giandomenico
 cos kt 
sin kt
k
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #32
One Method:
Running Sum Filter
Running sum, Sn with a decay constant, a
Sn  X n  a  Sn1
Sn converges to:
k
Sn 
1 a
n
Examples:
1a + ½a + ½2 a +½3 a + … + ½n a = 2a
1 + ¾2+ ¾3 + … + ¾n = 4
So, to prevent modifying our loop gain, return a scaled output, On
On  (1  a) S n
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #33
Code Implementation:
Integrate Error
Example Call:
turningError = m_turnRunningError.UpdateSum(turningError);
-------------------------------------------class RunningSum {
:
: constructor was here
:
public:
float UpdateSum(float x) {
m_runningSum *= m_decayConstant;
m_runningSum += x;
return m_runningSum * (1 - m_decayConstant);
}
private:
float m_decayConstant;
float m_runningSum;
};
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #34
FeedBack Control
for your FIRST robot’s drivetrain
David Giandomenico
Team mentor for
Lynbrook Robotics – Team #846
<DGiandomenico@LynbrookRobotics.com>
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #35
Jaguar CAN; limited by serial port
• Jaguar CAN bus 1Mbs
125KB/s, or 1.2KB every 1/100 sec
• Serial Port: 56KBaud
= ~ 5.6KB/sec
or 56 bytes in 1/100 sec
David Giandomenico
Feedback Control for your FIRST Robot’s Drivetrain
Dec 2010 WRRF Workshops #36