Constants.h

#pragma once
 
#include "utils/time.h"
#include "world_interface/data.h"
 
#include <array>
#include <chrono>
#include <cmath>
#include <string>
 
#include <frozen/unordered_map.h>
 
namespace Constants {
// TODO: make sure these are still accurate with the new arm.
extern const double SHOULDER_LENGTH;
extern const double ELBOW_LENGTH;

extern const float MDEG_PER_DEG;
 
// TODO: tune these drive extern constants
extern const double ROBOT_LENGTH;
extern const double ROBOT_WIDTH;
extern const double WHEEL_BASE;
extern const double EFF_WHEEL_BASE;
extern const double WHEEL_RADIUS;
extern const double PWM_PER_RAD_PER_SEC;
extern const double MAX_DRIVE_PWM;

extern const double MAX_WHEEL_VEL;
extern const double MAX_DTHETA;
 
// TODO: We need to recalibrate the camera, since we replaced it with a different one.
// TODO: rename cameras (in MC as well) as appropriate
extern const char* MAST_CAMERA_CONFIG_PATH;
extern const robot::types::CameraID MAST_CAMERA_ID;
 
extern const char* FOREARM_CAMERA_CONFIG_PATH;
extern const robot::types::CameraID FOREARM_CAMERA_ID;
 
extern const char* HAND_CAMERA_CONFIG_PATH;
extern const robot::types::CameraID HAND_CAMERA_ID;
 
extern const uint16_t WS_SERVER_PORT;

extern const char* MC_PROTOCOL_NAME;
extern const char* SIM_PROTOCOL_NAME;
extern const char* DGPS_PROTOCOL_NAME;
extern const char* ARDUPILOT_PROTOCOL_NAME;
 
extern const std::chrono::milliseconds JOINT_POWER_REPEAT_PERIOD;
extern const std::chrono::milliseconds ARM_IK_UPDATE_PERIOD;
 
namespace Drive {
// Represents the allowable error in millidegrees for steer motors to still process a drive
// request. That is, we make sure all the wheels are close enough to their target rotation
// before actually driving.
extern const double STEER_EPSILON;
} // namespace Drive
 
namespace Nav {
// Distance (m) we could have traveled forward in the time it takes to turn 1 radian
extern const double RADIAN_COST;
// Planner stays this far away from obstacles (m)
extern const double SAFE_RADIUS;
extern const int MAX_ITERS;
extern const double PLAN_RESOLUTION;
extern const double SEARCH_RADIUS_INCREMENT;
extern const double GPS_WAYPOINT_RADIUS;
extern const double LANDMARK_WAYPOINT_RADIUS;
extern const double EPS;
} // namespace Nav
 
// Lidar
namespace Lidar {
extern const std::string RP_PATH;
extern const double MM_PER_M;
extern const uint32_t RPLIDAR_A1_BAUDRATE;
extern const uint32_t RPLIDAR_S1_BAUDRATE;
} // namespace Lidar
 
// Video encoding
namespace video {
extern const int H264_RF_CONSTANT;

extern const std::unordered_map<robot::types::CameraID, int> STREAM_RFS;
} // namespace video

constexpr auto JOINT_MOTOR_MAP =
    frozen::make_unordered_map<robot::types::jointid_t, robot::types::motorid_t>(
        {{robot::types::jointid_t::frontLeftSwerve, robot::types::motorid_t::frontLeftSwerve},
         {robot::types::jointid_t::frontRightSwerve,
          robot::types::motorid_t::frontRightSwerve},
         {robot::types::jointid_t::rearLeftSwerve, robot::types::motorid_t::rearLeftSwerve},
         {robot::types::jointid_t::rearRightSwerve, robot::types::motorid_t::rearRightSwerve},
         {robot::types::jointid_t::armBase, robot::types::motorid_t::armBase},
         {robot::types::jointid_t::shoulder, robot::types::motorid_t::shoulder},
         {robot::types::jointid_t::elbow, robot::types::motorid_t::elbow},
         {robot::types::jointid_t::forearm, robot::types::motorid_t::forearm},
         {robot::types::jointid_t::hand, robot::types::motorid_t::hand},
         {robot::types::jointid_t::activeSuspension,
          robot::types::motorid_t::activeSuspension}});
 
// Arm inverse kinematics
namespace arm {
extern const double SAFETY_FACTOR;

extern const double MAX_EE_VEL;
extern const double IK_SOLVER_THRESH;
 
extern const int IK_SOLVER_MAX_ITER;

extern const std::array<robot::types::jointid_t, 2> IK_MOTOR_JOINTS;

extern const std::array<robot::types::motorid_t, 2> IK_MOTORS;

constexpr frozen::unordered_map<robot::types::motorid_t, std::pair<int, int>, IK_MOTORS.size()>
    JOINT_LIMITS{{robot::types::motorid_t::shoulder, {18200, 152500}},
                 {robot::types::motorid_t::elbow, {-169100, 0}}};

constexpr frozen::unordered_map<robot::types::motorid_t, double, IK_MOTORS.size()>
    SEGMENT_LENGTHS{{robot::types::motorid_t::shoulder, 0.3848608},
                    {robot::types::motorid_t::elbow, 0.461264}};
} // namespace arm
 
namespace autonomous {
extern const double THETA_KP;
extern const double DRIVE_VEL;
extern const double SLOW_DRIVE_THRESHOLD;
extern const double DONE_THRESHOLD;
// Duration long enough to confirm we are there, not so long that time is wasted
extern const util::dseconds CLOSE_TO_TARGET_DUR_VAL;
} // namespace autonomous
 
extern const double CONTROL_HZ;
 
} // namespace Constants