lifter_kinematics.h

Go to the documentation of this file.

 
 
#pragma once
#include <frc/geometry/Transform2d.h>
#include <frc/geometry/Translation2d.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/length.h>
 
#include "utils/custom_units.h"
 
struct ArmState {
  units::meter_t armLen;
  units::radian_t shoulderAngle;
};
 
class LifterKinematics {
 public:
  LifterKinematics(const frc::Translation2d& fulcrumPosition,
                   const units::meter_t armRotationOffset,
                   const frc::Translation2d& effectorOffset,
                   const frc::Translation2d& fixedBoomActuatorPosition,
                   const frc::Translation2d& actuatedBoomActuatorPosition);
 
  LifterKinematics() = delete;
 
  ArmState GetJointsFromEffector(const frc::Translation2d& pose, bool effectorInverted) const;
 
  ArmState GetJoints(const frc::Translation2d& pose) const;
 
  frc::Translation2d GetEffectorPose(const ArmState& state, bool effectorInverted) const;
 
  frc::Translation2d GetPose(const ArmState& state) const;
 
  units::inch_t ShoulderAngleToBoomExtension(units::degree_t shoulderAngle) const;
  units::degree_t BoomExtensionToShoulderAngle(units::inch_t boomExtension) const;
  units::velocity::inches_per_second_t ShoulderVelocityToBoomVelocity(units::radians_per_second_t shoulderVelocity,
                                                                      units::degree_t shoulderAngle) const;
  units::radians_per_second_t BoomVelocityToShoulderVelocity(units::velocity::inches_per_second_t boomVelocity,
                                                             units::inch_t boomPosition) const;
 
 private:
  const frc::Translation2d m_fulcrumPosition;  
  const units::meter_t
      m_armRotationOffset;  
  const frc::Translation2d
      m_effectorOffset;  
 
  const units::inch_t m_fixedBoomAnchorToFulcrumDist;
  const units::degree_t m_fixedBoomAnchorToFulcrumAngle;
  const units::inch_t m_articulatedBoomAnchorToFulcrumDist;
  const units::degree_t m_articulatedBoomAnchorToFulcrumAngle;
 
  ArmState GetJoints(const frc::Translation2d& pose, const frc::Translation2d& effectorOffset) const;
  frc::Translation2d GetPose(const ArmState& state, const frc::Translation2d& effectorOffset) const;
};