Differences

This shows you the differences between two versions of the page.

--- avatar_furo_source_codes [2022/06/27 01:30] – glee
+++ avatar_furo_source_codes [2022/06/30 08:58] (current) – glee
@@ Line 188: / Line 188: @@
 	print(output_y)
+def head_vel_callback(headCTR):
+	head_roll = headCTR.data
+	if head_roll==4:
+		rospy.loginfo("Head downward")
+		GPIO.output(DIG2, GPIO.HIGH)
+		p1.start(0)		# set AN2 as HIGH, M2B will turn ON
+		p2.start(30)				# set Direction for M2
+						#delay for 2 second
+	if head_roll==2:
+		rospy.loginfo("Head upward")
+		GPIO.output(DIG2, GPIO.LOW)
+		p1.start(0)
+		p2.start(30)
+	if head_roll==5:
+		rospy.loginfo("STOP")
+		p1.start(0)                          # Direction can ignore
+		p2.start(0)                          # Direction can ignore
+		                            #delay for 3 second
+def listener():
+	rospy.init_node("furo_head")
+	rospy.Subscriber("cmd_head",Float32, head_vel_callback)
+	rospy.Subscriber("target_coordinate",Point, head_motion_callback)
+	rate = rospy.Rate(200)
+	while not rospy.is_shutdown():
+		rate.sleep()
+if __name__ == '__main__':
+	listener()
 </code>
@@ Line 200: / Line 236: @@
 Directory: ''**learning_joy/src/main.cpp **''\\
 CMakeList Directory: ''**learning_joy/CMakeLists.txt **''\\
-\\
 <code cpp | CMakeLists.txt>
 cmake_minimum_required(VERSION 2.8.3)
@@ Line 253: / Line 288: @@
 target_link_libraries(furo ${catkin_LIBRARIES})
 </code>
-\\
 \\
 The ''**roboclaw.launch**'' script is to control the motor controller that is connected to the wheels of FURO.\\
@@ Line 287: / Line 321: @@
 </launch>
 </code>
 The ''**furo.py**'' from roboclaw_node directory controls the motor controller of Furo's wheels.\\
 Directory: ''**roboclaw_ros/roboclaw_node/nodes/furo.py**''
@@ Line 601: / Line 634: @@
     rospy.loginfo("Exiting")
-</code>
+</code>\\
+The ''**finalheadmotor.py**'' controls the motor controller of Furo's head.\\
+Directory: ''**head_motor/src/finalheadmotor.py **''
+<code python | finalheadmotor.py>
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+import rospy
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float32
+import RPi.GPIO as GPIO
+from time import sleep
+GPIO.setmode(GPIO.BOARD)
+GPIO.setwarnings(False)
+AN2 =33
+AN1 = 32
+DIG2 = 18
+DIG1 = 37
+GPIO.setup(AN2,GPIO.OUT) #speed of the pitch movement
+GPIO.setup(AN1, GPIO.OUT) #speed of the roll movement
+GPIO.setup(DIG2, GPIO.OUT) #direction of the pitch movement
+GPIO.setup(DIG1, GPIO.OUT) #direction of the roll movement
+sleep(1)
+p1=GPIO.PWM(AN1,100) #GPIO.PWM(pin,scale)
+p2=GPIO.PWM(AN2,100)
+__author__= "4dimentional@kau.kr"
+class callback:
+	def head_vel_callback(self,headCTR):
+		head_roll = headCTR.data
+		if head_roll==1:#1=A(Joystick)
+			rospy.loginfo("Head downward")
+			GPIO.output(DIG1, GPIO.HIGH) #downward direction
+			p1.start(60)		#speed 60 out of 100
+			p2.start(0)
+		elif head_roll==5: #5=LB(Joystick)
+			rospy.loginfo("Head upward")
+			GPIO.output(DIG1, GPIO.LOW)  #upward direction
+			p1.start(60)
+			p2.start(0)
+		elif head_roll==3: #3=Y(Joystick)
+			rospy.loginfo("Head left")
+			GPIO.output(DIG2,GPIO.HIGH) #left direction
+			p1.start(0)
+			p2.start(60)
+		elif head_roll==2: #2=B(Joystick)
+			rospy.loginfo("Head right")
+			GPIO.output(DIG2,GPIO.LOW) #right direction
+			p1.start(0)
+			p2.start(60)
+		elif head_roll==4: #LB
+			rospy.loginfo("STOP")
+			p1.start(0) #speed 0 out of 100
+			p2.start(0)
+	def __init__(self):
+		rospy.Subscriber("cmd_head",Float32, self.head_vel_callback)
+		rate = rospy.Rate(10)
+		while not rospy.is_shutdown():
+			rate.sleep()
+		print(1)
+if __name__ == '__main__':
+	rospy.init_node('head_roll',anonymous=True)
+	try:
+		wow = callback()
+	except rospy.ROSInitException:
+		pass
+</code>