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()
