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 "Rover.h"
// Function to set a desired pitch angle according to throttle
void Rover::balance_pitch(float &throttle, bool armed)
{
float balance_throttle = 0;
float throttle_new = 0;
float demanded_pitch = 0;
balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed)*100;
throttle_new = constrain_float(throttle + balance_throttle, -100, 100);
throttle = throttle_new;
// calculate desired pitch angle
float demanded_pitch = radians(-(throttle/100) * g2.bal_pitch_max);
// calculate required throttle using PID controller
float balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed)*100;
throttle = constrain_float(balance_throttle, -100, 100);
}
// 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
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
};

3
APMrover2/Parameters.h
View File

@ -341,6 +341,9 @@ public:
// 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[];