def update(self):
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
arduino_controller_msg = cmsg.JointCommandArduino()
dynamixel_controller_msg = cmsg.JointCommandDynamixel()
if self.arm_mode == 1:
self.extremum_update()
arduino_controller_msg.boomV = self.joy_val.boom * 100.0 + 2.5 * self.power_gradient + 0.5 * self.velocity_adaptation
else:
arduino_controller_msg.boomV = self.joy_val.boom * 100.0
arduino_controller_msg.armV = self.joy_val.arm * (-100.0)
if self.arm_mode == 0:
self.arm_motor_position_ref = self.arm_motor_position_ref + 0.5 * self.joy_val.arm
arduino_controller_msg.armP = self.arm_motor_position_ref
dynamixel_controller_msg.bucketV = self.joy_val.bucket * 1.0
arduino_controller_msg.BoomMode = self.boom_mode
arduino_controller_msg.ArmMode = self.arm_mode
self.pub_arduino.publish(arduino_controller_msg)
self.pub_dynamixel.publish(dynamixel_controller_msg)
r.sleep()
def update(self):
r = rospy.Rate(self.rate)
# Main Loop
while not rospy.is_shutdown():
arduino_controller_msg = cmsg.JointCommandArduino()
dynamixel_controller_msg = cmsg.JointCommandDynamixel()
arduino_controller_msg.boomV = self.joy_val.boom*100.0
arduino_controller_msg.armV = self.joy_val.arm*(-100.0)
if self.arm_mode == 0:
self.arm_motor_position_ref=self.arm_motor_position_ref + 0.1*self.joy_val.arm
arduino_controller_msg.armP = self.arm_motor_position_ref
dynamixel_controller_msg.bucketV = self.joy_val.bucket*1.0
arduino_controller_msg.BoomMode = self.boom_mode
arduino_controller_msg.ArmMode = self.arm_mode
self.pub_arduino.publish(arduino_controller_msg)
self.pub_dynamixel.publish(dynamixel_controller_msg)
r.sleep()
def update(self):
r = rospy.Rate(self.rate)
#modes for whole robot:
# 0 - disabled/rest
# 1 - manual joystick mode
# 2 - impedance mode tests
# 3 - trajectory tracking test
#modes for each joint:
# 0 - position control
# 1 - impedance simulated mode
# 2 - velocity ref to microcontroller
# 3 - disabled
while not rospy.is_shutdown():
arduino_controller_msg = cmsg.JointCommandArduino()
dynamixel_controller_msg = cmsg.JointCommandDynamixel()
arduino_controller_msg.BoomMode = self.boom_mode
arduino_controller_msg.ArmMode = self.arm_mode
if self.robot_mode == 0:
arduino_controller_msg.armV = 0
elif self.robot_mode == 1:
#set boom
arduino_controller_msg.boomV = self.joy_val.boom * 20.0
#set arm
self.arm_motor_position_ref = self.arm_motor_position_ref + 0.1 * self.joy_val.arm
arduino_controller_msg.armP = self.arm_motor_position_ref
arduino_controller_msg.armV = self.joy_val.arm * (-20.0)
#set bucket
dynamixel_controller_msg.bucketV = self.joy_val.bucket * 1.0
elif self.robot_mode == 2:
#set boom
self.extremum_update()
arduino_controller_msg.boomV = 2.0 * (
self.power_gradient + 0.7 * self.velocity_adaptation
) # + self.joy_val.boom*100.0
#set arm
#simulated dynamics set in microcontroller
#set bucket
dynamixel_controller_msg.bucketV = self.joy_val.bucket * 1.0
elif self.robot_mode == 3:
arduino_controller_msg.armV = 0
arduino_controller_msg.armP = self.arm_motor_position_ref
if (0.02 * self.boom_motor_position -
self.boom_calibration) > -0.70:
arduino_controller_msg.boomV = -0.0
else:
arduino_controller_msg.boomV = 0
if (self.bucket_motor_position) < -1.15:
dynamixel_controller_msg.bucketV = 0.8
else:
dynamixel_controller_msg.bucketV = 0
elif self.robot_mode == 5:
print(0.02 * self.arm_motor_position - self.arm_calibration)
if abs(0.02 * self.arm_motor_position - self.arm_calibration -
0.8) > 0.05:
if (0.02 * self.arm_motor_position -
self.arm_calibration) < 0.8:
self.arm_motor_position_ref = self.arm_motor_position_ref + 0.05 * 1.0
arduino_controller_msg.armP = self.arm_motor_position_ref
else:
self.arm_motor_position_ref = self.arm_motor_position_ref - 0.05 * 1.0
arduino_controller_msg.armP = self.arm_motor_position_ref
if abs(0.02 * self.boom_motor_position -
self.boom_calibration + 0.50) > 0.05:
if (0.02 * self.boom_motor_position -
self.boom_calibration) > -0.50:
arduino_controller_msg.boomV = -8
else:
arduino_controller_msg.boomV = 8
if abs(self.bucket_motor_position + 1.15) > 0.05:
if (self.bucket_motor_position) < -1.15:
dynamixel_controller_msg.bucketV = 0.7
else:
dynamixel_controller_msg.bucketV = -0.7
else:
arduino_controller_msg.boomV = self.joy_val.boom * 100.0
arduino_controller_msg.armV = self.joy_val.arm * (-100.0)
dynamixel_controller_msg.bucketV = self.joy_val.bucket * 1.0
self.pub_arduino.publish(arduino_controller_msg)
self.pub_dynamixel.publish(dynamixel_controller_msg)
r.sleep()
def update(self):
r = rospy.Rate(self.rate)
#modes for whole robot:
# 0 - disabled/rest
# 1 - manual joystick mode
# 2 - impedance mode tests
# 3 - trajectory tracking test
#modes for each joint:
# 0 - position control
# 1 - impedance simulated mode
# 2 - velocity ref to microcontroller
# 3 - disabled
while not rospy.is_shutdown():
arduino_controller_msg = cmsg.JointCommandArduino()
dynamixel_controller_msg = cmsg.JointCommandDynamixel()
arduino_controller_msg.BoomMode = self.boom_mode
arduino_controller_msg.ArmMode = self.arm_mode
if self.robot_mode == 0:
elif self.robot_mode == 1:
#set boom
arduino_controller_msg.boomV = self.joy_val.boom*100.0
#set arm
self.arm_motor_position_ref = self.arm_motor_position_ref + 0.5*self.joy_val.arm
arduino_controller_msg.armP = self.arm_motor_position_ref
arduino_controller_msg.armV = self.joy_val.arm*(-100.0)
#set bucket
dynamixel_controller_msg.bucketV = self.joy_val.bucket*1.0
elif: self.robot_mode == 2:
#set boom
self.trajectory_update()
self.extremum_update()
arduino_controller_msg.boomV = self.trajectory_setpoint_boomV 2.5*self.power_gradient + 0.5*self.velocity_adaptation # + self.joy_val.boom*100.0
#set arm
#simulated dynamics set in microcontroller
#set bucket
dynamixel_controller_msg.bucketV = self.joy_val.bucket*1.0
elif: self.robot_mode == 3:
self.trajectory_update()
arduino_controller_msg.boomV = self.trajectory_setpoint_boomV
arduino_controller_msg.armV = self.trajectory_setpoint_armV
dynamixel_controller_msg.bucketV = self.trajectory_setpoint_bucketV
else:
arduino_controller_msg.boomV = self.joy_val.boom*100.0
arduino_controller_msg.armV = self.joy_val.arm*(-100.0)
self.pub_arduino.publish(arduino_controller_msg)
self.pub_dynamixel.publish(dynamixel_controller_msg)
r.sleep()