Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #!/usr/bin/env python
- # Revision $Id$
- ## Simple talker demo that listens to std_msgs/Strings published
- ## to the 'chatter' topic
- import rospy
- from std_msgs.msg import String
- from geometry_msgs.msg import Twist
- from time import sleep
- import RPi.GPIO as GPIO
- r = 0.01
- velocity = 75
- en = 25
- temp1=1
- class Motor():
- def __init__(self, p1, p2):
- self.p1 = p1
- self.p2 = p2
- gpio.setup(p1, gpio.OUT)
- gpio.setup(p2, gpio.OUT)
- #x != 0 z = 0
- def runlinear(self, data):
- #Negatieve snelheid
- if data.linear.x < 0:
- GPIO.output(self.p1, False)
- GPIO.output(self.p2, True)
- #positieve snelheid
- elif data.linear.x > 0:
- GPIO.output(self.p1, True)
- GPIO.output(self.p2, False)
- #de x is nul => traag afbouwen
- else:
- p.ChangeDutyCycle(65)
- sleep(0.5)
- p.ChangeDutyCycle(50)
- sleep(0.5)
- p.ChangeDutyCycle(25)
- sleep(0.5)
- GPIO.output(self.p1, False)
- GPIO.output(self.p2, False)
- #x = 0 z !=0
- def turn(self, data):
- #data is de waarde van angular Z en is dus negatief bij een van de twee motoren
- #Als de juiste twist waarden worden doorgegeven wordt bepaald in welke richting gedraait moet worden en dit wordt dan doorgegeven
- #Bv -2 0 0 als angular waarden wordt aan 1 motor -2 gegeven en aan de andere 2
- vel = data.angular.z * r
- if vel > 0:
- GPIO.output(self.p1, True)
- GPIO.output(self.p2, False)
- else:
- #achteruit rijden
- GPIO.output(self.p1, False)
- GPIO.output(self.p2, True)
- vel *= -1
- #vel moet wel positief zijn
- p.ChangeDutyCycle(vel)
- #x =! 0 z !=0 => wiel langs een kant vertragen
- def linearturn(self, data):
- if data.angular.z > 0:
- GPIO.output(self.p1, True)
- GPIO.output(self.p2, False)
- vel = 75 - data.angular.z * r
- else:
- GPIO.output(self.p1, True)
- GPIO.output(self.p2, False)
- #velocity moet > zijn als de andere velocity op het andere wiel, + of - mag enkel bepalen welk wiel vertraagd wordt, niet ervoor zorgen dat
- posvel = data.angular.z * -1
- vel = 75 - posvel * r
- vel *= -1
- p.ChangeDutyCycle(vel)
- def conf():
- r = 0.01
- velocity = 95
- en = 25
- temp1=1
- GPIO.setmode(GPIO.BCM)
- GPIO.setup(en, GPIO.OUT)
- p=GPIO.PWM(en,1000)
- p.start(velocity)
- def callback(data):
- rospy.loginfo(data.linear.x)
- if data.linear.x != 0:
- if data.angular.z == 0:
- motor_left.runlinear(data)
- motor_right.runlinear(data)
- else:
- if data.angular.z > 0:
- motor_left.runlinear(data)
- motor_right.linearturn(data)
- else:
- if data.angular.z == 0:
- motor_left.turn(data)
- motor_right.turn(data)
- def listener():
- rospy.init_node('listener', anonymous=True)
- rospy.Subscriber('chatter', Twist, callback)
- # spin() simply keeps python from exiting until this node is stopped
- rospy.spin()
- if __name__ == '__main__':
- try:
- motor_left = Motor(17,22)
- motor_right = Motor(23,24)
- conf()
- listener()
- except keyboardinterrupt:
- rospy.loginfo("exit application")
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement