def __init__(self, pwm_name, dir_a, dir_b, eqep): # Initialize the thread base super(controller, self).__init__() # Create stop event object self.stop_event = threading.Event() self.stop_confirm = threading.Event() # Store parameters self.pwm = pwm_name self.dir_a = dir_a self.dir_b = dir_b # Create a PID controller self.pid = PID(0.01, 0.01, 0.0, -100.0, 100.0, 0.0) # Load the eQEP interface self.eqep = eQEP(eqep, eQEP.MODE_RELATIVE) self.eqep.set_period(10000000) # Setup the GPIO gpio.setup(self.dir_a, gpio.OUT) gpio.setup(self.dir_b, gpio.OUT) gpio.output(self.dir_a, gpio.LOW) gpio.output(self.dir_b, gpio.LOW) # Setup the PWM pwm.start(self.pwm, 100.0)
def __init__(self): self.baud=38400 self.addr=128 self.PWM_Freq=15000 self.PWM_RR="P9_14" self.PWM_RF="P9_22" self.PWM_LR="P8_13" self.PWM_LF="P9_42" self.ticks = 10000 self.wheelDiameter = 0.3 self.RedLED = "P8_7" self.GreenLED = "P8_9" self.BlueLED = "P8_11" self.rad_to_deg = 57.29578 self.Angle_offset = 3 self.SaberTooth_KpP = 250#250 self.SaberTooth_KiP = 1#1 self.SaberTooth_KdP = 20#20 self.SaberTooth_KpA = 16#16 self.SaberTooth_KiA = 1.7#1.7 self.SaberTooth_KdA = -2#-2 self.PWM_KpP = 220 self.PWM_KiP = 2 self.PWM_KdP = 8 self.PWM_KpA = 6 self.PWM_KiA = 1.2 self.PWM_KdA = -0.2 self.limitP = 310 self.limitA = 310 self.integrated_error_P1 = 0 self.integrated_error_P2 = 0 self.integrated_error_A1 = 0 self.integrated_error_A2 = 0 self.last_error_P1 = 0 self.last_error_P2 = 0 self.last_error_A1 = 0 self.last_error_A2 = 0 self.mLiPo = 25.2951 self.MinRedLiPo = 20 self.AnalogPinLiPo = "P9_40" self.ajustFR = 0.0085 self.ajustLR = 0.003 self.encoder1 = eqep.eQEP("/sys/devices/ocp.3/48302000.epwmss/48302180.eqep", eqep.eQEP.MODE_ABSOLUTE) self.encoder2 = eqep.eQEP("/sys/devices/ocp.3/48304000.epwmss/48304180.eqep", eqep.eQEP.MODE_ABSOLUTE)
# 6: ff:P-O-- Bone-Black-HDMIN,00A0,Texas Instrument,BB-BONELT-HDMIN # 7: ff:P-O-L Override Board Name,00A0,Override Manuf,BB-ADC # 9: ff:P-O-L Override Board Name,00A0,Override Manuf,am33xx_pwm # 10: ff:P-O-L Override Board Name,00A0,Override Manuf,bone_eqep1 # 14: ff:P-O-L Override Board Name,00A0,Override Manuf,bone_pwm_P8_34 # root@beaglebone:/home/debian/BB_24V_Motor# import pdb import time import Adafruit_BBIO.PWM as PWM import Adafruit_BBIO.GPIO as GPIO from eqep import eQEP # Note: find /sys/devices/ -iname "*qep*" to find which eqep you're using enc = eQEP("/sys/devices/ocp.3/48302000.epwmss/48302180.eqep", eQEP.MODE_ABSOLUTE) enc.set_period(100000000) # nsec (0.1 sec) GPIO.setup(MY_PWM_DIR_PIN, GPIO.OUT) GPIO.setup(MY_PWM_STBY, GPIO.OUT) def stby(): GPIO.output(MY_PWM_STBY, GPIO.HIGH) def go(): GPIO.output(MY_PWM_STBY, GPIO.LOW) def cw():
from eqep import eQEP import Adafruit_BBIO.GPIO as GPIO import Adafruit_BBIO.PWM as PWM import time import sys dir_A="P9_15" dir_B="P9_23" pwm_A="P9_14" pwm_B="P9_16" nperiod = 5000000 period = nperiod / 1e9 eQEP2="/sys/devices/ocp.3/48304000.epwmss/48304180.eqep" #eQEP2A_in(P8_12) eqep2=eQEP(eQEP2,eQEP.MODE_ABSOLUTE) #eQEP2B_in(P8_11) eqep2.set_period(nperiod) PWM.start(pwm_A) GPIO.setup(dir_A,GPIO.OUT) GPIO.setup(dir_B,GPIO.OUT) encoder=eqep2.poll_position print ("RIGHT") with open('data.csv', 'w') as file: GPIO.output(dir_A,1) GPIO.output(dir_B,0) PWM.set_duty_cycle(pwm_A,100)
#!/usr/bin/python # Python Standard Library Imports import eqep import math import ProBotConstants # Initialization of classes from local files Pconst = ProBotConstants.Constants() # Instantiate an instance of the driver for encoder of the motor 1 encoder1 = eqep.eQEP("/sys/devices/ocp.3/48302000.epwmss/48302180.eqep", eqep.eQEP.MODE_ABSOLUTE) # Instantiate an instance of the driver for encoder of the motor 2 encoder2 = eqep.eQEP("/sys/devices/ocp.3/48304000.epwmss/48304180.eqep", eqep.eQEP.MODE_ABSOLUTE) class EncodersReadings(): def __init__(self, LastwheelPosition1=0, LastwheelPosition2=0): self.LastwheelPosition1 = LastwheelPosition1 self.LastwheelPosition2 = LastwheelPosition2 def EncodersValues(self): wheelPosition1 = encoder1.get_position( ) # Get position from the first encoder wheelPosition2 = encoder2.get_position( ) # Get position from the second encoder wheelPosition1_m = (
from std_msgs.msg import Float32 # # this is probably fine for encoders for now but there are some # inherent granularity issues with using the eQEPs for velocity # control; probably want to switch to PRUs at some point # # ros stuff rospy.init_node('publish_eqep') pub_left = rospy.Publisher('/wheel_left/state', Float32, queue_size=1) pub_right = rospy.Publisher('/wheel_right/state', Float32, queue_size=1) # initialiaze eQEPs eqep_left = eQEP(eQEP.eQEP2, eQEP.MODE_RELATIVE) eqep_right = eQEP(eQEP.eQEP0, eQEP.MODE_RELATIVE) # set update frequency frequency = rospy.get_param('frequency', 30) period = 1000000000 / frequency eqep_left.set_period(period) eqep_right.set_period(period) # other params ticks_per_rev = rospy.get_param('ticks_per_rev', 360) rospy.loginfo("Starting publish for encoder data...") while not rospy.is_shutdown(): # get raw tick count
import Adafruit_BBIO.GPIO as GPIO from time import sleep import Image import ImageDraw from Adafruit_LED_Backpack import BicolorMatrix8x8 from eqep import eQEP display = BicolorMatrix8x8.BicolorMatrix8x8() eQEP1 = eQEP("/sys/devices/ocp.3/48302000.epwmss/48302180.eqep", eQEP.MODE_ABSOLUTE) eQEP2 = eQEP("/sys/devices/ocp.3/48304000.epwmss/48304180.eqep", eQEP.MODE_ABSOLUTE) eQEP1.set_period(100000000) eQEP2.set_period(100000000) display.begin() display.clear() display.write_display() BUTTON_1 = "P9_12" BUTTON_2 = "P9_25" BUTTON_3 = "P9_29" BUTTON_4 = "P9_41" BUTTON_CLR = "P8_10" LED_1 = "P9_14" LED_2 = "P9_26" LED_3 = "P9_30" LED_4 = "P9_42" GPIO.setup(BUTTON_1, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
#!/usr/bin/python # Python Standard Library Imports import eqep import math import ProBotConstants # Initialization of classes from local files Pconst = ProBotConstants.Constants() # Instantiate an instance of the driver for encoder of the motor 1 encoder1 = eqep.eQEP("/sys/devices/ocp.3/48302000.epwmss/48302180.eqep", eqep.eQEP.MODE_ABSOLUTE) # Instantiate an instance of the driver for encoder of the motor 2 encoder2 = eqep.eQEP("/sys/devices/ocp.3/48304000.epwmss/48304180.eqep", eqep.eQEP.MODE_ABSOLUTE) class EncodersReadings(): def __init__(self, LastwheelPosition1=0, LastwheelPosition2=0): self.LastwheelPosition1 = LastwheelPosition1 self.LastwheelPosition2 = LastwheelPosition2 def EncodersValues(self): wheelPosition1 = encoder1.get_position() # Get position from the first encoder wheelPosition2 = encoder2.get_position() # Get position from the second encoder wheelPosition1_m = (float(wheelPosition1)) / Pconst.ticks * math.pi * Pconst.wheelDiameter # Calculate the travelled distance for first encoder wheelPosition2_m = (float(wheelPosition2)) / Pconst.ticks * math.pi * Pconst.wheelDiameter # Calculated the travelled distance for second encoder wheelVelocity1 = float(wheelPosition1_m - self.LastwheelPosition1) # Wheel velocity wheelVelocity2 = float(wheelPosition2_m - self.LastwheelPosition2)
# this is probably fine for encoders for now but there are some # inherent granularity issues with using the eQEPs for velocity # control; probably want to switch to PRUs at some point # # overlay to device tree Overlay.apply("bone_eqep0") Overlay.apply("bone_eqep2b") # ros stuff rospy.init_node('odom_publisher') pub_left = rospy.Publisher('/wheel_left/state', Float32, queue_size=1) pub_right = rospy.Publisher('/wheel_right/state', Float32, queue_size=1) pub_odom = rospy.Publisher('/wheel_odom', Odometry, queue_size=3) # initialiaze eQEPs eqep_left = eQEP(eQEP.eQEP2, eQEP.MODE_ABSOLUTE) eqep_right = eQEP(eQEP.eQEP0, eQEP.MODE_ABSOLUTE) # set update frequency frequency = rospy.get_param('frequency', 30) period = 1000000000 / frequency eqep_left.set_period(period) eqep_right.set_period(period) # other params ticks_per_rev = rospy.get_param('ticks_per_rev', 360) wheel_track = rospy.get_param('wheel_track', 0.106) wheel_diameter = rospy.get_param('wheel_diameter', 0.04) theta = 0 x = 0
#!/usr/bin/python # Import the eQEP driver from eqep import eQEP # Instantiate an instance of the driver encoder1 = eQEP("/sys/devices/ocp.2/48302000.epwmss/48302180.eqep", eQEP.MODE_ABSOLUTE) # Set the polling period of the encoder to 0.1 seconds, or 100,000,000 nanoseconds encoder1.set_period(100000000) # Poll the position indefinitely. Program will provide a position at 10 Hz while True: print encoder1.poll_position()