gpio_test.py

#!/usr/bin/env python
import rospy
import roslib
import sys
import RPi.GPIO as GPIO
roslib.load_manifest('camera')
from camera.msg import ball
from std_msgs.msg import Int8

# DIFF_DRIVE CONTROL

EN_LEFT = 18   # EN_LEFT   : BCM18 : 12
IN1_LEFT = 14  # IN1_LEFT  : BCM14 : 8
IN2_LEFT = 15  # IN2_LEFT  : BCM15 : 10

EN_RIGHT = 13  # EN_RIGHT  : BCM13 : 33
IN1_RIGHT = 5  # IN1_RIGHT : BCM5  : 29
IN2_RIGHT = 6  # IN2_RIGHT : BCM6  : 31

MOVE = 1 #moving -- initial state

def applyVelocity(LEFT, RIGHT):
  GPIO.output(IN1_LEFT, 1)
  pwm_LEFT.start(LEFT)

  GPIO.output(IN2_RIGHT, 1)
  pwm_RIGHT.start(RIGHT)

  print("\nvl:%f\tvr:%f" %(LEFT, RIGHT))

def move(data):
  #print "got STOP packet"
  global MOVE
  #print data
  MOVE = str(data)
  #MOVE = int(MOVE)
  #print(type(MOVE))
  #print("\nmove " + MOVE)
  #print MOVE
  #callback(50)

def callback(data):
  global MOVE
  #print "callback MOVE value:"
  #print MOVE
  if MOVE == 1 or MOVE == "data: 1":
      # change reference to (320, 240)
      data.x -= 320
      data.y -= 240
      #print("\nX:%d\tY:%d\tRadius:%d" % (data.x, data.y, data.radius))

      x = data.x
      vc = 30
      x = x * 30 / 320 # x in range(-50, 50)
      vl = vc + x
      vr = vc - x

      # print("\nX:%f\tvl:%f\tvr:%f" %(x, vl, vr))
      applyVelocity(vl, vr)

  elif MOVE == 0 or MOVE == "data: 0":
      #print "\napplying 0 velocity"
      applyVelocity(0, 0)
  else:
      print "WTF Happening!?"
      applyVelocity(70, 5)

def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("ball_pos", ball, callback)
    rospy.Subscriber("stop", Int8, move)
    print("\nokay")
    # 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)

    # We want pin 18 to be an output pin, and it should start out with a low value.
    GPIO.setup(IN1_LEFT, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(IN2_LEFT, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(EN_LEFT, GPIO.OUT, initial=GPIO.LOW)
    pwm_LEFT = GPIO.PWM(EN_LEFT, 50) # frequency

    GPIO.setup(IN1_RIGHT, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(IN2_RIGHT, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(EN_RIGHT, GPIO.OUT, initial=GPIO.LOW)
    pwm_RIGHT = GPIO.PWM(EN_RIGHT, 50) # frequency

    listener()