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()