Merge pull request #26 from iralabdisco/fixes

Fix odometry
iralabdisco · Mar 2, 2020 · cadb9be · cadb9be
2 parents 0a61780 + c11f765
commit cadb9be
Show file tree

Hide file tree

Showing 5 changed files with 11 additions and 20 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/launch/dario.launch b/utils/catkin_ws/launch/dario.launch
@@ -1,7 +1,4 @@
-<launch>
-  <!-- Used to transmit and receive from serial -->
-  <include file="serial_bridge.launch" />
-
+<launch>  
   <!-- Used to receive cmd_vel from Arduino -->
   <node name="rosserial_python" pkg="rosserial_python" type="serial_node.py">
 	  <param name="port" value="/dev/ttyUSB1" />

diff --git a/utils/catkin_ws/launch/joypad.launch b/utils/catkin_ws/launch/joypad.launch
@@ -1,7 +1,4 @@
 <launch>
-  <!-- Used to transmit and receive from serial -->
-  <include file="serial_bridge.launch" />
-
   <node name="joy_node" pkg="joy" type="joy_node" />
   <node name="joy_to_cmd_vel" pkg="joypad_bridge" type="joy_to_cmd_vel.py"/>
 </launch>
diff --git a/utils/catkin_ws/launch/serial_bridge.launch b/utils/catkin_ws/launch/serial_bridge.launch
@@ -5,7 +5,7 @@
 
   <!-- Node used to slow down /cmd_vel transmission to 10Hz -->
   <node name="cmd_vel_throttler" type="throttle" pkg="topic_tools" 
-    args="messages /cmd_vel 10 /cmd_vel_throttled" />
+    args="messages /cmd_vel 20 /cmd_vel_throttled" />
 
   <!-- Serial port -->  
   <param name="serial_port" value="/dev/ttyUSB0"/>

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()