def __init__(self, s_pin = "P8_13", f_pin = "P9_14", b_pin = "P9_22", debug = False): # initialize servo for steering self.steer = Servo(s_pin) # save PWM pins for forward and reverse self.forward = f_pin self.reverse = b_pin # start servos and motors self.start() # center the steering center_steering()
def start(): # Starting servo controller global servo servo = Servo() # using default values servo.start() # Start motor control global forward global reverse global PWM forward = "P9_14" reverse = "P9_22" PWM.start(forward, 0, 200) PWM.start(reverse, 0, 200) # initializing the servo init_servo() # Subscribe to ROS Joy node rospy.Subscriber("joy", Joy, callback) rospy.init_node("Joy2Servo") rospy.spin()
import Adafruit_BBIO.GPIO as GPIO import time import math from servo_controller import Servo # most of the time.sleep calls work to give the servo # time to respond (not instantaneous) # setting up the limit switch pins pin1 = "P8_12" pin2 = "P8_14" GPIO.setup(pin1, GPIO.IN) GPIO.setup(pin2, GPIO.IN) # starting the servo servo = Servo() servo.start() # guessed center point center = 100 # current angle angle = center # left and right limits limit1 = angle limit2 = angle # turning left while not GPIO.input(pin1) and not GPIO.input(pin2): angle += .01 servo.set_angle(angle) time.sleep(.0001)
class Car: def __init__(self, s_pin = "P8_13", f_pin = "P9_14", b_pin = "P9_22", debug = False): # initialize servo for steering self.steer = Servo(s_pin) # save PWM pins for forward and reverse self.forward = f_pin self.reverse = b_pin # start servos and motors self.start() # center the steering center_steering() # set the "steering wheel" of the car # negative is left # positive is right def turn(self, angle = 0): self.steer.set_angle(self.center + angle) if debug: print angle return # set the car speed def set_speed(self, speed): speed = clamp(speed, -100, 100) if speed > 0: PWM.set_duty_cycle(reverse, 0) PWM.set_duty_cycle(forward, speed) else: PWM.set_duty_cycle(forward, 0) PWM.set_duty_cycle(reverse, -speed) return # start PWM lines for forward and reverse and start steering servo def start(self): PWM.start(self.forward, 0, 200) PWM.start(self.reverse, 0, 200) self.steer.start() return # stop PWM lines for forward and reverse and stop steering servo def stop(self): PWM.stop(self.forward) PWM.stop(self.reverse) self.steer.stop() PWM.cleanup() return # center the steering servo between two limit switches def center_steering(): pin1 = "P8_12" pin2 = "P8_14" GPIO.setup(pin1, GPIO.IN) GPIO.setup(pin2, GPIO.IN) # guessed center point center = 100 # current angle angle = center # left and right limits limit1 = angle limit2 = angle # turning left while not GPIO.input(pin1) and not GPIO.input(pin2): angle += .5 self.steer.set_angle(angle) time.sleep(.05) limit1 = angle time.sleep(.5) # resetting servo to guessed center angle = center servo.set_angle(angle) time.sleep(.2) # turning right while not GPIO.input(pin1) and not GPIO.input(pin2): angle -= .5 self.steer.set_angle(angle) time.sleep(.05) limit2 = angle time.sleep(.5) # calculating center from left and right limits self.center = (limit1 + limit2) / 2 # calculate total range (102% of limit differences) self.span = (limit1 - limit2) / 2 * 1.02 # allow for a little extra wiggle self.steer.set_limits(limit2, limit1) if debug: print "center = " + str(center) print "span = " + str(span) servo.set_angle(center) return # clamp x value between specified min and max values def clamp(x, min, max): if min > max: return False if x > max: return max if x < min: return min return x