Skip to content

Commit

Permalink
formatting and flow improvements
Browse files Browse the repository at this point in the history
  • Loading branch information
Darren W committed Jan 4, 2025
1 parent c671afc commit 3b784d6
Show file tree
Hide file tree
Showing 41 changed files with 1,115 additions and 1,002 deletions.
62 changes: 29 additions & 33 deletions src/arm_interface/arm_interface/joystickArmController.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,28 +8,32 @@
from std_msgs.msg import Int8, Float32, Bool
from ros_phoenix.msg import MotorControl, MotorStatus
from math import pi

# import Jetson.GPIO as GPIO
# import interfaces.msg as GPIOmsg


def map_range(value, old_min, old_max, new_min, new_max):
old_range = old_max - old_min
new_range = new_max - new_min
scaled_value = (value - old_min) / old_range
mapped_value = new_min + scaled_value * new_range
return mapped_value


def joystick_to_motor_control(vertical, horizontal):
vertical = max(min(vertical, 1.0), -1.0)
horizontal = max(min(horizontal, 1.0), -1.0)

left_motor = vertical + horizontal
right_motor = vertical - horizontal

left_motor = max(min(left_motor, 1.0), -1.0)
right_motor = max(min(right_motor, 1.0), -1.0)

return -left_motor, -right_motor


class joystickArmController(Node):
def __init__(self):
super().__init__("joystickControl")
Expand All @@ -48,12 +52,10 @@ def __init__(self):
# output_pin = output_pins.get(GPIO.model, None)
# if output_pin is None:
# raise Exception('PWM not supported on this board')


# GPIO.setup(output_pin, GPIO.OUT, initial=GPIO.HIGH)
# self.gripper = GPIO.PWM(output_pin, 50)


self.base = MotorControl()
self.diff1 = MotorControl()
self.diff2 = MotorControl()
Expand All @@ -68,29 +70,22 @@ def __init__(self):
# self.gripper.start(self.gripperVal)
# self.gripper.ChangeDutyCycle(self.gripperVal)

self.baseCommand = self.create_publisher(
MotorControl, "/base/set", 1)
self.diff1Command = self.create_publisher(
MotorControl, "/diff1/set", 1)
self.diff2Command = self.create_publisher(
MotorControl, "/diff2/set", 1)
self.elbowCommand = self.create_publisher(
MotorControl, "/elbow/set", 1)
self.wristTiltCommand = self.create_publisher(
MotorControl, "/wristTilt/set", 1)
self.wristTurnCommand = self.create_publisher(
MotorControl, "/wristTurn/set", 1)

self.baseCommand = self.create_publisher(MotorControl, "/base/set", 1)
self.diff1Command = self.create_publisher(MotorControl, "/diff1/set", 1)
self.diff2Command = self.create_publisher(MotorControl, "/diff2/set", 1)
self.elbowCommand = self.create_publisher(MotorControl, "/elbow/set", 1)
self.wristTiltCommand = self.create_publisher(MotorControl, "/wristTilt/set", 1)
self.wristTurnCommand = self.create_publisher(MotorControl, "/wristTurn/set", 1)

self.joystick = self.create_subscription(
Joy, "/joystick/arm", self.joy_callback, 5)

Joy, "/joystick/arm", self.joy_callback, 5
)

freq = 10
self.rate = self.create_rate(freq)
period = 1 / freq
self.timer = self.create_timer(period, self.controlPublisher)



def controlPublisher(self):
# if(Node.get_clock(self).now().seconds_nanoseconds()[0] - self.lastTimestamp > 2 or self.estop.data == True):
# return
Expand All @@ -112,35 +107,35 @@ def joy_callback(self, msg: Joy):
self.wristTurn.mode = 0
# self.get_logger().info("bruh")

if(msg.buttons[4]):#RIGHT BUMPER IDK THE VALUE
if msg.buttons[4]: # RIGHT BUMPER IDK THE VALUE
self.base.value = 0.5
elif(msg.buttons[5]): #LEFT BUMPER
elif msg.buttons[5]: # LEFT BUMPER
self.base.value = -0.5
else:
self.base.value = 0.0

if(msg.axes[2] < 1):#RIGHT TRIGGER IDK THE VALUE
if msg.axes[2] < 1: # RIGHT TRIGGER IDK THE VALUE
self.wristTurn.value = 1.0
elif(msg.axes[5] < 1): #LEFT TRIGGER
elif msg.axes[5] < 1: # LEFT TRIGGER
self.wristTurn.value = -1.0
else:
self.wristTurn.value = 0.0
if(msg.buttons[1]):#A IDK THE VALUE

