Zurück (C) Christof Ermer, Regensburg Gratis Counter
10.03.2011



http://tlb.org/scooter.html
Gyroscope:  "packed with five solid-state, vibrating-ring, angular-rate sensors"

Putting it all together

What takes many paragraphs to explain is surprisingly simple to code. Here is the basic pseudocode of the balance algorithm, complete with the numbers which made my scooter feel stable and responsive.
Inputs
angle, angle_rate: the tilt angle of the scooter in radians and its derivative in radians/sec 
steer_knob: the reading from the steering knob, between -1 and +1.
Balance
balance_torque = 5.0 * (angle - rest_angle) + 0.4 * angle_rate
Limit top speed by tilting back
overspeed = max(0, cur_speed - 0.5) 
if (overspeed > 0) { 
  overspeed_integral = min(0.4, overspeed_integral + min(0.2, overspeed+0.05) * dt) 
} 
else { 
  overspeed_integral = max(0, overspeed_integral - 0.04*dt) 
} 
rest_angle = 0.4*overspeed + 0.7*overspeed_integral
Steer. Decrease steering rate at high speed
steer_cmd = 0.07/(0.3+abs(cur_speed)) * steer_knob
Track current speed
cur_speed += 1.2 * balance_torque * dt
Differential steering
left_motor_pwm = balance_torque + cur_speed + steer_cmd 
right_motor_pwm = balance_torque + cur_speed - steer_cmd
Outputs
left_motor_pwm and right_motor_pwm directly set the duty cycle of the pulse width modulator for the wheel controller, and range from -1 to +1 (+1 is 100% forward, -1 is 100% reverse.)