From 2d3d17b267df5afa72f772d7940bd340d1d19fa0 Mon Sep 17 00:00:00 2001 From: Federica Di Lauro Date: Mon, 2 Mar 2020 15:47:08 +0100 Subject: [PATCH] fix odometry --- otto_controller/Core/Src/main.cpp | 14 +++++--------- .../src/serial_bridge/scripts/serial_receiver.py | 7 ++++--- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/otto_controller/Core/Src/main.cpp b/otto_controller/Core/Src/main.cpp index 9037cd1..06216ed 100644 --- a/otto_controller/Core/Src/main.cpp +++ b/otto_controller/Core/Src/main.cpp @@ -58,7 +58,7 @@ /* USER CODE BEGIN PV */ //Parameters -float baseline = 0.3; +float baseline = 0.435; int ticks_per_revolution = 148000; //x4 resolution float right_wheel_circumference = 0.783; //in meters float left_wheel_circumference = 0.789; //in meters @@ -300,7 +300,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { float left_wheel = left_encoder.GetLinearVelocity(); float right_wheel = right_encoder.GetLinearVelocity(); - odom.FromWheelVelToOdom(1, -1); + odom.FromWheelVelToOdom(left_wheel, right_wheel); status_msg.linear_velocity = odom.GetLinearVelocity(); status_msg.angular_velocity = odom.GetAngularVelocity(); @@ -343,16 +343,12 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) { float left_setpoint = odom.GetLeftVelocity(); float right_setpoint = odom.GetRightVelocity(); -// left_pid.set(left_setpoint); -// right_pid.set(right_setpoint); - - left_pid.set(0); - right_pid.set(0); + left_pid.set(left_setpoint); + right_pid.set(right_setpoint); float cross_setpoint = left_setpoint - right_setpoint; -// cross_pid.set(cross_setpoint); + cross_pid.set(cross_setpoint); - cross_pid.set(0); } diff --git a/utils/catkin_ws/src/serial_bridge/scripts/serial_receiver.py b/utils/catkin_ws/src/serial_bridge/scripts/serial_receiver.py index 51cfb9e..0acc9f9 100755 --- a/utils/catkin_ws/src/serial_bridge/scripts/serial_receiver.py +++ b/utils/catkin_ws/src/serial_bridge/scripts/serial_receiver.py @@ -50,9 +50,10 @@ def serial_receiver(): status_length = len(encoded_buffer) odom_values = numpy.array([0,0,0]) #x, y, th - icc_x = 0;current_time = rospy.Time.now() - icc_y = 0; - radius = 0; + icc_x = 0 + icc_y = 0 + radius = 0 + current_time = rospy.Time.now() while (not rospy.is_shutdown()): ser.reset_input_buffer() -- 2.52.0