def displayspeed():
enc1 = roboclaw.ReadEncM1(address)
enc2 = roboclaw.ReadEncM2(address)
speed1 = roboclaw.ReadSpeedM1(address)
speed2 = roboclaw.ReadSpeedM2(address)
print("Encoder1:"),
if (enc1[0] == 1):
print enc1[1],
print format(enc1[2], '02x'),
else:
print "failed",
print "Encoder2:",
if (enc2[0] == 1):
print enc2[1],
print format(enc2[2], '02x'),
else:
print "failed ",
print "Speed1:",
if (speed1[0]):
print speed1[1],
else:
print "failed",
print("Speed2:"),
if (speed2[0]):
print speed2[1]
else:
print "failed "
def readAngles(): kneeAngle = int(rc.ReadEncM1(config.address)[1]) hipAngle = int(rc.ReadEncM2(config.address)[1]) heelAngle = readPot() return kneeAngle, hipAngle, heelAngle
def update(self):
"""
Sends
"""
signal.setitimer(
signal.ITIMER_REAL, 0.25
) # set the watchdog for 0.25 seconds (SIGALARM_handler() will be
# called if this function takes too long)
try:
# Publish encoder readings
if self.m1_enc_pub is not None:
response_1 = roboclaw.ReadEncM1(self.address)
response_2 = roboclaw.ReadEncM2(self.address)
if response_1[0] == 0 or response_1[0] == 0:
rospy.logerr(
str((self.name, os.getpid())) +
": error returned from encoder reading: " +
str(response_1) + " " + str(response_2))
else:
m1_msg = Int32()
m1_msg.data = int(response_1[1])
m2_msg = Int32()
m2_msg.data = int(response_2[1])
self.m1_enc_pub.publish(m1_msg)
self.m2_enc_pub.publish(m2_msg)
# Publish current readings
if self.m1_enc_pub is not None:
currents = roboclaw.ReadCurrents(self.address)
if currents[0] == 0:
rospy.logerr(
str((self.name, os.getpid())) +
": error returned from current reading: " +
str(currents))
else:
m1_amp_msg = Float32()
m1_amp_msg.data = currents[1] / 100.0
m2_amp_msg = Float32()
m2_amp_msg.data = currents[2] / 100.0
self.m1_amp_pub.publish(m1_amp_msg)
self.m2_amp_pub.publish(m2_amp_msg)
bat_voltage = roboclaw.ReadMainBatteryVoltage(self.address)
if bat_voltage[0] == 0:
rospy.logerr(
str((self.name, os.getpid())) +
": error returned from battery voltage reading: " +
str(bat_voltage))
else:
bat_volt_msg = Float32()
bat_volt_msg.data = bat_voltage[1] / 10.0
self.battery_voltage_pub.publish(bat_volt_msg)
# Timeout if no command recieved for more than TIMEOUT_TIME
if int(round(
time.time() * 1000)) - self.timeout > self.TIMEOUT_TIME:
roboclaw.ForwardBackwardM1(self.address, 64)
roboclaw.ForwardBackwardM2(self.address, 64)
else:
# PWM control
if self.m1_pwm is not None:
roboclaw.ForwardBackwardM1(self.address, self.m1_pwm)
# roboclaw.DutyM1(self.address, (self.m1_pwm - 64) * int(32767 / 64))
self.m1_pwm = None
if self.m2_pwm is not None:
roboclaw.ForwardBackwardM2(self.address, self.m2_pwm)
# roboclaw.DutyM2(self.address, (self.m2_pwm - 64) * int(32768 / 64))
self.m2_pwm = None
# Velocity control
if self.m1_vel is not None:
# if the commanded velocity is 0 then turn the motors off instead of sending a zero speed.
# This is to keep the motors from overheating, we once left the rover still on a slight slope for
# a few minutes and the motors overheated trying to keep the rover still.
if self.m1_vel == 0:
roboclaw.DutyM1(self.address, 0)
else:
roboclaw.SpeedM1(self.address, self.m1_vel)
self.m1_vel = None
if self.m2_vel is not None:
if self.m2_vel == 0:
roboclaw.DutyM2(self.address, 0)
else:
roboclaw.SpeedM2(self.address, self.m2_vel)
self.m2_vel = None
# Position control
if self.m1_pos is not None:
roboclaw.SpeedAccelDeccelPositionM1(
self.address, 0, self.QPPS, 0, self.m1_pos, False)
self.m1_pos = None
if self.m2_pos is not None:
roboclaw.SpeedAccelDeccelPositionM2(
self.address, 0, self.QPPS, 0, self.m2_pos, False)
self.m2_pos = None
# this exception will be raised by the SIGALRM handler if sending/recieving data from the roboclaw took too long
except Exception as e:
rospy.logerr("SIGLARAM: " + str((self.name, os.getpid())) + ": " +
str(e))
def update(self):
for i, pub in enumerate(self.enc_publishers):
if pub is None:
continue
address = self.parameters[i]['address']
if self.parameters[i]['motor_num'] == 1:
response = roboclaw.ReadEncM1(address)
elif self.parameters[i]['motor_num'] == 2:
response = roboclaw.ReadEncM2(address)
else:
continue
msg = Int32()
msg.data = int(response[1])
pub.publish(msg)
for i, t in enumerate(self.timeouts):
if self.enc_publishers[i] is None:
continue
if int(round(time.time() * 1000)) - t > self.TIMEOUT_TIME:
address = self.parameters[i]['address']
if self.parameters[i]['motor_num'] == 1:
roboclaw.ForwardBackwardM1(address, 64)
elif self.parameters[i]['motor_num'] == 2:
roboclaw.ForwardBackwardM2(address, 64)
else:
continue
'''
for i, pwm in enumerate(self.pwms):
if pwm is None:
continue
if self.parameters[i] is not None:
address = self.parameters[i]['address']
if self.parameters[i]['motor_num'] == 1:
roboclaw.ForwardBackwardM1(address, pwm)
elif self.parameters[i]['motor_num'] == 2:
roboclaw.ForwardBackwardM2(address, pwm)
self.pwms[i] = None
'''
for i, vel in enumerate(self.velocities):
if vel is None:
continue
#print('found_vel: ' + str(vel))
if self.parameters[i] is not None:
address = self.parameters[i]['address']
#print('address: ' + str(address))
if self.parameters[i]['motor_num'] == 1:
#print('setting m1 speed')
if roboclaw.SpeedM1(address, vel):
#print('m1_set')
pass
else:
#print('m1_failed')
pass
elif self.parameters[i]['motor_num'] == 2:
#print('setting m2 speed')
if roboclaw.SpeedM2(address, vel):
#print('m1_set')
pass
else:
#print('m1_failed')
pass
self.velocities[i] = None
'''
def zero_func():
"""
Finds values for each encoder to be used as position reference.
Arguments:
(None)
Returns:
ref -- list of reference encoder values
"""
ref = [0]*3
ref[0] = rc.ReadEncM1(address)[1]
ref[1] = rc.ReadEncM2(address)[1]
ref[2] = 0 # read Pot Value
return ref # make this a global? Need to deal with
# user input --> then start trying to reach defined positions
# while running, return angle values with feedback, tighten string and get setup finalized
# when ready, return message that everything is good
# options: go to neutral state, or directly into primed state
def push_into_position_func():
"""
Applies constant torques to push apparatus into a specified position.
Reads config file to get target positions.
