def stop_motors():
print('shutting down motors...')
rc = Roboclaw("/dev/ttyACM0", 115200)
rc.Open()
address = 0x80
rc.SpeedM1(address, 0) # M1 for linear movement
rc.SpeedM2(address, 0) # M2 for turning
def run_motors(M1_counts, M2_counts):
### send motor commands M1 forward, M2 turning###
rc = Roboclaw("/dev/ttyACM0", 115200)
rc.Open()
address = 0x80
print('running motors')
rc.SpeedM1(address, int(M1_counts)) # M1 for linear movement
rc.SpeedM2(address, int(M2_counts)) # M2 for turning
display_speed()
display_power_values()
else:
print "failed",
print("Speed2:"),
if (speed2[0]):
print speed2[1]
else:
print "failed "
rc.Open()
address = 0x80
version = rc.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
while (1):
# rc.SpeedM1(address,12000)
rc.SpeedM2(address, -12000)
for i in range(0, 20):
displayspeed()
time.sleep(0.01)
# rc.SpeedM1(address,-12000)
rc.SpeedM2(address, 12000)
for i in range(0, 20):
displayspeed()
time.sleep(0.01)
class motor_driver_ada:
def __init__(self, log):
self.lfbias = 48 # experimentally determined for 'Spot 2'
self.lrbias = 44
self.rrbias = 69
self.rfbias = 40
self.pan_bias = 83
self.left_limit = -36
self.right_limit = 36
self.d1 = 7.254 #C/L to corner wheels
self.d2 = 10.5 #mid axle to fwd axle
self.d3 = 10.5 #mid axle to rear axle
self.d4 = 10.073 #C/L to mid wheels
self.speedfactor = 35 # 8000 counts at 100%
self.rr_motor = kit.servo[0]
self.rf_motor = kit.servo[1]
self.lf_motor = kit.servo[2]
self.lr_motor = kit.servo[3]
self.pan = kit.servo[15]
self.tilt = kit.servo[14]
#pan_bias = 0 self.rr_motor.actuation_range = 120
self.rf_motor.actuation_range = 120
self.lf_motor.actuation_range = 120
self.lr_motor.actuation_range = 120
self.rr_motor.set_pulse_width_range(700, 2300)
self.rf_motor.set_pulse_width_range(700, 2300)
self.lf_motor.set_pulse_width_range(700, 2300)
self.lr_motor.set_pulse_width_range(700, 2300)
self.log = log
self.rc = Roboclaw("/dev/ttyS0", 115200)
i = self.rc.Open()
self.crc = 0
self.port = serial.Serial("/dev/ttyS0", baudrate=115200, timeout=0.1)
self.lf_motor.angle = self.rfbias
self.rf_motor.angle = self.lfbias
self.lr_motor.angle = self.lrbias
self.rr_motor.angle = self.rrbias
self.stop_all()
ver = self.rc.ReadVersion(0x80)
print(ver[0], ver[1])
ver = self.rc.ReadVersion(0x81)
print(ver[0], ver[1])
ver = self.rc.ReadVersion(0x82)
print(ver[0], ver[1])
def diag(self):
print("servo rr =" + str(self.rr_motor.angle))
print("servo rf =" + str(self.rf_motor.angle))
print("servo lf =" + str(self.lf_motor.angle))
print("servo lr =" + str(self.lr_motor.angle))
# self.turn_motor(0x80, vel, voc, 1)
def set_motor(self, address, v, av, m12):
vx = int(v * av)
if (m12 == 1):
self.rc.SpeedM1(address, vx)
else:
self.rc.SpeedM2(address, vx)
'''
def turn_motor(self, address, v, av1, av2):
v1 = int(v * av1)
v2 = int(v * av2)
if v >= 0:
self.rc.ForwardM1(address, v1)
self.rc.ForwardM2(address, v2)
# self.M1Forward(address, v1)
# self.M2Forward(address, v2)
else:
self.rc.BackwardM1(address, abs(v1))
self.rc.BackwardM2(address, abs(v2))
# self.M1Backward(address, abs(v1))
# self.M2Backward(address, abs(v2))
# print("m1, m2 = "+str(v1)+", "+str(v2))
'''
def stop_all(self):
self.set_motor(0X80, 0, 0, 1)
self.set_motor(0X81, 0, 0, 1)
self.set_motor(0X82, 0, 0, 1)
self.set_motor(0X80, 0, 0, 2)
self.set_motor(0X81, 0, 0, 2)
self.set_motor(0X82, 0, 0, 2)
def motor_speed(self):
speed1 = self.rc.ReadSpeedM1(0x80)
speed2 = self.rc.ReadSpeedM2(0x80)
self.log.write("motor speed = %d, %d" % (speed1[1], speed2[1]))
print("motor speed = %d, %d" % (speed1[1], speed2[1]))
speed1 = self.rc.ReadSpeedM1(0x81)
speed2 = self.rc.ReadSpeedM2(0x81)
self.log.write("motor speed = %d, %d" % (speed1[1], speed2[1]))
print("motor speed = %d, %d" % (speed1[1], speed2[1]))
speed1 = self.rc.ReadSpeedM1(0x82)
speed2 = self.rc.ReadSpeedM2(0x82)
self.log.write("motor speed = %d, %d" % (speed1[1], speed2[1]))
print("motor speed = %d, %d" % (speed1[1], speed2[1]))
# self.battery_voltage()
err = self.rc.ReadError(0x80)[1]
if err:
print("status of 0x80", err)
self.log.write("0x80 error: %d" % err)
err = self.rc.ReadError(0x81)[1]
if err:
print("status of 0x81", err)
self.log.write("0x81 error: %d" % err)
err = self.rc.ReadError(0x82)[1]
if err:
print("status of 0x82", err)
self.log.write("0x82 error: %d" % err)
def battery_voltage(self):
volts = self.rc.ReadMainBatteryVoltage(0x80)[1] / 10.0
print("Ada Voltage = ", volts)
self.log.write("Voltage: %5.1f\n" % volts)
return (volts)
# based on speed & steer, command all motors
def motor(self, speed, steer):
# self.log.write("Motor speed, steer "+str(speed)+", "+str(steer)+'\n')
if (steer < self.left_limit):
steer = self.left_limit
if (steer > self.right_limit):
steer = self.right_limit
# vel = speed * 1.26
vel = self.speedfactor * speed
voc = 0
vic = 0
#roboclaw speed limit 0 to 127
# see BOT-2/18 (181201)
# math rechecked 200329
if steer != 0: #if steering angle not zero, compute angles, wheel speed
angle = math.radians(abs(steer))
ric = self.d3 / math.sin(angle) #turn radius - inner corner
rm = ric * math.cos(angle) + self.d1 #body central radius
roc = math.sqrt((rm + self.d1)**2 + self.d3**2) #outer corner
rmo = rm + self.d4 #middle outer
rmi = rm - self.d4 #middle inner
phi = math.degrees(math.asin(self.d3 / roc))
if steer < 0:
phi = -phi
voc = roc / rmo #velocity corners & middle inner
vic = ric / rmo
vim = rmi / rmo
# SERVO MOTORS ARE COUNTER CLOCKWISE
# left turn
if steer < 0:
self.lf_motor.angle = self.lfbias - steer
self.rf_motor.angle = self.rfbias - phi
self.lr_motor.angle = self.lrbias + steer
self.rr_motor.angle = self.rrbias + phi
# self.turn_motor(0x80, vel, voc, 1) #RC 1 - rf, rm
# self.turn_motor(0x81, vel, voc, vic) #RC 2 - lm, lf
# self.turn_motor(0x82, vel, vim, vic) #RC 3 - rr, lr
self.set_motor(0x80, vel, voc, 1) #RC 1 - rf, rm
self.set_motor(0x81, vel, voc, 1) #RC 2 - lm, lf
self.set_motor(0x82, vel, vim, 1) #RC 3 - rr, lr
self.set_motor(0x80, vel, 1, 2) #RC 1 - rf, rm
self.set_motor(0x81, vel, vic, 2) #RC 2 - lm, lf
self.set_motor(0x82, vel, vic, 2) #RC 3 - rr, lr
# cstr = "v, vout, vin %f %f %f\n" % (vel, voc, vic)
# self.log.write(cstr)
#right turn
elif steer > 0:
self.lf_motor.angle = self.lfbias - phi
self.rf_motor.angle = self.rfbias - steer
self.lr_motor.angle = self.lrbias + phi
self.rr_motor.angle = self.rrbias + steer
# self.turn_motor(0x80, vel, vic, vim)
# self.turn_motor(0x81, vel, vic, voc)
# self.turn_motor(0x82, vel, 1, voc)
self.set_motor(0x80, vel, vic, 1)
self.set_motor(0x81, vel, vic, 1)
self.set_motor(0x82, vel, 1, 1)
self.set_motor(0x80, vel, vim, 2)
self.set_motor(0x81, vel, voc, 2)
self.set_motor(0x82, vel, voc, 2)
# print("80 vic, vim ",vic,vim)
# print("81 vic, voc ",vic,voc)
# print("82 vom, voc ", 1, voc)
# cstr = "v, vout, vin %f %f %f\n" % (vel, voc, vic)
# self.log.write(cstr)
#straight ahead
else:
self.lf_motor.angle = self.lfbias
self.rf_motor.angle = self.rfbias
self.lr_motor.angle = self.lrbias
self.rr_motor.angle = self.rrbias
self.set_motor(0x80, vel, 1, 1)
self.set_motor(0x81, vel, 1, 1)
self.set_motor(0x82, vel, 1, 1)
self.set_motor(0x80, vel, 1, 2)
self.set_motor(0x81, vel, 1, 2)
self.set_motor(0x82, vel, 1, 2)
# print("v, vout, vin "+str(vel)+", "+str(voc)+", "+str(vic))
# self.diag()
# cstr = "v, vout, vin %f %f %f\n" % (vel, voc, vic)
# self.log.write(cstr)
def sensor_pan(self, angle):
self.pan.angle = self.pan_bias + angle
def depower(self):
self.lf_motor.angle = None
self.rf_motor.angle = None
self.lr_motor.angle = None
self.rr_motor.angle = None
self.pan.angle = None
class Argo:
def __init__(self):
print "Trying to connect to motors..."
self.motor_status = 0
try: # First step: connect to Roboclaw controller
self.port = "/dev/ttyACM0"
self.argo = Roboclaw(self.port, 115200)
self.argo.Open()
self.address = 0x80 # Roboclaw address
self.version = self.argo.ReadVersion(
self.address
) # Test connection by getting the Roboclaw version
except:
print "Unable to connect to Roboclaw port: ", self.port, "\nCheck your port and setup then try again.\nExiting..."
self.motor_status = 1
return
# Follow through with setup if Roboclaw connected successfully
print "Roboclaw detected! Version:", self.version[1]
print "Setting up..."
# Set up publishers and subscribers
self.cmd_sub = rospy.Subscriber("/argo/wheel_speeds", Twist,
self.get_wheel_speeds)
self.encoder = rospy.Publisher("/argo/encoders", Encoder, queue_size=5)
# Set class variables
self.radius = 0.0728 # Wheel radius (m)
self.distance = 0.372 # Distance between wheels (m)
self.max_speed = 13000 # Global max speed (in QPPS)
self.session_max = 13000 # Max speed for current session (in QPPS)
self.rev_counts = 3200 # Encoder clicks per rotation
self.circ = .4574 # Wheel circumference (m)
self.counts_per_m = int(self.rev_counts /
self.circ) # Encoder counts per meter
self.conv = self.rev_counts / (2 * pi) # Wheel speed conversion factor
self.Lref = 0 # Left wheel reference speed
self.Rref = 0 # Right wheel reference speed
self.Lprev_err = 0 # Previous error value for left wheel
self.Rprev_err = 0 # Previous error value for right wheel
self.Kp = .004 # Proportional gain
self.Kd = .001 # Derivative gain
self.Ki = .0004 # Integral gain
self.LEint = 0 # Left wheel integral gain
self.REint = 0 # Right wheel integral gain
print "Setup complete, let's roll homie ;)\n\n"
def reset_controller(self):
self.LEint = 0
self.REint = 0
self.Lprev_err = 0
self.Rprev_err = 0
def pd_control(self, Lspeed, Rspeed):
# Controller outputs a percent effort (0 - 100) which will be applied to the reference motor speeds
feedback = self.read_encoders()
M1 = feedback.speedM1
M2 = feedback.speedM2
# Calculate current speed error for both motors
Lerror = Lspeed - M2
Rerror = Rspeed - M1
# Calculate derivative error
Ldot = Lerror - self.Lprev_err
Rdot = Rerror - self.Rprev_err
# Calculate integral error
self.LEint += Lerror
self.REint += Rerror
# Compute effort
Lu = self.Kp * Lerror + self.Kd * Ldot + self.Ki * self.LEint
Ru = self.Kp * Rerror + self.Kd * Rdot + self.Ki * self.REint
# Saturate efforts if it is over +/-100%
if Lu > 100.0:
Lu = 100.0
elif Lu < -100.0:
Lu = -100
if Ru > 100.0:
Ru = 100.0
elif Ru < -100.0:
Ru = -100.0
# Set new L and R speeds
Lspeed = int((Lu / 100) * self.session_max)
Rspeed = int((Ru / 100) * self.session_max)
self.Rprev_err = Rerror
self.Lprev_err = Lerror
return (Lspeed, Rspeed)
def move(self, Lspeed, Rspeed):
if Lspeed == 0 and Rspeed == 0:
self.argo.SpeedM1(self.address, 0)
self.argo.SpeedM2(self.address, 0)
return
(Lspeed, Rspeed) = self.pd_control(Lspeed, Rspeed)
self.argo.SpeedM1(self.address, Rspeed)
self.argo.SpeedM2(self.address, Lspeed)
def force_speed(self, Lspeed, Rspeed):
self.argo.SpeedM1(self.address, Rspeed)
self.argo.SpeedM2(self.address, Lspeed)
def get_velocity(self, vx, vy, ax, ay):
v = sqrt((vx * vx) + (vy * vy)) # Linear velocity
# if vx < 0:
# v = -v
w = (vy * ax - vx * ay) / ((vx * vx) + (vy * vy)) # Angular velocity
return (v, w)
def get_wheel_speeds(self, data):
r = self.radius
d = self.distance
v = data.linear.x
w = data.angular.z
# Calculate left/right wheel speeds and round to nearest integer value
Ul = (v - w * d) / r
Ur = (v + w * d) / r
# Convert to QPPS
Rspeed = int(round(Ur * self.conv))
Lspeed = int(round(Ul * self.conv))
if Rspeed > 0:
Rspeed = Rspeed + 80
elif Rspeed < 0:
Rspeed = Rspeed - 20
self.move(Lspeed, Rspeed)
def reset_encoders(self):
self.argo.ResetEncoders(self.address)
def read_encoders(self):
mov = Encoder()
t = rospy.Time.now()
# Get encoder values from Roboclaw
enc2 = self.argo.ReadEncM2(self.address)
enc1 = self.argo.ReadEncM1(self.address)
# Get motor speeds
sp1 = self.argo.ReadSpeedM1(self.address)
sp2 = self.argo.ReadSpeedM2(self.address)
# Extract encoder ticks and motor speeds, and publish to /argo/encoders topic
mov.stamp.secs = t.secs
mov.stamp.nsecs = t.nsecs
mov.encoderM1 = enc1[1]
mov.encoderM2 = enc2[1]
mov.speedM1 = sp1[1]
mov.speedM2 = sp2[1]
return mov
def check_battery(self):
battery = self.argo.ReadMainBatteryVoltage(self.address)
return battery[1]
class Controller():
'''
Controller class where it contains the PID equation
'''
def __init__(self, configPath):
print 'reading config from path...' + configPath
parser = SafeConfigParser()
parser.read(configPath)
#Reading the configurations
self._P = parser.get('pid_coeff', 'P')
self._I = parser.get('pid_coeff', 'I')
self._D = parser.get('pid_coeff', 'D')
self._PR = parser.get('pid_coeff', 'PR')
self._PL = parser.get('pid_coeff', 'PL')
self._speed = parser.get('motion', 'speed')
self._accel = parser.get('motion', 'accel')
self._samplingRate = parser.get('pid_coeff', 'samplingRate')
self._port = parser.get('systems', 'port')
self._robot = Robot()
#setup the roboclaw here
self._rc = Roboclaw(self._port, 115200)
self._rc.Open()
#Obsolete
# def P():
# doc = "The P property."
# def fget(self):
# return self._P
# def fset(self, value):
# self._P = value
# def fdel(self):
# del self._P
# return locals()
# P = property(**P())
#
# def I():
# doc = "The I property."
# def fget(self):
# return self._I
# def fset(self, value):
# self._I = value
# def fdel(self):
# del self._I
# return locals()
# I = property(**I())
#
# def D():
# doc = "The D property."
# def fget(self):
# return self._D
# def fset(self, value):
# self._D = value
# def fdel(self):
# del self._D
# return locals()
# D = property(**D())
def _getCurDegree(self):
return self._robot.degree
def _getCorrection(self, toBeDegree):
#error Negative value == moving to the left
#error Positive value == moving to the right
error = toBeDegree - self._getCurDegree()
print 'Error: ' + str(error)
ctlSig = self._P * error #Simple P control Signal
print 'Control signal: ' + str(ctlSig)
return ctlSig
def _getLeftWheelSpeed(self, toBeDegree):
ctlSig = self._getCorrection(toBeDegree)
speed = self._speed - self._speed * ctlSig
return speed
def _getRightWheelSpeed(self, toBeDegree):
ctlSig = self._getCorrection(toBeDegree)
speed = (self._speed + self._speed * ctlSig) * -1
return speed
def _moveFW(self, speedL, speedR):
self._rc.SpeedAccelM1(constants.ADDRESS, self._accel, speedL)
self._rc.SpeedAccelM2(constants.ADDRESS, self._accel, speedR)
def stop(self):
self._rc.SpeedM1(constants.ADDRESS, 0)
self._rc.SpeedM2(constants.ADDRESS, 0)
def moveFWControlled(self, degree, seconds):
time = 0
while (time < seconds):
speedL = self._getLeftWheelSpeed(degree)
speedR = self._getRightWheelSpeed(degree)
self._moveFW(speedL, speedR)
sleep(self._samplingRate / 1000) #convert ms to secs
time = time + self._samplingRate / 1000
self.stop()