aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: pass vehicle speed to attitude control pitch control

Browse Source
This commit is contained in:
Randy Mackay 2018-08-08 14:42:44 +09:00
parent 106b8abab7
commit 19d943106b
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
APMrover2/balance_bot.cpp
View File

@ -7,8 +7,15 @@ void Rover::balancebot_pitch_control(float &throttle)
// calculate desired pitch angle
const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max);
// calculate speed from wheel encoders
float veh_speed_pct = 0.0f;
if (g2.wheel_encoder.enabled(0) && g2.wheel_encoder.enabled(1) && is_positive(g2.wheel_rate_control.get_rate_max_rads())) {
veh_speed_pct = (g2.wheel_encoder.get_rate(0) + g2.wheel_encoder.get_rate(1)) / (2.0f * g2.wheel_rate_control.get_rate_max_rads()) * 100.0f;
veh_speed_pct = constrain_float(veh_speed_pct, -100.0f, 100.0f);
}
// calculate required throttle using PID controller
throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, G_Dt) * 100.0f;
throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, veh_speed_pct, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, G_Dt) * 100.0f;
}
// returns true if vehicle is a balance bot