Constants.h

#pragma once
 
#include "utils/time.h"
#include "world_interface/data.h"
 
#include <array>
#include <chrono>
#include <cmath>
#include <string>
#include <unordered_set>
 
#include <frozen/unordered_map.h>
 
using robot::types::CameraID;
using robot::types::jointid_t;
using robot::types::motorid_t;

namespace Constants {

namespace Arm {
extern const double SHOULDER_LENGTH;
extern const double ELBOW_LENGTH;

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<jointid_t, 2> IK_MOTOR_JOINTS;

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

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

constexpr frozen::unordered_map<motorid_t, double, IK_MOTORS.size()> SEGMENT_LENGTHS{
    {motorid_t::shoulder, 0.3848608}, {motorid_t::elbow, 0.461264}};
 
} // namespace Arm
 

namespace Drive {
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;

extern const double STEER_EPSILON;
 
} // namespace Drive
 

namespace Camera {
extern const CameraID MAST_CAMERA_ID;
extern const CameraID WRIST_CAMERA_ID;
extern const CameraID HAND_CAMERA_ID;
 
extern const std::unordered_set<robot::types::CameraID> CAMERA_SET;
extern const std::unordered_map<robot::types::CameraID, std::string> CAMERA_CONFIG_PATHS;

extern const int H264_RF_CONSTANT;

extern const std::unordered_map<CameraID, int> STREAM_RFS;
 
} // namespace Camera
 

namespace Network {
extern const uint16_t WS_SERVER_PORT;

extern const char* MC_PROTOCOL_NAME;
extern const char* SIM_PROTOCOL_NAME;
extern const char* ARDUPILOT_PROTOCOL_NAME;
 
} // namespace Network
 

namespace Nav {
extern const double RADIAN_COST;
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;
 
extern const double THETA_KP;
extern const double DRIVE_VEL;
extern const double SLOW_DRIVE_THRESHOLD;
extern const double DONE_THRESHOLD;
extern const util::dseconds CLOSE_TO_TARGET_DUR_VAL;
 
} // namespace Nav
 

constexpr auto JOINT_MOTOR_MAP = frozen::make_unordered_map<jointid_t, motorid_t>(
    {{jointid_t::armBase, motorid_t::armBase},
     {jointid_t::shoulder, motorid_t::shoulder},
     {jointid_t::elbow, motorid_t::elbow},
     {jointid_t::forearm, motorid_t::forearm},
     {jointid_t::hand, motorid_t::hand},
     {jointid_t::drillActuator, motorid_t::drillActuator},
     {jointid_t::drillMotor, motorid_t::drillMotor}});
 
extern const double CONTROL_HZ;
 
extern const std::chrono::milliseconds JOINT_POWER_REPEAT_PERIOD;
extern const std::chrono::milliseconds ARM_IK_UPDATE_PERIOD;
 
} // namespace Constants