def init_i2c():
try:
i2c = i2cdev.I2C(PORT_I2C)
i2c.set_timeout(I2C_TIMEOUT)
except Exception as e:
print (e)
deinit_i2c(i2c)
return None
return i2c
# ADS1115 single-shot mode import i2cdev from time import sleep ADS1115 = i2cdev.I2C(0x48,0) # Address = 0x48, I2C bus = 0 def ADS1115_read(): sleep(0.001) ADS1115.write(bytes([int(1),int(197),int(131)])) # write config =1 & chanel 0 + FSE 2v + single shot & 128hz + b00011 sleep(0.008) # wait for conversion ADS1115.write(bytes([int(0)])) # change the pointer to measurment buffer sleep(0.001) c0 = int.from_bytes(ADS1115.read(2), byteorder='big') sleep(0.001) ADS1115.write(bytes([int(1),int(213),int(131)])) # write config =1 & chanel 1 + FSE 2v + single shot & 128hz + b00011 sleep(0.008) # wait for conversion ADS1115.write(bytes([int(0)])) # change the pointer to measurment buffer sleep(0.001) c1 = int.from_bytes(ADS1115.read(2), byteorder='big') sleep(0.001) ADS1115.write(bytes([int(1),int(229),int(131)])) # write config =1 & chanel 2 + FSE 2v + single shot & 128hz + b00011 sleep(0.008) # wait for conversion ADS1115.write(bytes([int(0)])) # change the pointer to measurment buffer sleep(0.001) c2 = int.from_bytes(ADS1115.read(2), byteorder='big') sleep(0.001)
def __init__(self, bus_address=0x40, bus_id=1):
self.bus_address = bus_address
self.bus_id = bus_id
self.pwm = i2cdev.I2C(self.bus_address, self.bus_id)
import i2cdev
import math
import time
pwm = i2cdev.I2C(0x40, 1)
'''Reset POWM'''
pwm.write(bytes([0xFA, 0])) # zero all pin
pwm.write(bytes([0xFB, 0])) # zero all pin
pwm.write(bytes([0xFC, 0])) # zero all pin
pwm.write(bytes([0xFD, 0])) # zero all pin
pwm.write(bytes(
[0x01,
0x04])) # The 16 LEDn outputs are configured with a totem pole structure.
pwm.write(bytes([0x00,
0x01])) #PCA9685 responds to LED All Call I2C-bus address
time.sleep(0.01) # wait for oscillator
'''
Set the PWM frequency to the provided value in hertz.
The maximum PWM frequency is 1526 Hz if the PRE_SCALE register is set "0x03h".
The minimum PWM frequency is 24 Hz if the PRE_SCALE register is set "0xFFh".
he PRE_SCALE register can only be set when the SLEEP bit of MODE1 register is set to logic 1.
'''
freq_hz = 400
freq_hz = freq_hz * 0.9 #correction
prescale = int(math.floor(25000000.0 / (4096.0 * float(freq_hz)) -
1)) # datasheet equation
print("prescale = ", prescale)
pwm.write(bytes([0x00, 0x10]))
time.sleep(0.01)
pwm.write(bytes([0xFE, prescale]))
trigger = None
trigger = self.rotate_v_stepper_motor(-360)
if trigger is not None:
self.rotate_v_stepper_motor(-self.phi)
def send_message(connection, msg):
msg.get_header().srcSystem = 0
msg.get_header().srcComponent = 1
connection.mav.send(msg, False)
def convert_time_from_s_to_s_us(current_time):
current_time = math.modf(current_time)
return (int(current_time[1]), int(current_time[0] * 1000000))
if __name__ == '__main__':
i2c = i2cdev.I2C(PORT_I2C)
i2c.set_timeout(I2C_TIMEOUT)
lis3mdl = Lis3mdl(i2c, LIS3MDL_ADRESS)
lsm6ds3 = Lsm6ds3(i2c, LSM6DS3_ADRESS)
gpsd = GPS_data()
wwm = WMM2020()
v_stepper_motor = DM422_control_client(pul_pin=V_PUL_PIN,
dir_pin=V_DIR_PIN,
enable_pin=V_ENABLE_PIN,
gearbox_num=V_GEARBOX_NUM,
deg_per_step=V_DEG_PER_STEP,
pos_dir_state=V_POS_DIR_STATE,
stop_state=V_STOP_STATE)
h_stepper_motor = DM422_control_client(pul_pin=H_PUL_PIN,
dir_pin=H_DIR_PIN,
enable_pin=H_ENABLE_PIN,
#!/usr/bin/env python import rospy, math, serial import time import numpy as np import Jetson.GPIO as GPIO import os import mmap import i2cdev MCP4725_ADD_ACC = 0x60 MCP4725_ADD_BRAKE = 0x61 dac_acc = i2cdev.I2C(MCP4725_ADD_ACC, 1) dac_brake = i2cdev.I2C(MCP4725_ADD_BRAKE, 1) ######### ##Pins #accSwitch = 21 #modePins = (16,22,18) # N - F - R # # i2c dac acc , brake --> 3 DAT , 5 CLK #accPins = (24,26,32,36,38,40) #breakPins = (23,29,31,33,35,37) ######### from collections import deque # ROS Msgs from std_msgs.msg import Int16 from sensor_msgs.msg import Imu from nav_msgs.msg import Odometry from ackermann_msgs.msg import AckermannDriveStamped #REGION SerialInit.
## PI GPIO configrations GPIO.setmode(GPIO.BOARD) accSwitch = 21 modePins = (16, 22, 18) #(16,18,22) # N - F - R accPins = (24, 26, 32, 36, 38, 40) breakPins = (23, 29, 31, 33, 35, 37) GPIO.setup(modePins, GPIO.OUT) GPIO.setup(accSwitch, GPIO.OUT) GPIO.setup(accPins, GPIO.OUT) GPIO.setup(breakPins, GPIO.OUT) ## End ## PI I2C configrations MCP4725_ADD_ACC = 0x61 #MCP4725_ADD_BRAKE = 0x61 dac_acc = i2cdev.I2C(MCP4725_ADD_ACC, 8) #dac_brake = i2cdev.I2C(MCP4725_ADD_BRAKE, 8) ## End ## Global Variables plan_on = False DEBUG = 0 timeout_feedback_flag = 0 command_angle = 0.0 changing = 0 started = 0 speed = 0.0 v_mps = 0.0 Vmps_arr = [0.0] * 4 last_acc = 0 mean_Vmps = 0.0
def open(self):
self.i2c = i2cdev.I2C(self.device, self.bus)
def __init__(self, bus, address, vcc=3.3):
self.vcc = vcc
self._i2c = i2cdev.I2C(address, bus)
ADS1115_REG_CONFIG_DR_128SPS = 0x80 # 128 samples per second (default)
ADS1115_REG_CONFIG_DR_250SPS = 0xA0 # 250 samples per second
ADS1115_REG_CONFIG_DR_475SPS = 0xC0 # 475 samples per second
ADS1115_REG_CONFIG_DR_860SPS = 0xE0 # 860 samples per second
ADS1115_REG_CONFIG_CMODE_TRAD = 0x00 # Traditional comparator with hysteresis (default)
ADS1115_REG_CONFIG_CMODE_WINDOW = 0x10 # Window comparator
ADS1115_REG_CONFIG_CPOL_ACTVLOW = 0x00 # ALERT/RDY pin is low when active (default)
ADS1115_REG_CONFIG_CPOL_ACTVHI = 0x08 # ALERT/RDY pin is high when active
ADS1115_REG_CONFIG_CLAT_NONLAT = 0x00 # Non-latching comparator (default)
ADS1115_REG_CONFIG_CLAT_LATCH = 0x04 # Latching comparator
ADS1115_REG_CONFIG_CQUE_1CONV = 0x00 # Assert ALERT/RDY after one conversions
ADS1115_REG_CONFIG_CQUE_2CONV = 0x01 # Assert ALERT/RDY after two conversions
ADS1115_REG_CONFIG_CQUE_4CONV = 0x02 # Assert ALERT/RDY after four conversions
ADS1115_REG_CONFIG_CQUE_NONE = 0x03 # Disable the comparator and put ALERT/RDY in high state (default)
bus_0 = i2cdev.I2C(ADS1115_ADDRESS_0, 1) # Address = 0x48, I2C bus = 1
bus_1 = i2cdev.I2C(ADS1115_ADDRESS_1, 1) # Address = 0x48, I2C bus = 1
bus_2 = i2cdev.I2C(ADS1115_ADDRESS_2, 1) # Address = 0x48, I2C bus = 1
bus_3 = i2cdev.I2C(ADS1115_ADDRESS_3, 1) # Address = 0x48, I2C bus = 1
# def address_switcher(address_number):
# switcher = {
# 0: ADS1115_ADDRESS_0,
# 1: ADS1115_ADDRESS_1,
# 2: ADS1115_ADDRESS_2,
# 3: ADS1115_ADDRESS_3,
# }
# return switcher.get(address_number,ADS1115_DEFAULT_ADDRESS)
class ADS1115_0():
MODE_1_SUB2 = 0x04
MODE_1_SUB1 = 0x08
MODE_1_SLEEP = 0x10
MODE_1_AI = 0x20
MODE_1_EXTCLK = 0x40
MODE_1_RESTART = 0x80
MODE_2_OUTNE = 0x00
MODE_2_OUTDRV = 0x04
MODE_2_OCH = 0x08
MODE_2_INVRT = 0x10
OSC_VAL = 25000000.0
PRSCLR_CONST = 4096.0
bus = i2cdev.I2C(ADDRESS_I2C_DEFAULT, 1)
class PCA9865():
def reset_PWM(self):
bus.write(bytes([ALL_LED_ON_L, 0])) #All pins set to zero
bus.write(bytes([ALL_LED_ON_H, 0])) #All pins set to zero
bus.write(bytes([ALL_LED_OFF_L, 0])) #All pins set to zero
bus.write(bytes([ALL_LED_OFF_H, 0])) #All pins set to zero
bus.write(bytes([MODE_2, MODE_2_OUTDRV])) # Totem pole config.
#PCA9685 responds to LED All Call I2C-bus address
bus.write(bytes([MODE_1, MODE_1_ALLCALL]))
time.sleep(0.01) # Wait for oscillator
def set_freq(self, freq_hz):
prescale = int(numpy.floor((OSC_VAL / (PRSCLR_CONST * freq_hz)) - 1))
import i2cdev
import numpy as np
from time import sleep
dac = i2cdev.I2C(0x62, 1)
for x in np.arange(0, 4095, 10):
v = ((np.floor((33000 * x) / 4095)) / (10000))
print(v)
H = int(x / 16)
L = int((x - (16 * H)) * 16)
data_to_send = bytes([64, H, L])
dac.write(data_to_send)
#print([64,H,L])
sleep(0.1)
print('finished')
# change frequency for changing the output and fixed pwm value.
import i2cdev
import numpy
import time
pwm = i2cdev.I2C(0x40, 1) # (PCA9685 address,I2C bus)
# Reset PWM
pwm.write(bytes([0xFA, 0])) # zero all pin
pwm.write(bytes([0xFB, 0])) # zero all pin
pwm.write(bytes([0xFC, 0])) # zero all pin
pwm.write(bytes([0xFD, 0])) # zero all pin
pwm.write(bytes(
[0x01,
0x04])) # The 16 LEDn outputs are configured with a totem pole structure.
pwm.write(bytes([0x00,
0x01])) #PCA9685 responds to LED All Call I2C-bus address
time.sleep(0.01) # wait for oscillator
'''
Set the PWM frequency to the provided value in hertz.
The maximum PWM frequency is 1526 Hz if the PRE_SCALE register is set "0x03h".
The minimum PWM frequency is 24 Hz if the PRE_SCALE register is set "0xFFh".
he PRE_SCALE register can only be set when the SLEEP bit of MODE1 register is set to logic 1.
'''
# Program to set the Duty cycle and frequency of PWM.
freq_hz = 100 # Frequency of PWM
freq_hz = freq_hz * 0.9 #correction
prescale = int(numpy.floor(25000000.0 / (4096.0 * float(freq_hz)) -
1)) # datasheet equation
print("prescale = ", prescale)