def SpeedM(motor, omega):
# Convert to rotations/second
qpps = radian2Qpps(omega)
if motor == 1:
return roboclaw.SpeedM1(addr1, qpps)
elif motor == 2:
return roboclaw.SpeedM2(addr1, qpps)
elif motor == 3:
return roboclaw.SpeedM1(addr2, qpps)
else:
raise mlibExcept(e_type.motorNumOff)
def velCallback(cmd_vel):
CONVERSION = 60000 # Conversion factor from the -1...1 input to the total velocity
ANGLE_DIM = 0.1 # Conversion factor to make the twists solwer
netVel = (cmd_vel.linear.x**2 + cmd_vel.linear.y**2)**0.5
angle = atan2(cmd_vel.linear.y, cmd_vel.linear.x)
y = cmd_vel.linear.y
x = cmd_vel.linear.x
roboclaw.Open("/dev/ttyACM0", 115200)
roboclaw.SpeedM1(
FLeftAddress,
int(CONVERSION *
(netVel * sin(angle + pi / 4) + cmd_vel.angular.z * ANGLE_DIM)))
roboclaw.SpeedM2(
FLeftAddress,
int(CONVERSION *
(netVel * sin(angle + pi / 4) - cmd_vel.angular.z * ANGLE_DIM)))
time.sleep(.01)
roboclaw.Open("/dev/ttyACM1", 115200)
roboclaw.SpeedM1(
FRightAddress,
int(-1 * CONVERSION *
(netVel * cos(angle + pi / 4) + cmd_vel.angular.z * ANGLE_DIM)))
roboclaw.SpeedM2(
FRightAddress,
int(-1 * CONVERSION *
(netVel * cos(angle + pi / 4) - cmd_vel.angular.z * ANGLE_DIM)))
# Print speeds of motors
rospy.loginfo("-----------------------------------")
rospy.loginfo("Front right " + str(
int(CONVERSION * (cmd_vel.linear.y - cmd.vel.linear.x) /
((cmd_vel.linear.y**2 + cmd_vel.linear.x**2)**(0.5)))))
rospy.loginfo("Front left " + str(
int(CONVERSION * (cmd_vel.linear.y + cmd.vel.linear.x) /
((cmd_vel.linear.y**2 + cmd_vel.linear.x**2)**(0.5)))))
rospy.loginfo("Back right " + str(
int(CONVERSION * (cmd_vel.linear.y - cmd.vel.linear.x) /
((cmd_vel.linear.y**2 + cmd_vel.linear.x**2)**(0.5)))))
rospy.loginfo("Back left " + str(
int(CONVERSION * (cmd_vel.linear.y + cmd.vel.linear.x) /
((cmd_vel.linear.y**2 + cmd_vel.linear.x**2)**(0.5)))))
address = 0x80
version = roboclaw.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
#Velocity PID coefficients
Kp = 35.0
Ki = 10.0
Kd = 0
qpps = 44000
#Set PID Coefficients
roboclaw.SetM1VelocityPID(address, Kp, Kd, Ki, qpps)
roboclaw.SetM2VelocityPID(address, Kp, Kd, Ki, qpps)
while (1):
roboclaw.SpeedM1(address, 12000)
roboclaw.SpeedM2(address, -12000)
for i in range(0, 200):
displayspeed()
time.sleep(0.01)
roboclaw.SpeedM1(address, -12000)
roboclaw.SpeedM2(address, 12000)
for i in range(0, 200):
displayspeed()
time.sleep(0.01)
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 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))
#Windows comport name
#roboclaw.Open("COM3",38400)
#Linux comport name
roboclaw.Open("/dev/ttySAC0", 38400)
address = 0x80
version = roboclaw.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
roboclaw.SpeedM1(address, 10)
roboclaw.SpeedM2(address, 10)
time.sleep(2)
roboclaw.SpeedM1(address, 0)
roboclaw.SpeedM2(address, 0)
# while(1):
# try:
# speed = int(raw_input("Enter speed: "))
# roboclaw.SpeedM1(address,speed)
# except ValueError:
# print "Not a number"
# roboclaw.SpeedM1(address,12000)
# roboclaw.SpeedM2(address,-12000)
# for i in range(0,200):
