Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: balance bot pitch control added in all modes

Browse Source
This commit is contained in:
Ebin 2018-06-21 18:06:58 +05:30 committed by Randy Mackay
parent c65405541e
commit 2780d1715c
5 changed files with 23 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
APMrover2/BalanceBot.cpp
View File

@ -2,21 +2,22 @@
#include "Rover.h" #include "Rover.h"
// Function to set a desired pitch angle according to throttle // Function to set a desired pitch angle according to throttle
void Rover::balance_pitch(float &throttle, bool armed) void Rover::balancebot_pitch_control(float &throttle, bool armed)
{ {
// calculate desired pitch angle // calculate desired pitch angle
float demanded_pitch = radians(-(throttle/100) * g2.bal_pitch_max); const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max);
// calculate required throttle using PID controller // calculate required throttle using PID controller
float balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed)*100; const float balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed) * 100.0f;
throttle = constrain_float(balance_throttle, -100, 100); // constrain throttle between -100 and 100
throttle = constrain_float(balance_throttle, -100.0f, 100.0f);
} }
// returns true if vehicle is a balance bot // returns true if vehicle is a balance bot
// called in AP_MotorsUGV::output() // called in AP_MotorsUGV::output()
// this affects whether the vehicle tries to control its pitch with throttle output // this affects whether the vehicle tries to control its pitch with throttle output
bool Rover::is_BalanceBot() const bool Rover::is_balancebot() const
{ {
return ((enum frame_class)g2.frame_class.get() == FRAME_BALANCEBOT); return ((enum frame_class)g2.frame_class.get() == FRAME_BALANCEBOT);
} }

4
APMrover2/Rover.h
View File

@ -559,8 +559,8 @@ public:
void failsafe_check(); void failsafe_check();
// BalanceBot.cpp // BalanceBot.cpp
void balance_pitch(float &, bool); void balancebot_pitch_control(float &, bool);
bool is_BalanceBot() const; bool is_balancebot() const;
void update_soft_armed(); void update_soft_armed();
// Motor test // Motor test

5
APMrover2/mode.cpp
View File

@ -252,6 +252,11 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_
throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt); throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
} }
// if vehicle is balance bot, calculate actual throttle required for balancing
if (rover.is_balancebot()) {
rover.balancebot_pitch_control(throttle_out, rover.arming.is_armed());
}
// send to motor // send to motor
g2.motors.set_throttle(throttle_out); g2.motors.set_throttle(throttle_out);
} }

9
APMrover2/mode_hold.cpp
View File

@ -3,7 +3,14 @@
void ModeHold::update() void ModeHold::update()
{ {
float throttle = 0.0f;
// if vehicle is balance bot, calculate actual throttle required for balancing
if (rover.is_balancebot()) {
rover.balancebot_pitch_control(throttle, rover.arming.is_armed());
}
// hold position - stop motors and center steering // hold position - stop motors and center steering
g2.motors.set_throttle(0.0f); g2.motors.set_throttle(throttle);
g2.motors.set_steering(0.0f); g2.motors.set_steering(0.0f);
} }

4
APMrover2/mode_manual.cpp
View File

@ -14,8 +14,8 @@ void ModeManual::update()
get_pilot_desired_lateral(desired_lateral); get_pilot_desired_lateral(desired_lateral);
// if vehicle is balance bot, calculate actual throttle required for balancing // if vehicle is balance bot, calculate actual throttle required for balancing
if (rover.is_BalanceBot()) { if (rover.is_balancebot()) {
rover.balance_pitch(desired_throttle, rover.arming.is_armed()); rover.balancebot_pitch_control(desired_throttle, rover.arming.is_armed());
} }
// copy RC scaled inputs to outputs // copy RC scaled inputs to outputs