def robotInit(self):
self.Arm1 = wpilib.CANTalon(0)
self.Arm2 = wpilib.CANTalon(1)
self.Control = ArmControl(self.Arm1, self.Arm2)
self.Control.setOffset1(-math.pi)
#self.Control.moveToPosition(20, 17.5)
#self.Control.moveMotorRotation(math.pi/2, math.pi/2)
self.Joystick = wpilib.Joystick(0)
self.TargetX = math.atan2(20,17.5)
self.TargetY = math.sqrt(20**2 + 17.5**2) #mag
def main():
rospy.init_node("arm_controller")
cfg_server = Server(armConfig, lambda config, level: config)
#start two arm control for the robot
right_arm = ArmControl(cfg_server, "right")
left_arm = ArmControl(cfg_server, "left")
_loop_rate = rospy.Rate(100)
while not rospy.is_shutdown():
#do other important stuff here
#look up whether there is any change in poses, etc...
#currently, we just update the torques
right_arm._update_torque()
left_arm._update_torque()
#if we have extra time, sleep
_loop_rate.sleep()
class TestArmEncoders (wpilib.IterativeRobot):
def robotInit(self):
self.Arm1 = wpilib.CANTalon(0)
self.Arm2 = wpilib.CANTalon(1)
self.Control = ArmControl(self.Arm1, self.Arm2)
self.Control.setOffset1(-math.pi)
#self.Control.moveToPosition(20, 17.5)
#self.Control.moveMotorRotation(math.pi/2, math.pi/2)
self.Joystick = wpilib.Joystick(0)
self.TargetX = math.atan2(20,17.5)
self.TargetY = math.sqrt(20**2 + 17.5**2) #mag
def autonomousPeriodic(self):
pass
def teleopInit(self):
pass
def teleopPeriodic(self):
#print(self.Arm1.getEncPosition(), " ", self.Arm2.getEncPosition())
if self.Joystick.getRawButton(4): #Up
self.TargetY += 6.0 / 50.0
print("Up!")
elif self.Joystick.getRawButton(1): #Down
self.TargetY -= 6.0 / 50.0
print("Down!")
elif self.Joystick.getRawButton(3): #Left
self.TargetX -= 1.0 / 50.0
print("Left")
elif self.Joystick.getRawButton(2): #Right
self.TargetX += 1.0 / 50.0
print("Right")
self.Control.moveTo(self.TargetY, self.TargetX)
self.Control.update()
'''
from xarm.wrapper import XArmAPI
from BodyGameRuntime import BodyGameRuntime
from ArmControl import ArmControl
from SerialCommunication import SerialCommunication
if __name__ == '__main__':
arm_left = XArmAPI('192.168.1.209')
arm_right = XArmAPI('192.168.1.151') # 连接控制器
arm_left.motion_enable(enable=True)
arm_right.motion_enable(enable=True) # 使能
arm_left.set_mode(0)
arm_right.set_mode(0) # 运动模式：位置控制模式
arm_left.set_state(state=0)
arm_right.set_state(state=0) # 运动状态：进入运动状态
s = SerialCommunication()
game = BodyGameRuntime()
arm = ArmControl(arm_left, arm_right)
while True:
a = s.ser()
if a == '88':
arm.initial()
game.run(arm)
arm.initial()
