def __init__(self, is_right, period = 0.20, sensor_state_manager = None, sensor_key = ''):
global RIGHT_CHANNEL
global LEFT_CHANNEL
if GPIO.getmode() != GPIO.TEGRA_SOC:
GPIO.setmode(GPIO.TEGRA_SOC) # Initialization of GPIO pin identification mode
self.period = period # Sampling time period (each measurement is taken at every <period>). Default set to 1 millisecond
self.current_time = 0.0 # Used to store current time in UTC ms from epoch (1/1/1970 00:00)
self.channel = RIGHT_CHANNEL # Left or right channel identification for left/right wheel
self.he_value = 0.0
#Measured value is instantaneous angular velocity (approximation)
self.measured_value = [0.0, 0.0, 0.0]
if is_right:
GPIO.setup(self.channel, GPIO.IN)
else:
self.channel = LEFT_CHANNEL
GPIO.setup(self.channel, GPIO.IN)
self.he_value = GPIO.input(self.channel) #Initialize hall effect sensor value
self.is_ready = True
self.sensor_key = sensor_key
self.sensor_state_manager = sensor_state_manager
def initPCA9685(self):
"""
@description configure Jetson Nano GPIO for communication with PCA9685
@return a PCA9685 Device object to communicate with PCA9685 chip
"""
GPIO.setmode(GPIO.BOARD)
mode = GPIO.getmode()
print("Jetson Nano GPIO Mode: {}".format(mode))
# discover I2C devices
i2c_devs = Device.get_i2c_bus_numbers()
print("The following /dev/i2c-* devices were found:\n{}".format(
i2c_devs))
# Create I2C device
"""
working_devs = list()
print("Looking out which /dev/i2c-* devices is connected to PCA9685")
for dev in i2c_devs:
try:
pca9685 = Device(0x40,dev)
# Set duty cycle
pca9685.set_pwm(5, 2047)
# set pwm freq
pca9685.set_pwm_frequency(1000)
print("Device {} works!".format(dev))
working_devs.append(dev)
except:
print("Device {} does not work.".format(dev))
# Select any working device, for example, the first one
print("Configuring PCA9685 connected to /dev/i2c-{} device.".format(working_devs[0]))
pca9685 = Device(0x40, working_devs[0])
"""
pca9685 = Device(
0x40, 1
) # Immediately set a working device, this assumes PCA9685 is connected to I2C channel 1
# ESC work at 50Hz
pca9685.set_pwm_frequency(50)
# Set slow speed duty cycles
self.set_channel_duty_cycle(pca9685, 0, 5.0)
self.set_channel_duty_cycle(pca9685, 1, 5.0)
self.set_channel_duty_cycle(pca9685, 2, 5.0)
self.set_channel_duty_cycle(pca9685, 3, 5.0)
# configure rest of channels to 0 duty cycle
rest = np.arange(4, 16, 1)
for channel in rest:
self.set_channel_duty_cycle(pca9685, channel, 0.0)
return pca9685
def setup(self):
""" Initialize GPIO pins and rotary encoder state """
# Set the GPIO mode if it has not been set
if not GPIO.getmode():
GPIO.setmode(GPIO.BOARD)
# These pins will need external pullups set up
GPIO.setup(self.PIN_1, GPIO.IN)
GPIO.setup(self.PIN_2, GPIO.IN)
GPIO.setup(self.BUTTON_PIN, GPIO.IN)
pin_1_state = GPIO.input(self.PIN_1)
pin_2_state = GPIO.input(self.PIN_2)
self.curr_state = (pin_1_state, pin_2_state)
self.dir = CLOCKWISE
self.count = 0
#!/usr/bin/env python
##removed from original
##import RPi.GPIO as GPIO
#addition
import Jetson.GPIO as GPIO
import os
PLD_PIN = 7
GPIO.setmode(GPIO.BOARD)
print(GPIO.getmode(), GPIO.BOARD)
GPIO.setup(PLD_PIN, GPIO.IN)
def my_callback(channel):
if GPIO.input(channel): # if port 6 == 1
print "---AC Power Loss OR Power Adapter Failure---"
else: # if port 6 != 1
print "---AC Power OK,Power Adapter OK---"
def shutdown():
os.system('shutdown -h now')
GPIO.add_event_detect(PLD_PIN, GPIO.BOTH, callback=my_callback)
print "1.Make sure your power adapter is connected"
print "2.Disconnect and connect the power adapter to test"
print "3.When power adapter disconnected, you will see: AC Power Loss or Power Adapter Failure"
# This is a simple test of the Jetson.GPIO library
# To understand more about this library, read /opt/nvidia/jetson-gpio/doc/README.txt
import sys
import Jetson.GPIO as GPIO
from periphery import PWM
import time
# set GPIO mode to BOARD
GPIO.setmode(GPIO.BOARD)
mode = GPIO.getmode()
print(mode)
# Open PWM channel 0, pin 32
pins = range(0, 41)
"""
for pin in pins:
try:
pwm = PWM(0, pin)
print("** Channel 0 pin {} works!".format(pin))
except:
print("Pin {} does not work".format(pin))
"""
pwm = PWM(0, 2)
pwm = PWM(0, 0)
#Channel 0 pin 2
# Set frequency to 1 kHz
pwm.frequency = 2e3
def main():
GPIO.setmode(GPIO.BOARD)
mode = GPIO.getmode()
GPIO.setup(led_pin, GPIO.OUT, initial=GPIO.HIGH)
def __init__(self):
RIGHT_CHANNEL = 'GPIO_PZ0'
LEFT_CHANNEL = 'LCD_BL_PW'
# Pull private parameters only for Wheel Encoder
channel = rospy.get_param('~channel')
period = rospy.get_param('~period')
# boolean for checking if the wheel encoder is for right/left wheel
is_right = channel == RIGHT_CHANNEL
# initialize GPIO bus mode
if GPIO.getmode() != GPIO.TEGRA_SOC:
GPIO.setmode(GPIO.TEGRA_SOC)
# Set publisher to publish on wheel encoder channel
topic = 'rwheel'
if channel == LEFT_CHANNEL:
topic = 'lwheel'
pub1 = rospy.Publisher(topic, Int16, queue_size=5)
pub2 = rospy.Publisher(channel, wheel_encoder_data, queue_size=5)
GPIO.setup(channel, GPIO.IN)
deltaT = 0.0
current_time = rospy.get_time()
previous_time = current_time
he_input = GPIO.input(channel)
tick_count = 0
total_tick_count = 0
rospy.loginfo(rospy.get_caller_id() + ' began wheel encoder ' +
channel)
while not rospy.is_shutdown():
current_time = rospy.get_time()
new_input = GPIO.input(channel)
if he_input != new_input:
he_input = new_input
tick_count += 1
total_tick_count += 1
deltaT += current_time - previous_time
if deltaT >= period:
msg1 = Int16()
msg1.data = total_tick_count
pub1.publish(msg1)
msg2 = wheel_encoder_data()
msg2.is_right = is_right
msg2.total_tick_count = total_tick_count
msg2.tick_count = tick_count
msg2.delta_t = deltaT
msg2.stamp.secs = current_time
msg2.is_ready = True
pub2.publish(msg2)
deltaT = 0
tick_count = 0
previous_time = current_time
GPIO.cleanup()
