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: manual mode support for balancebot

Browse Source
This commit is contained in:
Ebin 2018-06-20 16:41:58 +05:30 committed by Randy Mackay
parent 657ff06380
commit 6c2f18cc2f
3 changed files with 20 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
APMrover2/BalanceBot.cpp
View File

@ -1,15 +1,16 @@
#include<stdio.h> #include<stdio.h>
#include "Rover.h" #include "Rover.h"
// Function to set a desired pitch angle according to throttle
void Rover::balance_pitch(float &throttle, bool armed) void Rover::balance_pitch(float &throttle, bool armed)
{ {
float balance_throttle = 0; // calculate desired pitch angle
float throttle_new = 0; float demanded_pitch = radians(-(throttle/100) * g2.bal_pitch_max);
float demanded_pitch = 0;
balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed)*100; // calculate required throttle using PID controller
throttle_new = constrain_float(throttle + balance_throttle, -100, 100); float balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed)*100;
throttle = throttle_new;
throttle = constrain_float(balance_throttle, -100, 100);
} }
// returns true if vehicle is a balance bot // returns true if vehicle is a balance bot

9
APMrover2/Parameters.cpp
View File

@ -547,6 +547,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("PIVOT_TURN_RATE", 20, ParametersG2, pivot_turn_rate, 90), AP_GROUPINFO("PIVOT_TURN_RATE", 20, ParametersG2, pivot_turn_rate, 90),
// @Param: BAL_PITCH_MAX
// @DisplayName: BalanceBot Maximum Pitch
// @Description: Pitch angle in degrees at 100% throttle
// @Units: deg
// @Range: 0 5
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 5),
AP_GROUPEND AP_GROUPEND
}; };

3
APMrover2/Parameters.h
View File

@ -341,6 +341,9 @@ public:
// pivot turn rate // pivot turn rate
AP_Int16 pivot_turn_rate; AP_Int16 pivot_turn_rate;
// pitch angle at 100% throttle
AP_Float bal_pitch_max;
}; };
extern const AP_Param::Info var_info[]; extern const AP_Param::Info var_info[];