sensor_conversions.h

Go to the documentation of this file.

 
 
#pragma once
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
 
#include <numbers>
 
#include "custom_units.h"
 
namespace sensor_conversions {
  namespace swerve_drive {
    namespace turn {
      constexpr double sensorConversionFactor = 1;  
      constexpr units::angle::turn_t ToSensorUnit(const units::degree_t degrees) {
        return degrees / sensorConversionFactor;
      }
      constexpr units::degree_t ToAngle(const units::angle::turn_t sensorunit) {
        return sensorunit * sensorConversionFactor;
      }
      constexpr double turnGearRatio = 150.0 / 7.1;
      constexpr units::turn_t MotorToWheelAngle(const units::angle::turn_t motorPosition) {
        return motorPosition / turnGearRatio;
      }
      constexpr units::turn_t WheelToMotorAngle(const units::angle::turn_t wheelPosition) {
        return wheelPosition * turnGearRatio;
      }
    }  // namespace turn
    namespace drive {
      constexpr auto wheelDiameter = 4.0_in / units::angle::turn_t{1};
      constexpr auto wheelCircumference = wheelDiameter * std::numbers::pi;
      constexpr auto driveGearRatio = 6.12;
 
      constexpr units::inch_t ToDistance(const units::angle::turn_t sensorunit) {
        return wheelCircumference * (sensorunit / driveGearRatio);
      }
      constexpr units::angle::turn_t ToSensorPosition(const units::inch_t distance) {
        return (distance / wheelCircumference) * driveGearRatio;
      }
 
      constexpr units::inches_per_second_t ToVelocity(
          const units::angular_velocity::revolutions_per_minute_t sensorVelocity) {
        return ToDistance(sensorVelocity * units::second_t{1}) / units::second_t{1};
      }
      constexpr units::angular_velocity::revolutions_per_minute_t ToSensorVelocity(
          const units::inches_per_second_t velocity) {
        return ToSensorPosition(velocity * units::second_t{1}) / units::second_t{1};
      }
    }  // namespace drive
  }  // namespace swerve_drive
  namespace elevator {
    namespace elevator {
      constexpr auto sensorConversionFactor = 1_in / 0.625_tr;
      constexpr units::inch_t ToHeight(const units::angle::turn_t sensorUnit) {
        return sensorConversionFactor * sensorUnit;
      }
 
      constexpr units::angle::turn_t ToSensorUnit(const units::inch_t height) {
        return height / sensorConversionFactor;
      }
    }  // namespace elevator
    namespace arm {
      constexpr auto sensorConversionFactor = 1.0;  // Handled by motor config ratios
      constexpr units::angle::turn_t ToSensorUnit(const units::degree_t degrees) {
        return degrees / sensorConversionFactor;
      }
      constexpr units::degree_t ToAngle(const units::angle::turn_t sensorUnit) {
        return sensorUnit * sensorConversionFactor;
      }
    }  // namespace arm
    namespace wrist {
      constexpr double sensorConversionFactor = 1;  
 
      constexpr units::degree_t ToAngle(const units::angle::turn_t sensorUnit) {
        return sensorUnit * sensorConversionFactor;
      }
 
      constexpr units::angle::turn_t ToSensorUnit(const units::degree_t angle) {
        return angle / sensorConversionFactor;
      }
    }  // namespace wrist
  }  // namespace elevator
  namespace climber {
    constexpr auto sensorConversionFactor = 1.0;  // Handled by motor config ratios
    constexpr units::angle::turn_t ToSensorUnit(const units::degree_t degrees) {
      return degrees / sensorConversionFactor;
    }
    constexpr units::degree_t ToAngle(const units::angle::turn_t sensorUnit) {
      return sensorUnit * sensorConversionFactor;
    }
 
  }  // namespace climber
}  // namespace sensor_conversions