diff --git a/dbw/dbw/dbw_read_vel.py b/dbw/dbw/dbw_read_vel.py index db0c528..cb29e40 100644 --- a/dbw/dbw/dbw_read_vel.py +++ b/dbw/dbw/dbw_read_vel.py @@ -89,15 +89,15 @@ def dbw_callback(self, msg): if gear == 1: print('gear: forward') - throttle_cmd = np.uint8(int(hex(int(250.0*(throttle_percentage+100)/200.)), 16)) + throttle_cmd = np.uint8(250.0*(throttle_percentage+100)/200.) elif gear == 2: print('gear: backward') - throttle_cmd = np.uint8(int(hex(int(250.0 * (100.0 - throttle_percentage) / 200.)), 16)) + throttle_cmd = np.uint8(250.0*(100.0 - throttle_percentage)/200.) else: print('gear: neutral') - throttle_cmd = np.uint8(int(hex(int(250.0 * (0 + 100) / 200.)), 16)) + throttle_cmd = np.uint8(250.0*(0+100)/200.) - steering_cmd = np.uint8(int(hex(int(250.0*(steering_percentage+100)/200.)), 16)) + steering_cmd = np.uint8(250.0*(steering_percentage+100)/200.) # throttle self.set_2_bytes_number(self.propulsion.data, throttle_cmd, 1) diff --git a/dbw/test/test_uint8_throttle.py b/dbw/test/test_uint8_throttle.py new file mode 100644 index 0000000..4dd8f0e --- /dev/null +++ b/dbw/test/test_uint8_throttle.py @@ -0,0 +1,34 @@ +import numpy as np + +throttle_cmd_old = [] +throttle_cmd_new = [] +steering_cmd_old = [] +steering_cmd_new = [] + +for i in range(12): + throttle_percentage = 100/11 * i + steering_percentage = 200/11 * i - 100 + + gear = i%3 + + if gear == 1: + print('gear: forward') + throttle_cmd_old.append(np.uint8(int(hex(int(250.0*(throttle_percentage+100)/200.)), 16))) + throttle_cmd_new.append(np.uint8(250.0*(throttle_percentage+100)/200.)) + elif gear == 2: + print('gear: backward') + throttle_cmd_old.append(np.uint8(int(hex(int(250.0*(100.0 - throttle_percentage)/200.)), 16))) + throttle_cmd_new.append(np.uint8(250.0*(100.0 - throttle_percentage)/200.)) + + else: + print('gear: neutral') + throttle_cmd_old.append(np.uint8(int(hex(int(250.0*(0+100)/200.)), 16))) + throttle_cmd_new.append(np.uint8(250.0*(0+100)/200.)) + + steering_cmd_old.append(np.uint8(int(hex(int(250.0*(steering_percentage+100)/200.)), 16))) + steering_cmd_new.append(np.uint8(250.0*(steering_percentage+100)/200.)) + +print(throttle_cmd_old) +print(throttle_cmd_new) +print(steering_cmd_old) +print(steering_cmd_new) \ No newline at end of file