Print PDF
Favorite
Email
Step 11The control part of the controller.
Maybe the rest of the software is just setup for this part: the feedback control algorithm that actually decides how to correct for leaning and keep the platform balanced. For the Segstick, I used a proportional-derivative (PD) controller, a subset of PID control.
Proportional. This is a corrective action that scales proportionally to the angle. If the stick leans forward twice as far, the corrective action is twice as great. This is like the "spring constant" of the system, applying a restoring force as the stick moves away from vertical.
Derivative. This is a corrective action that scales proportionally to the derivative of the angle, or the angular rate. If the stick is falling twice as fast, the corrective action is twice as great. This is like the "damping constant" of the system, applying a force that resists rotation in either direction.
Together, this forms a mass-spring-damper system, except with virtual springs and dampers. The relative spring constant and damping constant affect how much the system oscillates as it corrects for angular displacement. Simple, right? Here's the code:
output += angle * KP + rate * KD;
Yep, simple. output is the command to be sent to the motors. KP and KD are tweaked until it balances (or goes goes totally unstable). One subtlety here is that the output command is incremented by the value output by the PD controller. So, if the angle is held at some offset from vertical, the motor command will keep increasing. It's like adding an extra integral to the system, as shown in the block diagram below.
Usually in feedback diagrams, the feedback path has a negative sign, but it really depends on the way the motors are set up. Choosing the sign for output can either be done in software (-= instead of +=) or in hardware by just swapping the motor leads. If both wheels start moving the wrong way in response to the angle, you can flip the sign. If one wheel starts moving the wrong way, you can just swap its motor leads.
Proportional. This is a corrective action that scales proportionally to the angle. If the stick leans forward twice as far, the corrective action is twice as great. This is like the "spring constant" of the system, applying a restoring force as the stick moves away from vertical.
Derivative. This is a corrective action that scales proportionally to the derivative of the angle, or the angular rate. If the stick is falling twice as fast, the corrective action is twice as great. This is like the "damping constant" of the system, applying a force that resists rotation in either direction.
Together, this forms a mass-spring-damper system, except with virtual springs and dampers. The relative spring constant and damping constant affect how much the system oscillates as it corrects for angular displacement. Simple, right? Here's the code:
output += angle * KP + rate * KD;
Yep, simple. output is the command to be sent to the motors. KP and KD are tweaked until it balances (or goes goes totally unstable). One subtlety here is that the output command is incremented by the value output by the PD controller. So, if the angle is held at some offset from vertical, the motor command will keep increasing. It's like adding an extra integral to the system, as shown in the block diagram below.
Usually in feedback diagrams, the feedback path has a negative sign, but it really depends on the way the motors are set up. Choosing the sign for output can either be done in software (-= instead of +=) or in hardware by just swapping the motor leads. If both wheels start moving the wrong way in response to the angle, you can flip the sign. If one wheel starts moving the wrong way, you can just swap its motor leads.
| « Previous Step | Download PDFView All Steps | Next Step » |
1
comment
|
Add Comment
|
Aug 14, 2011. 9:28 AMdreadengineer
says:
This is a refreshingly clear explanation of both the angle sensing and the control system. Thanks!
Reply
 |
Add Comment
|
