compAIR/compLIB
compAIR/compLIB
Archived
4
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Tune turning a bit

Browse source
This commit is contained in:
Konstantin Lampalzer 2022-05-29 15:20:11 +02:00
parent 9000316350
commit 4ee0b8045f
5 changed files with 42 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

10
server_v2/include/Robot.hpp
View file

@ -23,17 +23,21 @@
#define ROBOT_WHEEL_RADIUS_MM 35.5
#define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM / 1000.0)
#define ROBOT_WHEEL_CIRCUMFERENCE_MM (2 * ROBOT_WHEEL_RADIUS_MM * M_PI)
#define ROBOT_WHEEL_CIRCUMFERENCE_MM (2.0 * ROBOT_WHEEL_RADIUS_MM * M_PI)
#define ROBOT_WHEEL_CIRCUMFERENCE_M (2 * ROBOT_WHEEL_RADIUS_M * M_PI)
#define ROBOT_TICKS_PER_TURN (27.7 * 100.0)
#define ROBOT_ARBOR_LENGTH_MM 139.0
#define ROBOT_ARBOR_LENGTH_MM 140.0
#define ROBOT_ARBOR_LENGTH_M (ROBOT_ARBOR_LENGTH_MM / 1000.0)
#define ROBOT_TICKS_PER_METER (1000.0 / ROBOT_WHEEL_CIRCUMFERENCE_MM * ROBOT_TICKS_PER_TURN)
#define ROBOT_GO_TO_GOAL_K 0.99 // Between 0.5 and 1
#define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0
#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 5.0
#define ROBOT_GO_TO_DISTANCE_END_M 0.01 // stop approx 1 cm before target
#define ROBOT_GO_TO_ANGLE_END_RAD (0.5 * (M_PI / 180.0)) // stop 0.5 deg before target
#define ROBOT_GO_TO_ANGLE_END_RAD (0.1 * (M_PI / 180.0)) // stop 0.1 deg before target
#define ROBOT_GO_TO_ANGLE_ALPHA_P 0.5
#define ROBOT_GO_TO_ANGLE_MIN_VEL (M_PI / 3.0)
#endif //COMPLIB_SERVER_ROBOT_HPP

17
server_v2/include/mathUtils.hpp
View file

@ -40,6 +40,23 @@ namespace mathUtils {
inline T wrap_angle_to_pi(T angle) {
return atan2(sin(angle), cos(angle));
}
template<class T>
inline T limit_min(T val, T min) {
if (val < 0.0 && val > -min) {
return -min;
} else if (val > 0.0 && val < min) {
return min;
} else if (val == 0) {
return min;
}
return val;
}
template<class T>
inline T limit_max(T val, T min, T max) {
return fmax(min, fmin(max, val));
}
}
#endif // COMPLIB_SERVER_MATHUTILS_HPP

19
server_v2/src/GoToController.cpp
View file

@ -105,12 +105,12 @@ void GoToController::drive_distance(double distance_m, double v_m_s) {
}
void GoToController::turn_degrees(double angle_deg, double v_rad_s) {
if (angle_deg < 0 && v_rad_s > 0) {
auto angle_rad = mathUtils::wrap_angle_to_pi(angle_deg * (M_PI / 180.0));
if (angle_rad < 0 && v_rad_s > 0) {
v_rad_s *= -1;
}
auto angle_rad = mathUtils::wrap_angle_to_pi(angle_deg * (M_PI / 180.0));
auto was_enabled = OdometryController::getInstance().is_enabled();
OdometryController::getInstance().enable();
auto start_pos = OdometryController::getInstance().get();
@ -118,19 +118,22 @@ void GoToController::turn_degrees(double angle_deg, double v_rad_s) {
auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ);
auto angle_to_target = angle_rad;
while (fabs(angle_to_target) > ROBOT_GO_TO_ANGLE_END_RAD) {
auto angle_turned = 0.0;
while (fabs(angle_to_target) > ROBOT_GO_TO_ANGLE_END_RAD && angle_turned < angle_rad) {
std::this_thread::sleep_for(sleep_duration);
auto current_pos = OdometryController::getInstance().get();
auto angle_turned = current_pos.get_angular_orientation() - start_pos.get_angular_orientation();
angle_turned = current_pos.get_angular_orientation() - start_pos.get_angular_orientation();
angle_to_target = mathUtils::wrap_angle_to_pi(angle_rad - angle_turned);
diff_drive_inverse_kinematics(0.0, v_rad_s);
auto speed_mult = mathUtils::limit_max(fabs(angle_to_target) * ROBOT_GO_TO_ANGLE_ALPHA_P, 0.0, 1.0);
auto adjusted_speed = mathUtils::limit_min(v_rad_s * speed_mult, ROBOT_GO_TO_ANGLE_MIN_VEL);
diff_drive_inverse_kinematics(0.0, adjusted_speed);
spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f};",
spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f};",
current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(),
angle_turned, angle_to_target
angle_turned, angle_to_target, adjusted_speed
);
}

8
server_v2/src/OdometryController.cpp
View file

@ -80,10 +80,10 @@ OdometryController::OdometryController() {
}
current_odometry = Odometry(new_x_position, new_y_position, new_angular_orientation);
// spdlog::info("{} {} {} {} {} {}",
// current_position_left, current_position_right,
// distance_left, distance_right,
// distance, theta);
spdlog::info("{} {} {} {} {} {}",
current_position_left, current_position_right,
distance_left, distance_right,
distance, theta);
}
}
}

6
server_v2/src/main.cpp
View file

@ -38,7 +38,7 @@ int main(int argc, char *argv[]) {
// GoToGoalController::go_to_point(2, 0, 0.2);
// GoToController::drive_distance(-4, 0.2);
// GoToController::turn_degrees(-90, M_PI_2);
GoToController::turn_degrees(180, M_PI);
// GoToController::drive_distance(2, 0.2);
// GoToController::turn_degrees(-90, M_PI_2);
@ -52,10 +52,10 @@ int main(int argc, char *argv[]) {
// GoToController::drive_distance(2, 0.2);
// GoToController::turn_degrees(-90, M_PI_2);
ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(5, stoi(argv[1]));
// ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(5, stoi(argv[1]));
// ClosedLoopMotorController::getInstance().generate_step_response_data();
// ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();
// std::this_thread::sleep_for(std::chrono::hours(12));
std::this_thread::sleep_for(std::chrono::hours(12));
return 0;
}