Skip to content

Commit 02cf569

Browse files
StarrryNightStarrryNight
committed
add more constants
1 parent c3087d7 commit 02cf569

6 files changed

Lines changed: 187 additions & 7 deletions

File tree

‎src/shared/robot_constants.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ RobotConstants createRobotConstants()
4646
.robot_trajectory_max_acceleration_m_per_s_2 = 3.5f,
4747
.robot_trajectory_max_deceleration_m_per_s_2 = 3.5f,
4848

49-
.robot_max_jerk_m_per_s_3 = 0.5f,
49+
.robot_max_jerk_m_per_s_3 = 40.0f,
5050
.robot_min_jerk_m_per_s_3 = -5.0f,
5151

5252
// Robot's angular movement constants

‎src/software/constants.h‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,33 @@ const std::string ROBOT_ORIENTATION_CONTROLLER_KD_CONFIG_KEY =
6969
const std::string ROBOT_ORIENTATION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY =
7070
"orientation_controller_max_integral";
7171

72+
// Per-robot overrides for RobotConstants movement limits. If a key is absent from
73+
// robot_config.toml, the compiled-in value from createRobotConstants() is used instead.
74+
const std::string ROBOT_MAX_SPEED_M_PER_S_CONFIG_KEY = "robot_max_speed_m_per_s";
75+
const std::string BALL_PLACEMENT_WALL_MAX_SPEED_M_PER_S_CONFIG_KEY =
76+
"ball_placement_wall_max_speed_m_per_s";
77+
const std::string BALL_PLACEMENT_RETREAT_MAX_SPEED_M_PER_S_CONFIG_KEY =
78+
"ball_placement_retreat_max_speed_m_per_s";
79+
const std::string DRIBBLE_SPEED_M_PER_S_CONFIG_KEY = "dribble_speed_m_per_s";
80+
const std::string ROBOT_TRAJECTORY_MAX_SPEED_M_PER_S_CONFIG_KEY =
81+
"robot_trajectory_max_speed_m_per_s";
82+
const std::string ROBOT_MAX_ACCELERATION_M_PER_S_2_CONFIG_KEY =
83+
"robot_max_acceleration_m_per_s_2";
84+
const std::string ROBOT_MAX_DECELERATION_M_PER_S_2_CONFIG_KEY =
85+
"robot_max_deceleration_m_per_s_2";
86+
const std::string ROBOT_TRAJECTORY_MAX_ACCELERATION_M_PER_S_2_CONFIG_KEY =
87+
"robot_trajectory_max_acceleration_m_per_s_2";
88+
const std::string ROBOT_TRAJECTORY_MAX_DECELERATION_M_PER_S_2_CONFIG_KEY =
89+
"robot_trajectory_max_deceleration_m_per_s_2";
90+
const std::string ROBOT_MAX_JERK_M_PER_S_3_CONFIG_KEY = "robot_max_jerk_m_per_s_3";
91+
const std::string ROBOT_MIN_JERK_M_PER_S_3_CONFIG_KEY = "robot_min_jerk_m_per_s_3";
92+
const std::string ROBOT_MAX_ANG_SPEED_RAD_PER_S_CONFIG_KEY =
93+
"robot_max_ang_speed_rad_per_s";
94+
const std::string ROBOT_TRAJECTORY_MAX_ANG_SPEED_RAD_PER_S_CONFIG_KEY =
95+
"robot_trajectory_max_ang_speed_rad_per_s";
96+
const std::string ROBOT_MAX_ANG_ACCELERATION_RAD_PER_S_2_CONFIG_KEY =
97+
"robot_max_ang_acceleration_rad_per_s_2";
98+
7299
const std::string SSL_VISION_ADDRESS = "224.5.23.2";
73100
static constexpr unsigned int SSL_VISION_PORT = 10006;
74101

‎src/software/embedded/robot_diagnostics_cli/embedded_data.py‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,30 @@ def get_kick_coeff(self) -> str:
6464
def get_chip_pulse_width(self) -> str:
6565
return self._get_value(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY)
6666

