servo.py

#!/usr/bin/env python

import rospy
import roslib
import sys
import RPi.GPIO as GPIO
import time
from std_msgs.msg import Int8

STATE = 1 # pick -- initial state
SERVO_DOWN = 2.5 + 10 * 60 / 180 # servo angles
SERVO_UP = 2.5 + 10 * 160 / 180

MOTOR1 = 20
MOTOR2 = 21

COUNT = 0
DATA_PRV = 1#"data: 0"

def callback(data):
    global STATE
    global COUNT
    global DATA_PRV
    print("\nrobot moving:")
    pub = rospy.Publisher('robot_state', Int8, queue_size = 10)
    pub.publish(STATE)
    data = str(data)
    print(data)
    print "prev: %d" % DATA_PRV
    print "Count: %d" % COUNT
    if data == "data: 0" or data == 0: # robot not moving
        print "data: 0 ckeck OK!"
        if DATA_PRV == int(data[6]):
            COUNT += 1
        if COUNT == 3:
            COUNT = 0
            # actuate servo to move down
            p.start(SERVO_DOWN)
            time.sleep(2) # sleep 2 second
            if STATE == 1: #gripper control
                # pick
                GPIO.output(MOTOR1, 1)
                GPIO.output(MOTOR2, 0)
                print("PICK")
                time.sleep(5)
                GPIO.output(MOTOR1, 0)
            else:
                # drop
                print("DROP")
                time.sleep(5)
            # actuate servo to move up
            p.start(SERVO_UP)
            time.sleep(2) # sleep 2 second
            STATE =  not STATE
    else:
        COUNT = 0
    DATA_PRV = int(data[6])
    return
def listener():
    rospy.Subscriber("stop", Int8, callback)
    print("\ntest_servo")

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    # Initialize to use BCM numbering.
    GPIO.setmode(GPIO.BCM)

    GPIO.setup(12, GPIO.OUT)
    GPIO.setup(MOTOR1, GPIO.OUT)
    GPIO.setup(MOTOR2, GPIO.OUT)
    p = GPIO.PWM(12, 50)

    print("setting initial angle")
    # Setting servo's initial angle as 90 degree
   # p.start(9)

    rospy.init_node('servo', anonymous=True)
    listener()