if msg.buttons[1]: # A IDK THE VALUE
self.wristTilt.value = 1.0
elif(msg.buttons[0]): #B
elif msg.buttons[0]: # B
self.wristTilt.value = -1.0
else:
self.wristTilt.value = 0.0
self.elbow.value = msg.axes[4] #LEFT VERTICAL
self.elbow.value = msg.axes[4] # LEFT VERTICAL
diff1, diff2 = joystick_to_motor_control(msg.axes[0], msg.axes[1])
# self.get_logger().info(f'diff1: {self.diff1.value}, diff2: {self.diff2.value}')
self.diff1.value = float(diff1)
self.diff2.value = float(diff2)
if(msg.buttons[9]):
if msg.buttons[9]:
self.estop.data = True
self.estopTimestamp = msg.header.stamp.sec
if(msg.buttons[8] and msg.header.stamp.sec - self.estopTimestamp > 2):
if msg.buttons[8] and msg.header.stamp.sec - self.estopTimestamp > 2:
self.estop.data = False
# if(msg.buttons[3] and self.gripperVal <= 70):
# self.gripperVal = self.gripperVal + self.gripperInc
Expand All @@ -159,6 +154,7 @@ def main(args=None):
# GPIO.cleanup()
node.destroy_node()
rclpy.shutdown()



if __name__ == "__main__":
main()
21 changes: 10 additions & 11 deletions src/arm_interface/arm_interface/joystickScienceController.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
from std_msgs.msg import Int8, Float32, Bool
from ros_phoenix.msg import MotorControl, MotorStatus


class joystickScienceController(Node):
def __init__(self):
super().__init__("joystickControl")
Expand All @@ -16,21 +17,18 @@ def __init__(self):
self.estop = Bool()
self.lastTimestamp = 0

self.baseCommand = self.create_publisher(
MotorControl, "/scienceBase/set", 1)
self.diggerCommand = self.create_publisher(
MotorControl, "/digger/set", 1)

self.baseCommand = self.create_publisher(MotorControl, "/scienceBase/set", 1)
self.diggerCommand = self.create_publisher(MotorControl, "/digger/set", 1)

self.joystick = self.create_subscription(
Joy, "/joystick/arm", self.joy_callback, 5)

Joy, "/joystick/arm", self.joy_callback, 5
)

freq = 10
self.rate = self.create_rate(freq)
period = 1 / freq
self.timer = self.create_timer(period, self.controlPublisher)



def controlPublisher(self):
# if(Node.get_clock(self).now().seconds_nanoseconds()[0] - self.lastTimestamp > 2 or self.estop.data == True):
# return
Expand All @@ -41,7 +39,7 @@ def joy_callback(self, msg: Joy):
self.lastTimestamp = msg.header.stamp.sec
self.scienceBase.mode = 0
self.digger.mode = 0
self.get_logger().info(f'base: {msg.axes[3]}, digger: { msg.axes[1]}')
self.get_logger().info(f"base: {msg.axes[3]}, digger: { msg.axes[1]}")
self.digger.value = msg.axes[1]
self.scienceBase.value = msg.axes[3] / 2

Expand All @@ -52,6 +50,7 @@ def main(args=None):
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()



if __name__ == "__main__":
main()
87 changes: 53 additions & 34 deletions src/arm_interface/arm_interface/trajectoryInterpreter.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
from ros_phoenix.msg import MotorControl, MotorStatus
from math import pi


def map_range(value, old_min, old_max, new_min, new_max):
old_range = old_max - old_min
new_range = new_max - new_min
Expand All @@ -25,7 +26,6 @@ def __init__(self):
self.expectedTraj = JointTrajectoryPoint()
self.motorCommand = MotorControl()


self.baseAngle = 0.0
self.baseZeroPoint = 0.0
self.diff1Cont = 0.0
Expand All @@ -41,103 +41,122 @@ def __init__(self):
self.wristTiltAngle = 0.0
self.wristTiltZeroPoint = 0.0


self.estop = Bool()
self.lastTimestamp = 0

self.srv = self.create_service(ArmPos, 'arm_pos', self.get_arm_pos_callback)
self.srv = self.create_service(ArmPos, "arm_pos", self.get_arm_pos_callback)

self.setEstop = self.create_publisher(Bool, "/drive/estop", 1)

self.anglePub = self.create_publisher(SixFloats, "/arm/Angle", 1)

self.setEstop = self.create_publisher(
Bool, "/drive/estop", 1)

self.anglePub = self.create_publisher(
SixFloats, "/arm/Angle" , 1)

self.traj_sub = self.create_subscription(
JointTrajectoryPoint, "/arm/expectedTraj", self.cmd_traj_callback, 10)

JointTrajectoryPoint, "/arm/expectedTraj", self.cmd_traj_callback, 10
)

