def __init__(self, left_side_address=76, right_side_address=82):
self.left_front_motor = DrillerMotor(address=left_side_address, driller_num=0)
self.left_central_motor = DrillerMotor(address=left_side_address, driller_num=1)
self.left_rear_motor = DrillerMotor(address=left_side_address, driller_num=2)
self.right_front_motor = DrillerMotor(address=right_side_address, driller_num=0)
self.right_central_motor = DrillerMotor(address=right_side_address, driller_num=1)
self.right_rear_motor = DrillerMotor(address=right_side_address, driller_num=2)
self.sensivity = 0.5
self.left_power = 0
self.left_direction = 0
self.right_power = 0
self.right_direction = 0
def __init__(self, left_side_address=76, right_side_address=82):
self.left_front_motor = DrillerMotor(address=left_side_address,
driller_num=0)
self.left_central_motor = DrillerMotor(address=left_side_address,
driller_num=1)
self.left_rear_motor = DrillerMotor(address=left_side_address,
driller_num=2)
self.right_front_motor = DrillerMotor(address=right_side_address,
driller_num=0)
self.right_central_motor = DrillerMotor(address=right_side_address,
driller_num=1)
self.right_rear_motor = DrillerMotor(address=right_side_address,
driller_num=2)
self.sensivity = 0.5
self.left_power = 0
self.left_direction = 0
self.right_power = 0
self.right_direction = 0
class Chassis(object):
def __init__(self, left_side_address=76, right_side_address=82):
self.left_front_motor = DrillerMotor(address=left_side_address,
driller_num=0)
self.left_central_motor = DrillerMotor(address=left_side_address,
driller_num=1)
self.left_rear_motor = DrillerMotor(address=left_side_address,
driller_num=2)
self.right_front_motor = DrillerMotor(address=right_side_address,
driller_num=0)
self.right_central_motor = DrillerMotor(address=right_side_address,
driller_num=1)
self.right_rear_motor = DrillerMotor(address=right_side_address,
driller_num=2)
self.sensivity = 0.5
self.left_power = 0
self.left_direction = 0
self.right_power = 0
self.right_direction = 0
def drive(self, speed=0.0, rotation=0.0):
if rotation < 0:
rotation = math.fabs(rotation)
ratio = self.calculate_ratio(rotation)
left_speed = speed / ratio
right_speed = speed
self.calculate_steering(left_speed, right_speed)
self.steer_front()
self.steer_central()
self.steer_rear()
elif rotation > 0:
ratio = self.calculate_ratio(rotation)
left_speed = speed
right_speed = speed / ratio
self.calculate_steering(left_speed, right_speed)
self.steer_front()
self.steer_central()
self.steer_rear()
else:
self.calculate_steering(speed, speed)
self.steer_rear()
self.steer_central()
self.steer_front()
def stop(self):
self.left_front_motor.stop()
self.right_front_motor.stop()
self.left_central_motor.stop()
self.right_central_motor.stop()
self.left_rear_motor.stop()
self.right_rear_motor.stop()
def calculate_ratio(self, curve):
ratio = math.log(curve)
ratio = (ratio - self.sensivity) / (ratio + self.sensivity)
if ratio == 0:
ratio = .0000000001
return ratio
def calculate_steering(self, left_speed, right_speed):
self.left_direction = int(math.copysign(1, left_speed))
self.right_direction = int(math.copysign(1, right_speed))
self.left_power = math.fabs(left_speed)
self.left_power = int(self.left_power * 255)
self.right_power = math.fabs(right_speed)
self.right_power = int(self.right_power * 255)
def steer_front(self):
self.left_front_motor.set_direction(self.left_direction)
self.right_front_motor.set_direction(self.right_direction)
self.left_front_motor.set_power(self.left_power)
self.right_front_motor.set_power(self.right_power)
def steer_central(self):
self.left_central_motor.set_direction(self.left_direction)
self.right_central_motor.set_direction(self.right_direction)
self.left_central_motor.set_power(self.left_power)
self.right_central_motor.set_power(self.right_power)
def steer_rear(self):
self.left_rear_motor.set_direction(self.left_direction)
self.right_rear_motor.set_direction(self.right_direction)
self.left_rear_motor.set_power(self.left_power)
self.right_rear_motor.set_power(self.right_power)
class Chassis(object):
def __init__(self, left_side_address=76, right_side_address=82):
self.left_front_motor = DrillerMotor(address=left_side_address, driller_num=0)
self.left_central_motor = DrillerMotor(address=left_side_address, driller_num=1)
self.left_rear_motor = DrillerMotor(address=left_side_address, driller_num=2)
self.right_front_motor = DrillerMotor(address=right_side_address, driller_num=0)
self.right_central_motor = DrillerMotor(address=right_side_address, driller_num=1)
self.right_rear_motor = DrillerMotor(address=right_side_address, driller_num=2)
self.sensivity = 0.5
self.left_power = 0
self.left_direction = 0
self.right_power = 0
self.right_direction = 0
def drive(self, speed=0.0, rotation=0.0):
if rotation < 0:
rotation = math.fabs(rotation)
ratio = self.calculate_ratio(rotation)
left_speed = speed / ratio
right_speed = speed
self.calculate_steering(left_speed, right_speed)
self.steer_front()
self.steer_central()
self.steer_rear()
elif rotation > 0:
ratio = self.calculate_ratio(rotation)
left_speed = speed
right_speed = speed / ratio
self.calculate_steering(left_speed, right_speed)
self.steer_front()
self.steer_central()
self.steer_rear()
else:
self.calculate_steering(speed, speed)
self.steer_rear()
self.steer_central()
self.steer_front()
def stop(self):
self.left_front_motor.stop()
self.right_front_motor.stop()
self.left_central_motor.stop()
self.right_central_motor.stop()
self.left_rear_motor.stop()
self.right_rear_motor.stop()
def calculate_ratio(self, curve):
ratio = math.log(curve)
ratio = (ratio - self.sensivity) / (ratio + self.sensivity)
if ratio == 0:
ratio = .0000000001
return ratio
def calculate_steering(self, left_speed, right_speed):
self.left_direction = int(math.copysign(1, left_speed))
self.right_direction = int(math.copysign(1, right_speed))
self.left_power = math.fabs(left_speed)
self.left_power = int(self.left_power * 255)
self.right_power = math.fabs(right_speed)
self.right_power = int(self.right_power * 255)
def steer_front(self):
self.left_front_motor.set_direction(self.left_direction)
self.right_front_motor.set_direction(self.right_direction)
self.left_front_motor.set_power(self.left_power)
self.right_front_motor.set_power(self.right_power)
def steer_central(self):
self.left_central_motor.set_direction(self.left_direction)
self.right_central_motor.set_direction(self.right_direction)
self.left_central_motor.set_power(self.left_power)
self.right_central_motor.set_power(self.right_power)
def steer_rear(self):
self.left_rear_motor.set_direction(self.left_direction)
self.right_rear_motor.set_direction(self.right_direction)
self.left_rear_motor.set_power(self.left_power)
self.right_rear_motor.set_power(self.right_power)
