mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
471f375a38
This removes the name and subsystem from individual objects, and instead puts this data into a new singleton class, SendableRegistry. Much of LiveWindow has been refactored into SendableRegistry. In C++, a new CRTP helper class, SendableHelper, has been added to provide move and destruction functionality. Shims for GetName, SetName, GetSubsystem, and SetSubsystem have been added to Command and Subsystem (both old and new), and also to SendableHelper to prevent code breakage. This deprecates SendableBase in preparation for future removal.
118 lines
4.5 KiB
C++
118 lines
4.5 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
|
/* the project. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#include "frc/drive/KilloughDrive.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
|
|
#include <hal/HAL.h>
|
|
#include <wpi/math>
|
|
|
|
#include "frc/SpeedController.h"
|
|
#include "frc/smartdashboard/SendableBuilder.h"
|
|
#include "frc/smartdashboard/SendableRegistry.h"
|
|
|
|
using namespace frc;
|
|
|
|
KilloughDrive::KilloughDrive(SpeedController& leftMotor,
|
|
SpeedController& rightMotor,
|
|
SpeedController& backMotor)
|
|
: KilloughDrive(leftMotor, rightMotor, backMotor, kDefaultLeftMotorAngle,
|
|
kDefaultRightMotorAngle, kDefaultBackMotorAngle) {}
|
|
|
|
KilloughDrive::KilloughDrive(SpeedController& leftMotor,
|
|
SpeedController& rightMotor,
|
|
SpeedController& backMotor, double leftMotorAngle,
|
|
double rightMotorAngle, double backMotorAngle)
|
|
: m_leftMotor(leftMotor), m_rightMotor(rightMotor), m_backMotor(backMotor) {
|
|
m_leftVec = {std::cos(leftMotorAngle * (wpi::math::pi / 180.0)),
|
|
std::sin(leftMotorAngle * (wpi::math::pi / 180.0))};
|
|
m_rightVec = {std::cos(rightMotorAngle * (wpi::math::pi / 180.0)),
|
|
std::sin(rightMotorAngle * (wpi::math::pi / 180.0))};
|
|
m_backVec = {std::cos(backMotorAngle * (wpi::math::pi / 180.0)),
|
|
std::sin(backMotorAngle * (wpi::math::pi / 180.0))};
|
|
auto& registry = SendableRegistry::GetInstance();
|
|
registry.AddChild(this, &m_leftMotor);
|
|
registry.AddChild(this, &m_rightMotor);
|
|
registry.AddChild(this, &m_backMotor);
|
|
static int instances = 0;
|
|
++instances;
|
|
registry.AddLW(this, "KilloughDrive", instances);
|
|
}
|
|
|
|
void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
|
|
double zRotation, double gyroAngle) {
|
|
if (!reported) {
|
|
HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
|
|
HALUsageReporting::kRobotDrive2_KilloughCartesian);
|
|
reported = true;
|
|
}
|
|
|
|
ySpeed = std::clamp(ySpeed, -1.0, 1.0);
|
|
ySpeed = ApplyDeadband(ySpeed, m_deadband);
|
|
|
|
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
|
|
xSpeed = ApplyDeadband(xSpeed, m_deadband);
|
|
|
|
// Compensate for gyro angle.
|
|
Vector2d input{ySpeed, xSpeed};
|
|
input.Rotate(-gyroAngle);
|
|
|
|
double wheelSpeeds[3];
|
|
wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + zRotation;
|
|
wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
|
|
wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
|
|
|
|
Normalize(wheelSpeeds);
|
|
|
|
m_leftMotor.Set(wheelSpeeds[kLeft] * m_maxOutput);
|
|
m_rightMotor.Set(wheelSpeeds[kRight] * m_maxOutput);
|
|
m_backMotor.Set(wheelSpeeds[kBack] * m_maxOutput);
|
|
|
|
Feed();
|
|
}
|
|
|
|
void KilloughDrive::DrivePolar(double magnitude, double angle,
|
|
double zRotation) {
|
|
if (!reported) {
|
|
HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
|
|
HALUsageReporting::kRobotDrive2_KilloughPolar);
|
|
reported = true;
|
|
}
|
|
|
|
DriveCartesian(magnitude * std::sin(angle * (wpi::math::pi / 180.0)),
|
|
magnitude * std::cos(angle * (wpi::math::pi / 180.0)),
|
|
zRotation, 0.0);
|
|
}
|
|
|
|
void KilloughDrive::StopMotor() {
|
|
m_leftMotor.StopMotor();
|
|
m_rightMotor.StopMotor();
|
|
m_backMotor.StopMotor();
|
|
Feed();
|
|
}
|
|
|
|
void KilloughDrive::GetDescription(wpi::raw_ostream& desc) const {
|
|
desc << "KilloughDrive";
|
|
}
|
|
|
|
void KilloughDrive::InitSendable(SendableBuilder& builder) {
|
|
builder.SetSmartDashboardType("KilloughDrive");
|
|
builder.SetActuator(true);
|
|
builder.SetSafeState([=] { StopMotor(); });
|
|
builder.AddDoubleProperty("Left Motor Speed",
|
|
[=]() { return m_leftMotor.Get(); },
|
|
[=](double value) { m_leftMotor.Set(value); });
|
|
builder.AddDoubleProperty("Right Motor Speed",
|
|
[=]() { return m_rightMotor.Get(); },
|
|
[=](double value) { m_rightMotor.Set(value); });
|
|
builder.AddDoubleProperty("Back Motor Speed",
|
|
[=]() { return m_backMotor.Get(); },
|
|
[=](double value) { m_backMotor.Set(value); });
|
|
}
|