self.base_motor = self.create_subscription(
MotorStatus, "/base/status", self.base_callback, 5)
MotorStatus, "/base/status", self.base_callback, 5
)
self.diff_motor1 = self.create_subscription(
MotorStatus, "/diff1/status", self.diff1_callback, 5)
MotorStatus, "/diff1/status", self.diff1_callback, 5
)
self.diff_motor2 = self.create_subscription(
MotorStatus, "/diff2/status", self.diff2_callback, 5)
MotorStatus, "/diff2/status", self.diff2_callback, 5
)
self.elbow_motor = self.create_subscription(
MotorStatus, "/elbow/status", self.elbow_callback, 5)
MotorStatus, "/elbow/status", self.elbow_callback, 5
)
self.wrist_turn_motor = self.create_subscription(
MotorStatus, "/wristTurn/status", self.wrist_turn_callback, 5)
MotorStatus, "/wristTurn/status", self.wrist_turn_callback, 5
)
self.wrist_tilt_motor = self.create_subscription(
MotorStatus, "/wristTilt/status", self.wrist_tilt_callback, 5)

self.setEstop.publish(self.estop) #init as not estoped
MotorStatus, "/wristTilt/status", self.wrist_tilt_callback, 5
)

self.setEstop.publish(self.estop) # init as not estoped
freq = 10
self.rate = self.create_rate(freq)
period = 1 / freq
self.timer = self.create_timer(period, self.anglepublisher)

def base_callback(self, msg: MotorStatus):
self.baseAngle = 0.0

def diff1_callback(self, msg: MotorStatus):
self.diff1Cont = self.diff1ZeroPoint + msg.position * ((2*pi)/1000 * 1/83 * 1/100)
self.diff1Cont = self.diff1ZeroPoint + msg.position * (
(2 * pi) / 1000 * 1 / 83 * 1 / 100
)
self.diff1Angle = (self.diff1Cont - self.diff2Cont) * 4
print("bruh1" + str(self.diff1Angle))

def diff2_callback(self, msg: MotorStatus):
self.diff2Cont = self.diff2ZeroPoint + msg.position * ((2*pi)/1000 * 1/83 * 1/100)
self.diff2Cont = self.diff2ZeroPoint + msg.position * (
(2 * pi) / 1000 * 1 / 83 * 1 / 100
)
self.diff2Angle = (self.diff1Cont + self.diff2Cont) * 4
print("bruh2" + str(self.diff2Angle))

def elbow_callback(self, msg: MotorStatus):
self.elbowAngle = (self.elbowZeroPoint + msg.position * ((2*pi)/1000 * ((1/83) * (1/100) * (16/13)))) * 4
self.elbowAngle = (
self.elbowZeroPoint
+ msg.position * ((2 * pi) / 1000 * ((1 / 83) * (1 / 100) * (16 / 13)))
) * 4

def wrist_turn_callback(self, msg: MotorStatus):
self.wristTurnAngle = 0.0

def wrist_tilt_callback(self, msg: MotorStatus):
self.wristTiltAngle = 0.0

def anglepublisher(self):
out = SixFloats()
out.m0 = self.elbowAngle
out.m1 = self.diff1Angle
out.m0 = self.elbowAngle
out.m1 = self.diff1Angle
out.m2 = self.diff2Angle
out.m3 = self.elbowAngle
self.m4 = self.wristTiltAngle
self.m5 = self.wristTurnAngle
self.anglePub.publish(out)

def get_arm_pos_callback(self, request, response):
response.base = self.elbowAngle
response.diff1 = self.diff1Angle
response.base = self.elbowAngle
response.diff1 = self.diff1Angle
response.diff2 = self.diff2Angle
response.elbow = self.elbowAngle
response.wristturn = self.wristTurnAngle
response.wristtilt = self.wristTiltAngle
return response

def cmd_traj_callback(self, msg: JointTrajectoryPoint):
self.expectedTraj = msg;
self.expectedTraj = msg
self.lastTimestamp = Node.get_clock(self).now().seconds_nanoseconds()[0]

def trajToTalon(pt: JointTrajectoryPoint):
pt.positions

def baseToTraj(self, status: MotorStatus):
self.expectedTraj #math to convert status to traj point
self.expectedTraj # math to convert status to traj point

def diff1ToTraj(self, status: MotorStatus):
self.expectedTraj #math to convert status to traj point
self.expectedTraj # math to convert status to traj point

def diff2ToTraj(self, status: MotorStatus):
self.expectedTraj #math to convert status to traj point
self.expectedTraj # math to convert status to traj point

def elbowToTraj(self, status: MotorStatus):
self.expectedTraj #math to convert status to traj point
self.expectedTraj # math to convert status to traj point

def wristToTraj(self, status: MotorStatus):
self.expectedTraj #math to convert status to traj point
self.expectedTraj # math to convert status to traj point


def main(args=None):
rclpy.init(args=args)
node = trajectoryInterpreter()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()



if __name__ == "__main__":
main()
Loading

0 comments on commit 3b784d6

Please sign in to comment.