fix odometry

iralabdisco · Mar 2, 2020 · 2d3d17b · 2d3d17b
1 parent 0a61780
commit 2d3d17b
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 12 deletions.
diff --git 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
@@ -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()