Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: Fix for unrealistic speed and acceleration in Balance Bot

This commit is contained in:
Ebin 2019-10-17 23:05:09 +05:30 committed by Peter Barker
parent 16d2dd4e2d
commit 3daebe974e
1 changed files with 21 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/SITL/SIM_BalanceBot.cpp
View File

@ -53,22 +53,23 @@ float BalanceBot::calc_yaw_rate(float steering)
void BalanceBot::update(const struct sitl_input &input)
{
// pendulum/chassis constants
const float m_p = 3.060f; //pendulum mass(kg)
const float width = 0.0650f; //width(m)
const float height = 0.240f; //height(m)
const float l = 0.120f; //height of center of mass from base(m)
const float i_p = (1/12.0f)*m_p*(width*width + height*height); //Moment of inertia about pitch axis(SI units)
const float m_p = 3.0f; //pendulum mass(kg)
// const float width = 0.0650f; //width(m)
// const float height = 0.240f; //height(m)
const float l = 0.10f; //height of center of mass from base(m)
const float i_p = 0.01250f; //Moment of inertia about pitch axis(SI units)
// wheel constants
const float r_w = 0.10f; //wheel radius(m)
const float m_w = 0.120f; //wheel mass(kg)
const float i_w = 0.5f*m_w*r_w*r_w; // moment of inertia of wheel(SI units)
const float r_w = 0.05f; //wheel radius(m)
const float m_w = 0.1130f; //wheel mass(kg)
const float i_w = 0.00015480f; // moment of inertia of wheel(SI units)
// motor constants
const float R = 1.0f; //Winding resistance(ohm)
const float k_e = 0.13f; //back-emf constant(SI units)
const float k_t = 0.242f; //torque constant(SI units)
const float R = 3.0f; //Winding resistance(ohm)
const float k_e = 0.240f; //back-emf constant(SI units)
const float k_t = 0.240f; //torque constant(SI units)
const float v_max = 12.0f; //max input voltage(V)
const float gear_ratio = 50.0f;
// balance bot uses skid steering
const float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
@ -93,36 +94,36 @@ void BalanceBot::update(const struct sitl_input &input)
float ang_vel = gyro.y; //radians/s
// t1,t2,t3 are terms in the equation to find vehicle frame x acceleration
const float t1 = ((2.0f*k_t*v/(R*r_w)) - (2.0f*k_t*k_e*velocity_vf_x/(R*r_w*r_w)) - (m_p*l*ang_vel*ang_vel*sin(theta))) * (i_p + m_p*l*l);
const float t2 = -m_p*l*cos(theta)*((2.0f*k_t*k_e*velocity_vf_x/(R*r_w)) - (2.0f*k_t*v/(R)) + (m_p*GRAVITY_MSS*l*sin(theta)));
const float t1 = ((2.0f*gear_ratio*k_t*v/(R*r_w)) - (2.0f*gear_ratio*k_t*k_e*velocity_vf_x/(R*r_w*r_w)) - (m_p*l*ang_vel*ang_vel*sin(theta))) * (i_p + m_p*l*l);
const float t2 = -m_p*l*cos(theta)*((2.0f*gear_ratio*k_t*k_e*velocity_vf_x/(R*r_w)) - (2.0f*gear_ratio*k_t*v/(R)) + (m_p*GRAVITY_MSS*l*sin(theta)));
const float t3 = ( ((2.0f*m_w + 2.0f*i_w/(r_w*r_w) + m_p) * (i_p + m_p*l*l)) - (m_p*m_p*l*l*cos(theta)*cos(theta)) );
//vehicle frame x acceleration
const float accel_vf_x = (t1-t2)/t3;
const float angular_accel_bf_y = ((2.0f*k_t*k_e*velocity_vf_x/(R*r_w)) - (2.0f*k_t*v/(R)) + m_p*l*accel_vf_x*cos(theta) + m_p*GRAVITY_MSS*l*sin(theta))
const float angular_accel_bf_y = ((2.0f*gear_ratio*k_t*k_e*velocity_vf_x/(R*r_w)) - (2.0f*gear_ratio*k_t*v/(R)) + m_p*l*accel_vf_x*cos(theta) + m_p*GRAVITY_MSS*l*sin(theta))
/ (i_p + m_p*l*l);
// accel in body frame due to motor
accel_body = Vector3f(accel_vf_x*cos(theta), 0, -accel_vf_x*sin(theta));
// update theta and angular velocity
ang_vel += angular_accel_bf_y * delta_time;
theta += ang_vel * delta_time;
theta = fmod(theta, radians(360));
// update x velocity in vehicle frame
velocity_vf_x += accel_vf_x * delta_time;
gyro = Vector3f(0, ang_vel, radians(yaw_rate));
// update attitude
dcm.rotate(gyro * delta_time);
dcm.normalize();
// accel in body frame due to motor
accel_body = Vector3f(accel_vf_x*cos(theta), 0, -accel_vf_x*sin(theta));
// add in accel due to direction change
accel_body.y += radians(yaw_rate) * velocity_vf_x;
// update x velocity in vehicle frame
velocity_vf_x += accel_vf_x * delta_time;
// now in earth frame
Vector3f accel_earth = dcm * accel_body;
accel_earth += Vector3f(0, 0, GRAVITY_MSS);
@ -137,8 +138,6 @@ void BalanceBot::update(const struct sitl_input &input)
dcm.identity();
gyro.zero();
velocity_vf_x =0;
theta = radians(0);
ang_vel = 0;
}
// work out acceleration as seen by the accelerometers. It sees the kinematic