class MecanumRobot:
def __init__(self):
# GPIO.setmode(GPIO.BCM)
# GPIO.setwarnings(False)
self.address_front_wheels = 0x80
self.address_rear_wheels = 0x81
self.full_speed = 127
self.sleep_time = 0.005
self.roboclaw = Roboclaw("/dev/ttyS0", 38400)
self.roboclaw.Open()
print("Error")
print(self.roboclaw.ReadError(self.address_front_wheels))
self.roboclaw.SetMinVoltageMainBattery(self.address_front_wheels, 62)
self.roboclaw.SetMaxVoltageMainBattery(self.address_front_wheels, 112)
self.roboclaw.SetMinVoltageMainBattery(self.address_rear_wheels, 62)
self.roboclaw.SetMaxVoltageMainBattery(self.address_rear_wheels, 112)
def move_backward(self):
threading.Thread(target=self.roboclaw.BackwardM1, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM1, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
def move_forward(self):
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
def slide_right(self):
threading.Thread(target=self.roboclaw.BackwardM1, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
def slide_left(self):
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM1, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
def rotate_left(self):
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
def stop(self):
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_front_wheels, 0)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_rear_wheels, 0)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_front_wheels, 0)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_rear_wheels, 0)).start()
sleep(self.sleep_time)
#Linux comport name
rc = Roboclaw("/dev/ttyACM0", 115200)
rc.Open()
print("-----------------------------------------------------------")
if rc.Open() == 0:
print("No roboclaw found on the comport\n")
else:
print("Roboclaw connected\n")
name = ['claw', 'claw_2', 'claw_3']
address = [128, 129, 130]
for i in range(3):
try:
print('\t'.join((name[i], 'set up', str(rc.ReadError(address[i])))))
except:
print('\t'.join((name[i], "not found")))
print("-----------------------------------------------------------")
def pitchStop():
rc.SpeedM1(128, 0)
def rotationStop():
rc.SpeedM2(128, 0)
def columnStop():
rc.SpeedM1(129, 0)