From 0c420d8a6a34ced6e90976c330211ca1dd2ccdf7 Mon Sep 17 00:00:00 2001 From: Abby Pribisova Date: Mon, 12 Apr 2021 14:59:59 -0600 Subject: [PATCH 1/2] Added steering cap --- src/drive_controller/src/drive_controller.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/drive_controller/src/drive_controller.py b/src/drive_controller/src/drive_controller.py index c19800e..2fb31ed 100644 --- a/src/drive_controller/src/drive_controller.py +++ b/src/drive_controller/src/drive_controller.py @@ -10,6 +10,7 @@ from std_msgs.msg import Float64 M_PI_2 = 1.5708 +M_PI = M_PI_2 * 2 class DriveController: @@ -51,6 +52,11 @@ def _drive(self, msg): front_right_steering = math.atan2(y_comp_plus_ang, x_comp_plus_ang) back_left_steering = math.atan2(y_comp_minus_ang, x_comp_minus_ang) back_right_steering = math.atan2(y_comp_minus_ang, x_comp_plus_ang) + + front_left_steering, vel_left_front = clip_steering_angle(front_left_steering, vel_left_front) + front_right_steering, vel_right_front = clip_steering_angle(front_right_steering, vel_right_front) + back_left_steering, vel_left_back = clip_steering_angle(back_left_steering, vel_left_back) + back_right_steering, vel_right_back = clip_steering_angle(back_right_steering, vel_right_back) self.__drive_front_left_wheel(vel_left_front) self.__drive_front_right_wheel(vel_right_front) @@ -62,6 +68,15 @@ def _drive(self, msg): self.__steer_back_left_wheel(back_left_steering) self.__steer_back_right_wheel(back_right_steering) + def clip_steering_angle(steer, vel): + max_steer = M_PI_2 + min_steer = -1 * M_PI_2 + if (steer > max_steer) and (steer - M_PI > min_steer): + return (steer - M_PI, -1 * vel) + if (steer < min_steer) and (steer + M_PI < max_steer): + return (steer + M_PI, -1 * vel) + return steer, vel + def __drive_front_left_wheel(self, velocity): v = Float64() v.data = velocity From 08d4a7c2fb5408d128b87b2add9f37e9134e3aa5 Mon Sep 17 00:00:00 2001 From: Abby Pribisova Date: Mon, 12 Apr 2021 15:42:57 -0600 Subject: [PATCH 2/2] Fixed member function arguements --- src/drive_controller/src/drive_controller.py | 29 ++++++++++---------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/drive_controller/src/drive_controller.py b/src/drive_controller/src/drive_controller.py index 2fb31ed..d896a4e 100644 --- a/src/drive_controller/src/drive_controller.py +++ b/src/drive_controller/src/drive_controller.py @@ -33,6 +33,16 @@ def __init__(self): self.wheel_base = 1.0 self.steering_track = 1.0 + + def clip_steering_angle(self, steer, vel): + max_steer = M_PI_2 + min_steer = -1 * M_PI_2 + if (steer > max_steer) and (steer - M_PI > min_steer): + return steer - M_PI, -1 * vel + if (steer < min_steer) and (steer + M_PI < max_steer): + return steer + M_PI, -1 * vel + return steer, vel + def _drive(self, msg): lin_x = msg.linear.x lin_y = msg.linear.y @@ -53,10 +63,10 @@ def _drive(self, msg): back_left_steering = math.atan2(y_comp_minus_ang, x_comp_minus_ang) back_right_steering = math.atan2(y_comp_minus_ang, x_comp_plus_ang) - front_left_steering, vel_left_front = clip_steering_angle(front_left_steering, vel_left_front) - front_right_steering, vel_right_front = clip_steering_angle(front_right_steering, vel_right_front) - back_left_steering, vel_left_back = clip_steering_angle(back_left_steering, vel_left_back) - back_right_steering, vel_right_back = clip_steering_angle(back_right_steering, vel_right_back) + front_left_steering, vel_left_front = self.clip_steering_angle(front_left_steering, vel_left_front) + front_right_steering, vel_right_front = self.clip_steering_angle(front_right_steering, vel_right_front) + back_left_steering, vel_left_back = self.clip_steering_angle(back_left_steering, vel_left_back) + back_right_steering, vel_right_back = self.clip_steering_angle(back_right_steering, vel_right_back) self.__drive_front_left_wheel(vel_left_front) self.__drive_front_right_wheel(vel_right_front) @@ -67,16 +77,7 @@ def _drive(self, msg): self.__steer_front_right_wheel(front_right_steering) self.__steer_back_left_wheel(back_left_steering) self.__steer_back_right_wheel(back_right_steering) - - def clip_steering_angle(steer, vel): - max_steer = M_PI_2 - min_steer = -1 * M_PI_2 - if (steer > max_steer) and (steer - M_PI > min_steer): - return (steer - M_PI, -1 * vel) - if (steer < min_steer) and (steer + M_PI < max_steer): - return (steer + M_PI, -1 * vel) - return steer, vel - + def __drive_front_left_wheel(self, velocity): v = Float64() v.data = velocity