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 {
// 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 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 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
 
// Video encoding
namespace video {
extern const int H264_RF_CONSTANT;

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

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}});
 
// 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<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 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