Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add trajectory generation using hermite splines (#1843)

Browse Source
This commit is contained in:
Prateek Machiraju
2019-09-28 18:40:56 -04:00
committed by Peter Johnson
parent fd612052f3
commit 457f94ba26
56 changed files with 4185 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp Normal file
View File

@@ -0,0 +1,31 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 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/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
using namespace frc;
DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
: m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
const Pose2d& pose, curvature_t curvature,
units::meters_per_second_t velocity) {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
wheelSpeeds.Normalize(m_maxSpeed);
return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
}
TrajectoryConstraint::MinMax
DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
const Pose2d& pose, curvature_t curvature,
units::meters_per_second_t speed) {
return {};
}