fix odometry

diff --git a/otto_controller/Core/Src/main.cpp b/otto_controller/Core/Src/main.cpp
index 9037cd1d018f0114c0c31113fcf999e903425ecb..06216ed0d39e9dd138decea0113218c4ef76dc7d 100644 (file)
--- a/otto_controller/Core/Src/main.cpp
+++ b/otto_controller/Core/Src/main.cpp
@@ -58,7 +58,7 @@
 /* USER CODE BEGIN PV */\r
 \r
 //Parameters\r
-float baseline = 0.3;\r
+float baseline = 0.435;\r
 int ticks_per_revolution = 148000;  //x4 resolution\r
 float right_wheel_circumference = 0.783;  //in meters\r
 float left_wheel_circumference = 0.789;  //in meters\r
@@ -300,7 +300,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
     float left_wheel = left_encoder.GetLinearVelocity();\r
     float right_wheel = right_encoder.GetLinearVelocity();\r
 \r
-    odom.FromWheelVelToOdom(1, -1);\r
+    odom.FromWheelVelToOdom(left_wheel, right_wheel);\r
 \r
     status_msg.linear_velocity = odom.GetLinearVelocity();\r
     status_msg.angular_velocity = odom.GetAngularVelocity();\r
@@ -343,16 +343,12 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
     float left_setpoint = odom.GetLeftVelocity();\r
     float right_setpoint = odom.GetRightVelocity();\r
 \r
-//    left_pid.set(left_setpoint);\r
-//    right_pid.set(right_setpoint);\r
-\r
-    left_pid.set(0);\r
-    right_pid.set(0);\r
+    left_pid.set(left_setpoint);\r
+    right_pid.set(right_setpoint);\r
 \r
     float cross_setpoint = left_setpoint - right_setpoint;\r
-//    cross_pid.set(cross_setpoint);\r
+    cross_pid.set(cross_setpoint);\r
 \r
-    cross_pid.set(0);\r
 \r
   }\r
 \r

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 51cfb9eaa86e9cb292d010b01182a8975ae3965f..0acc9f90906de828507a281f24c002610e2258c1 100755 (executable)
--- 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()