67+
def get_position_controller_kp(self) -> str:
68+
return self._get_value(ROBOT_POSITION_CONTROLLER_KP_CONFIG_KEY)
69+
70+
def get_position_controller_ki(self) -> str:
71+
return self._get_value(ROBOT_POSITION_CONTROLLER_KI_CONFIG_KEY)
72+
73+
def get_position_controller_kd(self) -> str:
74+
return self._get_value(ROBOT_POSITION_CONTROLLER_KD_CONFIG_KEY)
75+
76+
def get_position_controller_max_integral(self) -> str:
77+
return self._get_value(ROBOT_POSITION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY)
78+
79+
def get_orientation_controller_kp(self) -> str:
80+
return self._get_value(ROBOT_ORIENTATION_CONTROLLER_KP_CONFIG_KEY)
81+
82+
def get_orientation_controller_ki(self) -> str:
83+
return self._get_value(ROBOT_ORIENTATION_CONTROLLER_KI_CONFIG_KEY)
84+
85+
def get_orientation_controller_kd(self) -> str:
86+
return self._get_value(ROBOT_ORIENTATION_CONTROLLER_KD_CONFIG_KEY)
87+
88+
def get_orientation_controller_max_integral(self) -> str:
89+
return self._get_value(ROBOT_ORIENTATION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY)
90+
6791
# TODO: #3809
6892
# def get_cap_volt(self) -> str:
6993
# return self._get_value(ROBOT_CAPACITOR_VOLTAGE_CONFIG_KEY)

‎src/software/embedded/robot_diagnostics_cli/robot_diagnostics_cli.py‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -182,6 +182,46 @@ def __generate_config_table(self) -> Table:
182182
f"{ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY}",
183183
self.embedded_data.get_chip_pulse_width(),
184184
)
185+
table.add_row(
186+
"Position Controller Kp",
187+
f"{ROBOT_POSITION_CONTROLLER_KP_CONFIG_KEY}",
188+
self.embedded_data.get_position_controller_kp(),
189+
)
190+
table.add_row(
191+
"Position Controller Ki",
192+
f"{ROBOT_POSITION_CONTROLLER_KI_CONFIG_KEY}",
193+
self.embedded_data.get_position_controller_ki(),
194+
)
195+
table.add_row(
196+
"Position Controller Kd",
197+
f"{ROBOT_POSITION_CONTROLLER_KD_CONFIG_KEY}",
198+
self.embedded_data.get_position_controller_kd(),
199+
)
200+
table.add_row(
201+
"Position Controller Max Integral",
202+
f"{ROBOT_POSITION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY}",
203+
self.embedded_data.get_position_controller_max_integral(),
204+
)
205+
table.add_row(
206+
"Orientation Controller Kp",
207+
f"{ROBOT_ORIENTATION_CONTROLLER_KP_CONFIG_KEY}",
208+
self.embedded_data.get_orientation_controller_kp(),
209+
)
210+
table.add_row(
211+
"Orientation Controller Ki",
212+
f"{ROBOT_ORIENTATION_CONTROLLER_KI_CONFIG_KEY}",
213+
self.embedded_data.get_orientation_controller_ki(),
214+
)
215+
table.add_row(
216+
"Orientation Controller Kd",
217+
f"{ROBOT_ORIENTATION_CONTROLLER_KD_CONFIG_KEY}",
218+
self.embedded_data.get_orientation_controller_kd(),
219+
)
220+
table.add_row(
221+
"Orientation Controller Max Integral",
222+
f"{ROBOT_ORIENTATION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY}",
223+
self.embedded_data.get_orientation_controller_max_integral(),
224+
)
185225
# TODO: #3809
186226
# table.add_row(
187227
# "Capacitor Voltage",

‎src/software/embedded/thunderloop.cpp‎

Lines changed: 79 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,78 @@
2828
extern int clock_nanosleep(clockid_t __clock_id, int __flags,
2929
__const struct timespec* __req, struct timespec* __rem);
3030

