2018-06-05 05:56:46 -03:00
|
|
|
#include<stdio.h>
|
|
|
|
#include "Rover.h"
|
|
|
|
|
2018-06-20 08:11:58 -03:00
|
|
|
// Function to set a desired pitch angle according to throttle
|
2018-06-21 09:36:58 -03:00
|
|
|
void Rover::balancebot_pitch_control(float &throttle, bool armed)
|
2018-06-05 05:56:46 -03:00
|
|
|
{
|
2018-06-20 08:11:58 -03:00
|
|
|
// calculate desired pitch angle
|
2018-06-21 09:36:58 -03:00
|
|
|
const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max);
|
2018-06-20 08:11:58 -03:00
|
|
|
|
|
|
|
// calculate required throttle using PID controller
|
2018-06-21 09:36:58 -03:00
|
|
|
const float balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed) * 100.0f;
|
2018-06-20 08:11:58 -03:00
|
|
|
|
2018-06-21 09:36:58 -03:00
|
|
|
// constrain throttle between -100 and 100
|
|
|
|
throttle = constrain_float(balance_throttle, -100.0f, 100.0f);
|
2018-06-05 05:56:46 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// returns true if vehicle is a balance bot
|
|
|
|
// called in AP_MotorsUGV::output()
|
|
|
|
// this affects whether the vehicle tries to control its pitch with throttle output
|
2018-06-21 09:36:58 -03:00
|
|
|
bool Rover::is_balancebot() const
|
2018-06-05 05:56:46 -03:00
|
|
|
{
|
|
|
|
return ((enum frame_class)g2.frame_class.get() == FRAME_BALANCEBOT);
|
|
|
|
}
|