SHARE
TWEET

Untitled

a guest Dec 8th, 2019 67 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2. # Revision $Id$
  3.  
  4. ## Simple talker demo that listens to std_msgs/Strings published
  5. ## to the 'chatter' topic
  6.  
  7. import rospy
  8. from std_msgs.msg import String
  9. from geometry_msgs.msg import Twist
  10. from time import sleep
  11. import RPi.GPIO as GPIO
  12. r = 0.01
  13. velocity = 75
  14. en = 25
  15. temp1=1
  16. class Motor():
  17.     def __init__(self, p1, p2):
  18.         self.p1 = p1
  19.         self.p2 = p2
  20.         gpio.setup(p1, gpio.OUT)
  21.         gpio.setup(p2, gpio.OUT)
  22.     #x != 0 z = 0
  23.     def runlinear(self, data):
  24.         #Negatieve snelheid
  25.         if data.linear.x < 0:
  26.             GPIO.output(self.p1, False)
  27.             GPIO.output(self.p2, True)
  28.         #positieve snelheid
  29.         elif data.linear.x > 0:
  30.             GPIO.output(self.p1, True)
  31.             GPIO.output(self.p2, False)
  32.         #de x is nul => traag afbouwen
  33.         else:      
  34.             p.ChangeDutyCycle(65)
  35.             sleep(0.5) 
  36.             p.ChangeDutyCycle(50)
  37.             sleep(0.5) 
  38.             p.ChangeDutyCycle(25)
  39.             sleep(0.5)
  40.             GPIO.output(self.p1, False)
  41.             GPIO.output(self.p2, False)
  42.     #x = 0 z !=0
  43.     def turn(self, data):
  44.         #data is de waarde van angular Z en is dus negatief bij een van de twee motoren
  45.         #Als de juiste twist waarden worden doorgegeven wordt bepaald in welke richting gedraait moet worden en dit wordt dan doorgegeven
  46.         #Bv -2 0 0 als angular waarden wordt aan 1 motor -2 gegeven en aan de andere 2
  47.         vel = data.angular.z * r
  48.        
  49.         if vel > 0:
  50.             GPIO.output(self.p1, True)
  51.             GPIO.output(self.p2, False)
  52.         else:
  53.             #achteruit rijden
  54.             GPIO.output(self.p1, False)
  55.             GPIO.output(self.p2, True)
  56.             vel *= -1
  57.             #vel moet wel positief zijn
  58.         p.ChangeDutyCycle(vel)
  59.     #x =! 0 z !=0 => wiel langs een kant vertragen
  60.     def linearturn(self, data):
  61.        
  62.         if data.angular.z > 0:
  63.             GPIO.output(self.p1, True)
  64.             GPIO.output(self.p2, False)
  65.             vel = 75 - data.angular.z * r
  66.         else:
  67.             GPIO.output(self.p1, True)
  68.             GPIO.output(self.p2, False)
  69.             #velocity moet > zijn als de andere velocity op het andere wiel, + of - mag enkel bepalen welk wiel vertraagd wordt, niet ervoor zorgen dat
  70.             posvel = data.angular.z * -1
  71.             vel = 75 - posvel * r
  72.             vel *= -1
  73.         p.ChangeDutyCycle(vel)
  74.  
  75.            
  76. def conf():
  77.     r = 0.01
  78.     velocity = 95
  79.     en = 25
  80.     temp1=1
  81.     GPIO.setmode(GPIO.BCM)
  82.     GPIO.setup(en, GPIO.OUT)
  83.     p=GPIO.PWM(en,1000)
  84.     p.start(velocity)
  85.  
  86. def callback(data):
  87.     rospy.loginfo(data.linear.x)
  88.     if data.linear.x != 0:
  89.         if data.angular.z == 0:        
  90.                 motor_left.runlinear(data)
  91.                 motor_right.runlinear(data)
  92.         else:
  93.         if data.angular.z > 0:
  94.                 motor_left.runlinear(data)
  95.                 motor_right.linearturn(data)
  96.     else:
  97.         if data.angular.z == 0:
  98.             motor_left.turn(data)
  99.             motor_right.turn(data)
  100. def listener():
  101.  
  102.     rospy.init_node('listener', anonymous=True)
  103.  
  104.     rospy.Subscriber('chatter', Twist, callback)
  105.  
  106.     # spin() simply keeps python from exiting until this node is stopped
  107.     rospy.spin()
  108.  
  109. if __name__ == '__main__':
  110.     try:
  111.         motor_left = Motor(17,22)
  112.         motor_right = Motor(23,24)
  113.         conf()
  114.         listener()
  115.     except keyboardinterrupt:
  116.         rospy.loginfo("exit application")
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top