Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   Balance Bot simulator class
      17             : */
      18             : 
      19             : #include "SIM_BalanceBot.h"
      20             : #include <stdio.h>
      21             : 
      22             : extern const AP_HAL::HAL& hal;
      23             : 
      24             : namespace SITL {
      25             : 
      26          32 : BalanceBot::BalanceBot(const char *frame_str) :
      27             :     Aircraft(frame_str),
      28          32 :     skid_turn_rate(0.15708) // meters/sec
      29             : {
      30          32 :     dcm.from_euler(0,0,0); // initial yaw, pitch and roll in radians
      31          32 :     lock_step_scheduled = true;
      32          32 :     printf("Balance Bot Simulation Started\n");
      33          32 : }
      34             : 
      35             : /*
      36             :    return yaw rate in degrees/second given steering_angle
      37             : */
      38     1410742 : float BalanceBot::calc_yaw_rate(float steering) const
      39             : {
      40     1410742 :     float wheel_base_length = 0.15f;
      41     1410742 :     return steering * degrees( skid_turn_rate/wheel_base_length );
      42             : }
      43             : 
      44             : 
      45             : /*
      46             :   update the Balance Bot simulation by one time step
      47             :  */
      48             : /*
      49             :  * The balance bot is physically modeled as an inverted pendulum(cuboid) on wheels
      50             :  * Further details on the equations used can be found here:
      51             :  * 1) http://robotics.ee.uwa.edu.au/theses/2003-Balance-Ooi.pdf page 33 onwards
      52             :  * 2) http://journals.sagepub.com/doi/pdf/10.5772/63933
      53             :  */
      54     1410742 : void BalanceBot::update(const struct sitl_input &input)
      55             : {
      56             :     // pendulum/chassis constants
      57     1410742 :     const float m_p = 3.0f; //pendulum mass(kg)
      58             :     // const float width = 0.0650f; //width(m)
      59             :     // const float height = 0.240f; //height(m)
      60     1410742 :     const float l = 0.10f; //height of center of mass from base(m)
      61     1410742 :     const float i_p = 0.01250f; //Moment of inertia about pitch axis(SI units)
      62             : 
      63             :     // wheel constants
      64     1410742 :     const float r_w = 0.05f; //wheel radius(m)
      65     1410742 :     const float m_w = 0.1130f; //wheel mass(kg)
      66     1410742 :     const float i_w = 0.00015480f; // moment of inertia of wheel(SI units)
      67             : 
      68             :     // motor constants
      69     1410742 :     const float R = 3.0f; //Winding resistance(ohm)
      70     1410742 :     const float k_e = 0.240f; //back-emf constant(SI units)
      71     1410742 :     const float k_t = 0.240f; //torque constant(SI units)
      72     1410742 :     const float v_max = 12.0f; //max input voltage(V)
      73     1410742 :     const float gear_ratio = 50.0f;
      74             : 
      75             :     // balance bot uses skid steering
      76     1410742 :     float motor1 = input.servos[0] ? normalise_servo_input(input.servos[0]) : 0;
      77     1410742 :     float motor2 = input.servos[2] ? normalise_servo_input(input.servos[2]) : 0;
      78     1410742 :     const float steering = motor1 - motor2;
      79     1410742 :     const float throttle = 0.5 * (motor1 + motor2);
      80             : 
      81             :     // motor input voltage: (throttle/max_throttle)*v_max
      82     1410742 :     const float v = throttle*v_max;
      83             : 
      84             :     // how much time has passed?
      85     1410742 :     const float delta_time = frame_time_us * 1.0e-6f;
      86             : 
      87             :     // yaw rate in degrees/s
      88     1410742 :     float yaw_rate = calc_yaw_rate(steering);
      89             : 
      90             :     // obtain roll, pitch, yaw from dcm
      91             :     float r, p, y;
      92     1410742 :     dcm.to_euler(&r, &p, &y);
      93     1410742 :     float theta = p; //radians
      94             : 
      95     1410742 :     float ang_vel = gyro.y; //radians/s
      96     1410742 :     if (!hal.util->get_soft_armed()) {
      97             :         // simulated fingers uprighting the vehicle
      98      826949 :         const float p_gain = 200;
      99      826949 :         const float pitch_response = -sin(p) * p_gain * delta_time;
     100      826949 :         ang_vel += pitch_response;
     101             : 
     102             :         // simulated fingers rotating the vehicle
     103      826949 :         const float y_gain = 100000;
     104      826949 :         const float yaw_response = -sin(wrap_180(y)) * y_gain * delta_time;
     105      826949 :         yaw_rate += yaw_response;
     106             :     }
     107             : 
     108             :     // t1,t2,t3 are terms in the equation to find vehicle frame x acceleration
     109     1410742 :     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);
     110     1410742 :     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)));
     111     1410742 :     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)) );
     112             : 
     113             :     //vehicle frame x acceleration
     114     1410742 :     const float accel_vf_x = (t1-t2)/t3;
     115             : 
     116     1410742 :     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))
     117     1410742 :                                      / (i_p + m_p*l*l);
     118             : 
     119             :     // accel in body frame due to motor
     120     1410742 :     accel_body = Vector3f(accel_vf_x*cos(theta), 0, -accel_vf_x*sin(theta));
     121             : 
     122             :     // update angular velocity (theta is re-derived from the attitude each
     123             :     // step via the DCM below, so it is not integrated here)
     124     1410742 :     ang_vel += angular_accel_bf_y * delta_time;
     125             : 
     126     1410742 :     gyro = Vector3f(0, ang_vel, radians(yaw_rate));
     127             : 
     128             :     // update attitude
     129     1410742 :     dcm.rotate(gyro * delta_time);
     130     1410742 :     dcm.normalize();
     131             : 
     132             :     // add in accel due to direction change
     133     1410742 :     accel_body.y += radians(yaw_rate) * velocity_vf_x;
     134             : 
     135             :     // update x velocity in vehicle frame
     136     1410742 :     velocity_vf_x += accel_vf_x * delta_time;
     137             : 
     138             :     // now in earth frame
     139     1410742 :     Vector3f accel_earth = dcm * accel_body;
     140     1410742 :     accel_earth += Vector3f(0, 0, GRAVITY_MSS);
     141             : 
     142             :     // we are on the ground, so our vertical accel is zero
     143     1410742 :     accel_earth.z = 0;
     144             : 
     145     2237691 :     if (!hal.util->get_soft_armed() &&
     146      826949 :         p < radians(2)) {
     147             :         // reset to vertical when not armed for faster testing
     148      826949 :         accel_earth.zero();
     149      826949 :         velocity_ef.zero();
     150      826949 :         velocity_vf_x =0;
     151      826949 :         gyro[1] = 0; // no pitch rate
     152      826949 :         if (y < radians(2)) {
     153             :             // no rates at all:
     154      820618 :             dcm.identity();
     155      820618 :             gyro.zero();
     156             :         }
     157             :     }
     158             : 
     159             :     // work out acceleration as seen by the accelerometers. It sees the kinematic
     160             :     // acceleration (ie. real movement), plus gravity
     161     1410742 :     accel_body += dcm.transposed() * (Vector3f(0, 0, -GRAVITY_MSS));
     162             : 
     163             :     // new velocity vector
     164     1410742 :     velocity_ef += accel_earth * delta_time;
     165             : 
     166             :     // new position vector
     167     1410742 :     position += (velocity_ef * delta_time).todouble();
     168             : 
     169             :     // neglect roll
     170     1410742 :     dcm.to_euler(&r, &p, &y);
     171     1410742 :     dcm.from_euler(0.0f, p, y);
     172     1410742 :     use_smoothing = true;
     173             : 
     174             :     // update lat/lon/altitude
     175     1410742 :     update_position();
     176     1410742 :     time_advance();
     177             : 
     178             :     // update magnetic field
     179     1410742 :     update_mag_field_bf();
     180             : 
     181     1410742 :     update_battery();
     182     1410742 : }
     183             : 
     184             : }// namespace SITL