#!/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