31+
namespace
32+
{
33+
/**
34+
* Returns a copy of robot_constants with its movement limits overridden by any
35+
* matching keys present in robot_config.toml. Fields whose key is absent from the
36+
* TOML file keep their compiled-in value from createRobotConstants().
37+
*
38+
* @param robot_constants The compiled-in robot constants to override
39+
* @param toml_config_client The TOML config client to read overrides from
40+
*
41+
* @return robot_constants with movement limits overridden by robot_config.toml
42+
*/
43+
robot_constants::RobotConstants applyTomlRobotConstantsOverrides(
44+
robot_constants::RobotConstants robot_constants, TomlConfigClient& toml_config_client)
45+
{
46+
robot_constants.robot_max_speed_m_per_s =
47+
std::stof(toml_config_client.get(ROBOT_MAX_SPEED_M_PER_S_CONFIG_KEY,
48+
std::to_string(robot_constants.robot_max_speed_m_per_s)));
49+
robot_constants.ball_placement_wall_max_speed_m_per_s = std::stof(
50+
toml_config_client.get(BALL_PLACEMENT_WALL_MAX_SPEED_M_PER_S_CONFIG_KEY,
51+
std::to_string(
52+
robot_constants.ball_placement_wall_max_speed_m_per_s)));
53+
robot_constants.ball_placement_retreat_max_speed_m_per_s = std::stof(
54+
toml_config_client.get(
55+
BALL_PLACEMENT_RETREAT_MAX_SPEED_M_PER_S_CONFIG_KEY,
56+
std::to_string(robot_constants.ball_placement_retreat_max_speed_m_per_s)));
57+
robot_constants.dribble_speed_m_per_s =
58+
std::stof(toml_config_client.get(DRIBBLE_SPEED_M_PER_S_CONFIG_KEY,
59+
std::to_string(robot_constants.dribble_speed_m_per_s)));
60+
robot_constants.robot_trajectory_max_speed_m_per_s = std::stof(
61+
toml_config_client.get(
62+
ROBOT_TRAJECTORY_MAX_SPEED_M_PER_S_CONFIG_KEY,
63+
std::to_string(robot_constants.robot_trajectory_max_speed_m_per_s)));
64+
robot_constants.robot_max_acceleration_m_per_s_2 = std::stof(
65+
toml_config_client.get(
66+
ROBOT_MAX_ACCELERATION_M_PER_S_2_CONFIG_KEY,
67+
std::to_string(robot_constants.robot_max_acceleration_m_per_s_2)));
68+
robot_constants.robot_max_deceleration_m_per_s_2 = std::stof(
69+
toml_config_client.get(
70+
ROBOT_MAX_DECELERATION_M_PER_S_2_CONFIG_KEY,
71+
std::to_string(robot_constants.robot_max_deceleration_m_per_s_2)));
72+
robot_constants.robot_trajectory_max_acceleration_m_per_s_2 = std::stof(
73+
toml_config_client.get(
74+
ROBOT_TRAJECTORY_MAX_ACCELERATION_M_PER_S_2_CONFIG_KEY,
75+
std::to_string(robot_constants.robot_trajectory_max_acceleration_m_per_s_2)));
76+
robot_constants.robot_trajectory_max_deceleration_m_per_s_2 = std::stof(
77+
toml_config_client.get(
78+
ROBOT_TRAJECTORY_MAX_DECELERATION_M_PER_S_2_CONFIG_KEY,
79+
std::to_string(robot_constants.robot_trajectory_max_deceleration_m_per_s_2)));
80+
robot_constants.robot_max_jerk_m_per_s_3 =
81+
std::stof(toml_config_client.get(ROBOT_MAX_JERK_M_PER_S_3_CONFIG_KEY,
82+
std::to_string(robot_constants.robot_max_jerk_m_per_s_3)));
83+
robot_constants.robot_min_jerk_m_per_s_3 =
84+
std::stof(toml_config_client.get(ROBOT_MIN_JERK_M_PER_S_3_CONFIG_KEY,
85+
std::to_string(robot_constants.robot_min_jerk_m_per_s_3)));
86+
robot_constants.robot_max_ang_speed_rad_per_s = std::stof(
87+
toml_config_client.get(
88+
ROBOT_MAX_ANG_SPEED_RAD_PER_S_CONFIG_KEY,
89+
std::to_string(robot_constants.robot_max_ang_speed_rad_per_s)));
90+
robot_constants.robot_trajectory_max_ang_speed_rad_per_s = std::stof(
91+
toml_config_client.get(
92+
ROBOT_TRAJECTORY_MAX_ANG_SPEED_RAD_PER_S_CONFIG_KEY,
93+
std::to_string(robot_constants.robot_trajectory_max_ang_speed_rad_per_s)));
94+
robot_constants.robot_max_ang_acceleration_rad_per_s_2 = std::stof(
95+
toml_config_client.get(
96+
ROBOT_MAX_ANG_ACCELERATION_RAD_PER_S_2_CONFIG_KEY,
97+
std::to_string(robot_constants.robot_max_ang_acceleration_rad_per_s_2)));
98+
99+
return robot_constants;
100+
}
101+
} // namespace
102+
31103
// signal handling is done by csignal which requires a function pointer with C linkage
32104
extern "C"
33105
{
@@ -75,14 +147,15 @@ extern "C"
75147
Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,
76148
bool enable_log_merging, const int loop_hz)
77149
: toml_config_client_(std::make_unique<TomlConfigClient>(TOML_CONFIG_FILE_PATH)),
78-
robot_constants_(robot_constants),
150+
robot_constants_(
151+
applyTomlRobotConstantsOverrides(robot_constants, *toml_config_client_)),
79152
robot_id_(std::stoi(toml_config_client_->get(ROBOT_ID_CONFIG_KEY))),
80153
channel_id_(
81154
std::stoi(toml_config_client_->get(ROBOT_MULTICAST_CHANNEL_CONFIG_KEY))),
82155
network_interface_(toml_config_client_->get(ROBOT_NETWORK_INTERFACE_CONFIG_KEY)),
83156
loop_hz_(loop_hz),
84157
primitive_executor_(
85-
robot_constants,
158+
robot_constants_,
86159
PositionControllerConfig{
87160
std::stod(toml_config_client_->get(
88161
ROBOT_POSITION_CONTROLLER_KP_CONFIG_KEY)),
@@ -102,9 +175,9 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,
102175
std::stod(toml_config_client_->get(
103176
ROBOT_ORIENTATION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY))}),
104177
robot_localizer_(RobotLocalizer::RobotLocalizerConfig{
105-
robot_constants.kalman_process_noise_variance_rad_per_s_4,
106-
robot_constants.kalman_vision_noise_variance_rad_2,
107-
robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2})
178+
robot_constants_.kalman_process_noise_variance_rad_per_s_4,
179+
robot_constants_.kalman_vision_noise_variance_rad_2,
180+
robot_constants_.kalman_motor_sensor_noise_variance_rad_per_s_2})
108181
{
109182
waitForNetworkUp();
110183

@@ -148,7 +221,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,
148221
#endif
149222

150223
#ifndef DISABLE_MOTOR_SERVICE
151-
motor_service_ = std::make_unique<MotorService>(robot_constants);
224+
motor_service_ = std::make_unique<MotorService>(robot_constants_);
152225
g_motor_service = motor_service_.get();
153226
motor_service_->setup();
154227

‎src/software/py_constants.cpp‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -157,6 +157,22 @@ PYBIND11_MODULE(py_constants, m)
157157
m.attr("ROBOT_KICK_EXP_COEFF_CONFIG_KEY") = ROBOT_KICK_EXP_COEFF_CONFIG_KEY;
158158
m.attr("ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY") = ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY;
159159
m.attr("TOML_CONFIG_FILE_PATH") = TOML_CONFIG_FILE_PATH;
160+
m.attr("ROBOT_POSITION_CONTROLLER_KP_CONFIG_KEY") =
161+
ROBOT_POSITION_CONTROLLER_KP_CONFIG_KEY;
162+
m.attr("ROBOT_POSITION_CONTROLLER_KI_CONFIG_KEY") =
163+
ROBOT_POSITION_CONTROLLER_KI_CONFIG_KEY;
164+
m.attr("ROBOT_POSITION_CONTROLLER_KD_CONFIG_KEY") =
165+
ROBOT_POSITION_CONTROLLER_KD_CONFIG_KEY;
166+
m.attr("ROBOT_POSITION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY") =
167+
ROBOT_POSITION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY;
168+
m.attr("ROBOT_ORIENTATION_CONTROLLER_KP_CONFIG_KEY") =
169+
ROBOT_ORIENTATION_CONTROLLER_KP_CONFIG_KEY;
170+
m.attr("ROBOT_ORIENTATION_CONTROLLER_KI_CONFIG_KEY") =
171+
ROBOT_ORIENTATION_CONTROLLER_KI_CONFIG_KEY;
172+
m.attr("ROBOT_ORIENTATION_CONTROLLER_KD_CONFIG_KEY") =
173+
ROBOT_ORIENTATION_CONTROLLER_KD_CONFIG_KEY;
174+
m.attr("ROBOT_ORIENTATION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY") =
175+
ROBOT_ORIENTATION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY;
160176

161177
// Robot power constants
162178
m.attr("MIN_CAPACITOR_VOLTAGE") = MIN_CAPACITOR_VOLTAGE;

0 commit comments

Comments
 (0)