def set_wheel_speeds(self, left_speed, right_speed):
'''Sets the speed of both wheels to the speed'''
self.get_mode()
self.left_speed = generic_functions.constrainf(left_speed + 1, -100,
100)
self.right_speed = generic_functions.constrainf(
right_speed + 1, -100, 100)
try:
self.motor_direction_left = self.left_speed / abs(self.left_speed)
except ZeroDivisionError as e:
self.motor_direction_left = 0.0
try:
self.motor_direction_right = self.right_speed / abs(
self.right_speed)
except:
self.motor_direction_right = 0.0
if (self.mode == 0) or (self.mode == 1):
left_value = self.__translate_value(self.left_speed, -100, 100)
right_value = self.__translate_value(self.right_speed, -100, 100)
self.i2c_object.write(0, left_value)
self.i2c_object.write(1, right_value)
elif (self.mode == 2) or (self.mode == 3):
if self.left_speed != self.right_speed:
raise ValueError(
"Both wheel speeds({1}, {2}) must be the same in this mode({0})"
.format(self.mode, self.left_speed, self.right_speed))
else:
value = self.__translate_value(self.left_speed, -100, 100)
self.i2c_object.write(0, value)
else:
raise ValueError("Invalid mode({0}) detected".format(self.mode))
def __update_command_wheel_speed(self, amplitude=0.25):
# Calculate wheel speed error
left_speed_error = self.target_left_speed - self.left_wheel_speed
right_speed_error = self.target_right_speed - self.right_wheel_speed
# Calculate the new motor power
self.left_command_wheel_power = self.left_command_wheel_power + (
amplitude * left_speed_error)
self.right_command_wheel_power = self.right_command_wheel_power + (
amplitude * right_speed_error)
# Constrain motor power
self.left_command_wheel_power = generic_functions.constrainf(
self.left_command_wheel_power, -100.0, 100.0)
self.right_command_wheel_power = generic_functions.constrainf(
self.right_command_wheel_power, -100.0, 100.0)
# Set the motor power
return self.wheels_controller.set_wheel_speeds(
self.left_command_wheel_power, self.right_command_wheel_power)
def change_address(self, new_address_index):
'''Sets the address of the md25 to the new address selected by the index'''
possible_addresses = (0xB0, 0xB2, 0xB4, 0xB6, 0xB8, 0xBA, 0xBC, 0xBE)
new_address_index = generic_functions.constrainf(
new_address_index, 0,
len(possible_addresses) - 1)
new_address = possible_addresses[new_address_index]
# Data to send
cmd = (0xA0, 0xAA, 0xA5, new_address)
self.i2c_object.write(16, cmd)
self.address = new_address
return self.address
def __translate_value(self, value, low_threshold, high_threshold):
'''Returns the right value based on the current mode'''
self.get_mode() # Gets the mode of the MD25
value = generic_functions.constrainf(value, low_threshold,
high_threshold)
if self.mode == 0 or self.mode == 2:
value = generic_functions.mapf(value, low_threshold,
high_threshold, 0, 255)
elif self.mode == 1 or self.mode == 3:
value = generic_functions.mapf(value, low_threshold,
high_threshold, -128, 127)
else:
raise ValueError("Invalid mode({0}) detected.".format(self.mode))
return int(value)
def set_wheel_speed_left(self, speed):
'''Sets the speed of the left wheel'''
self.get_mode()
self.left_speed = generic_functions.constrainf(speed + 1, -100, 100)
try:
self.motor_direction_left = self.left_speed / abs(self.left_speed)
except ZeroDivisionError as e:
self.motor_direction_left = 0.0
if (self.mode == 0) or (self.mode == 1):
left_value = self.__translate_value(self.left_speed, -100, 100)
self.i2c_object.write(0, left_value)
elif (self.mode == 2) or (self.mode == 3):
value = self.__translate_value(self.left_speed, -100, 100)
self.i2c_object.write(0, value)
else:
raise ValueError("Invalid mode({0}) detected".format(self.mode))
def set_mode(self, mode):
'''Sets the mode of the controller'''
# Ensure that the mode is between the acceptable range
self.mode = int(generic_functions.constrainf(int(mode), 0, 3))
self.i2c_object.write(15, self.mode)
def set_acceleration_rate(self, rate):
'''Sets the acceleration to one of 10 steps'''
rate = generic_functions.constrainf(rate, 0, 10)
self.i2c_object.write(14, rate)