def init_rc():
global rc
global rc_address
# Initialise the roboclaw motorcontroller
print("logo: initialising roboclaw driver...")
from roboclaw_3 import Roboclaw
rc_address = 0x80
rc = Roboclaw("/dev/roboclaw", 115200)
rc.Open()
# Get roboclaw version to test if is attached
version = rc.ReadVersion(rc_address)
if version[0] is False:
print("logo init: roboclaw get version failed")
sys.exit()
else:
print("logo init:",repr(version[1]))
# Set motor controller variables to those required by K9
rc.SetM1VelocityPID(rc_address, M1_P, M1_I, M1_D, M1_QPPS)
rc.SetM2VelocityPID(rc_address, M2_P, M2_I, M2_D, M2_QPPS)
rc.SetMainVoltages(rc_address,240,292) # 24V min, 29.2V max
rc.SetPinFunctions(rc_address,2,0,0)
# Zero the motor encoders
rc.ResetEncoders(rc_address)
# Print Motor PID Settings
m1pid = rc.ReadM1VelocityPID(rc_address)
m2pid = rc.ReadM2VelocityPID(rc_address)
print("logo init: m1 p: " + str(m1pid[1]) + ", i:" + str(m1pid[2]) + ", d:" + str(m1pid[3]))
print("m2 p: " + str(m2pid[1]) + ", i:" + str(m2pid[2]) + ", d:" + str(m2pid[3]))
# Print Min and Max Main Battery Settings
minmaxv = rc.ReadMinMaxMainVoltages(rc_address) # get min max volts
print ("logo init: min main battery: " + str(int(minmaxv[1])/10.0) + "V")
print ("logo init: max main battery: " + str(int(minmaxv[2])/10.0) + "V")
# Print S3, S4 and S5 Modes
S3mode=['Default','E-Stop (latching)','E-Stop','Voltage Clamp','Undefined']
S4mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M1 Home']
S5mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M2 Home']
pinfunc = rc.ReadPinFunctions(rc_address)
print ("logo init: s3 pin: " + S3mode[pinfunc[1]])
print ("logo init: s4 pin: " + S4mode[pinfunc[2]])
print ("logo init: s5 pin: " + S5mode[pinfunc[3]])
print("logo init: roboclaw motor controller initialised...")
class Swag:
def __init__(self):
self.rc = Roboclaw("/dev/ttyACM0", 115200) # Linux comport name
self.address = 0x80
self.rc.Open()
self.ready = True
version = self.rc.ReadVersion(self.address)
if not version[0]:
print("GETVERSION Failed")
exit()
else:
print(repr(version[1]))
print("Car main battery voltage at start of script: ", self.rc.ReadMainBatteryVoltage(self.address))
for i in range(1000):
try:
self.rc.ForwardM2(self.address, i)
time.sleep(0.1)
print(i)
except Exception as e:
print(e)
def control_speed(self, mc, adr, speed_m1, speed_m2):
# speedM1 = leftMotorSpeed, speedM2 = rightMotorSpeed
if speed_m1 > 0:
mc.ForwardM1(adr, speed_m1)
elif speed_m1 < 0:
speed_m1 = speed_m1 * (-1)
mc.BackwardM1(adr, speed_m1)
else:
mc.ForwardM1(adr, 0)
if speed_m2 > 0:
mc.ForwardM2(adr, speed_m2)
elif speed_m2 < 0:
speed_m2 = speed_m2 * (-1)
mc.BackwardM2(adr, speed_m2)
else:
mc.ForwardM2(adr, 0)
import time
from roboclaw_3 import Roboclaw
#Windows comport name
rc = Roboclaw("/dev/ttyACM0", 115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
rc.Open()
while 1:
#Get version string
version = rc.ReadVersion(0x80)
if version[0] == False:
print("GETVERSION Failed")
else:
print(repr(version[1]))
time.sleep(1)
print("Speed1:")
if speed1[0]:
print(speed1[1])
else:
print("failed")
print("Speed2:")
if speed2[0]:
print(speed2[1])
else:
print("failed")
rc.open()
address = 0x80
version = rc.ReadVersion(address)
if not version[0]:
print("GET VERSION Failed")
else:
print(repr(version[1]))
nb = 0
while nb < 100:
rc.SpeedM1(address, 2000)
rc.SpeedM2(address, -2000)
for i in range(0, 200):
display_speed()
time.sleep(0.01)
rc.SpeedM1(address, -2000)
class Footballmachine:
def __init__(self, address=0x80, baudrate=38400, port="/dev/ttyS0"):
self.address = address
self.rc = Roboclaw(port, baudrate)
self.rc.Open()
version = self.rc.ReadVersion(self.address)
if version[0] == False:
print("GETVERSION Failed - check power supply and conections")
return
else:
print(repr(version[1]))
def has_angle_motor_stopped_moving(self):
interval = 1
first = self.rc.ReadEncM1(self.address)
sleep(interval)
second = self.rc.ReadEncM1(self.address)
while (first != second):
first = self.rc.ReadEncM1(self.address)
sleep(interval)
second = self.rc.ReadEncM1(self.address)
def init_motors(self):
print("Initializing all motors...")
self.rc.BackwardM1(self.address, 126)
self.has_angle_motor_stopped_moving()
self.rc.BackwardM1(self.address, 0)
self.rc.ResetEncoders(self.address)
print("Angle encoder:", self.rc.ReadEncM1(self.address)[1])
def displayspeed(self):
enc1 = self.rc.ReadEncM1(self.address)
enc2 = self.rc.ReadEncM2(self.address)
speed1 = self.rc.ReadSpeedM1(self.address)
speed2 = self.rc.ReadSpeedM2(self.address)
print(("Encoder1:"), end=' ')
if (enc1[0] == 1):
print(enc1[1], end=' ')
print(format(enc1[2], '02x'), end=' ')
else:
print("failed", end=' ')
print("Encoder2:", end=' ')
if (enc2[0] == 1):
print(enc2[1], end=' ')
print(format(enc2[2], '02x'), end=' ')
else:
print("failed ", end=' ')
print("Speed1:", end=' ')
if (speed1[0]):
print(speed1[1], end=' ')
else:
print("failed", end=' ')
print(("Speed2:"), end=' ')
if (speed2[0]):
print(speed2[1])
else:
print("failed ")
def speed_to_QPPS(self, speed):
radius = 0.1
encoder_pulses_per_rad = 1024 / 2
angular_speed = speed / (2 * pi * radius)
QPPS = encoder_pulses_per_rad * angular_speed
return int(QPPS)
def angle_to_QP(self, angle):
range_min = 0
range_max = 225
angle_min = 0
angle_max = 55
a1 = int(angle) - angle_min
a2 = range_max - range_min
a3 = angle_max - angle_min
angle = int((a1 * a2) / a3 + range_min)
return angle
def set_angle(self, angle):
print("Set_angle: ", angle)
angle = self.angle_to_QP(angle)
print("Target position M1:", angle)
self.rc.SpeedAccelDeccelPositionM1(self.address, 10, 10, 10, angle, 0)
self.has_angle_motor_stopped_moving()
print("Angle encoder:", self.rc.ReadEncM1(self.address)[1])
def set_speed_then_stop(self, speed):
print("Set_speed: ", speed)
speed = self.speed_to_QPPS(int(speed))
self.rc.SpeedAccelM2(self.address, 22000, speed)
sleep(4)
self.rc.SpeedAccelM2(self.address, 22000, 0)
def set_speed(self, speed):
print("Set_speed: ", speed)
speed = self.speed_to_QPPS(int(speed))
self.rc.SpeedAccelM2(self.address, 22000, speed)
for i in range(0, 50):
print(("{} # ".format(i)), end=' ')
self.displayspeed()
sleep(0.1)
def check_encoders(self, seconds):
for i in range(0, seconds):
print(("{} # ".format(i)), end=' ')
self.displayspeed()
sleep(0.1)
