Added scaling to odrive track velocities.

arbie_hardware/src/odrives/main.py

@@ -37,6 +37,9 @@ flippers_front_offset = 373
 flippers_rear_offset =  605
 flippers_scale = 2 * pi/1024.0
 
+# TODO: Set these as parameters
+odrives_cpr = 8192
+tracks_gear_ratio = 5
 
 # Service callbacks for flippers_front_...
 
@@ -89,7 +92,8 @@ def tracks_left_read(req):
 def tracks_left_write(req):
     if tracks.is_connected():
         # WRITE AXIS0
-        tracks.write_velocity_axis0(req.cmd.data)
+        vel_cmd = (req.cmd.data/(2*pi))*odrives_cpr*tracks_gear_ratio
+        tracks.write_velocity_axis0(vel_cmd);
         tracks_left_res.vel.data = req.cmd.data
         return WriteHardwareResponse(True)
     else:
@@ -111,6 +115,7 @@ def tracks_right_read(req):
 def tracks_right_write(req):
     if tracks.is_connected():
         # WRITE AXIS1
+        vel_cmd = (req.cmd.data/(2*pi))*odrives_cpr*tracks_gear_ratio
         tracks.write_velocity_axis1(req.cmd.data)
         tracks_right_res.vel.data = req.cmd.data
         return WriteHardwareResponse(True)