class GFDMPhy(object):
def __init__(self, rio):
self._rio = rio
self._bitfilename = os.path.abspath(
os.path.join(os.path.dirname(__file__),
'../Builds/GFDM FPGA Toplevel for 40MHz USRP.lvbitx'))
assert os.path.exists(
self._bitfilename), "Bitfile not found as usual position!"
self._rxFifoName = 'Needed Resources.grsc\\RX.Data out.T2H'
self._txFifoName = 'Needed Resources.grsc\\TX.data in.H2T'
def __enter__(self):
self._session = Session(self._bitfilename, self._rio)
self._session.__enter__()
self._txFifo = self._session.fifos[self._txFifoName]
self._txFifo.configure(10000000)
self._txFifo.start()
self._rxFifo = self._session.fifos[self._rxFifoName]
self._rxFifo.configure(10000000)
self._rxFifo.start()
return self
def __exit__(self, exception_type, exception_val, trace):
self._session.__exit__(exception_type, exception_val, trace)
def read(self, numBytes, timeout_ms=200):
return np.array(self._rxFifo.read(numBytes, timeout_ms)[0],
dtype=np.uint8)
def write(self, data, timeout_ms=200):
self._txFifo.write(data, timeout_ms=timeout_ms)
def __enter__(self):
self._session = Session(self._bitfilename, self._rio)
self._session.__enter__()
self._txFifo = self._session.fifos[self._txFifoName]
self._txFifo.configure(10000000)
self._txFifo.start()
self._rxFifo = self._session.fifos[self._rxFifoName]
self._rxFifo.configure(10000000)
self._rxFifo.start()
return self
def __init__(self, key_manager, first_slot, is_fpga):
self.key_manager = key_manager
self.is_fpga = is_fpga
if self.is_fpga:
self.session = Session("real.lvbitx", "RIO0") #Session("real.lvbitx", "PXI1Slot2")
if(first_slot):
self.key = self.session.registers['Key1']
self.input = self.session.registers['Input1']
self.output = self.session.registers['Output1']
self.irq = 1
else:
self.key = self.session.registers['Key2']
self.input = self.session.registers['Input2']
self.output = self.session.registers['Output2']
self.irq = 2
def alignement(nx):
ny=nx
DMD = ALPlib.ALP4(version = '4.3', libDir = 'C:/Program Files/ALP-4.3/ALP-4.3 API')
DMD.Initialize()
DMD.SeqAlloc(nbImg = nseq, bitDepth = 1)
DMD.SeqPut(a,dataFormat='C')
DMD.SetTiming(illuminationTime=int(It))
with Session("C:\\Users\\lac\\Desktop\\Florian\\FPGA\\holoscan\\FPGA Bitfiles\\holoscan_FPGATarget_fpgamain_FGl-dvEuVeU.lvbitx","RIO0") as session :
count = session.fifos['count to PC']
it = session.fifos['integration time to PC']
count.start()
it.start()
session.reset()
session.run()
DMD.Run(loop=False)
DMD.Wait()
DMD.Free()
pc=count.read(nx+ny,200)
itime=it.read(nx+ny,200)
count.stop()
it.stop()
pici=np.zeros((nx+ny))
for i in range(2*nx):
pici[i]= pc.data[i]/(It*1e-6)
return(pici.tolist())
def Raster_Scan(Xpix,Ypix,xinit,yinit,zinit,pictime,dx,dy):
pici=zeros((Xpix,Ypix))
pict=zeros((Xpix,Ypix))
with Session("C:\\Users\\lac\\Desktop\\Florian\\FPGA\\TestFPGA\\scan_imaging\\FPGA Bitfiles\\scanimaging_FPGATarget_fpgamain_BYS4O-sfyPE.lvbitx","RIO0") as session :
count = session.fifos['count to PC']
it = session.fifos['integration time to PC']
count.start()
it.start()
for m in range(Xpix):
for n in range(Ypix):
l.append(Holo_Position(xinit+Xpix*mdx,yinit+Ypix*dy,zinit,correction_mask).ravel())
adresses.append(DMD.SeqAlloc(nbImg =Xpix*Ypix, bitDepth = 1))
imgSeq=concatenate([i for i in l]) #on met les tableaux 1D les uns à la suite des autres pour créer une séquence
DMD.SeqPut(imgSeq,SequenceId=adresses[0])
DMD.SetTiming(SequenceId = adresses[0], pictureTime = pictime) #picture time est le temps écoulé entre deux images d'une séquence en microsecondes
DMD.Run(SequenceId=adresses[0],loop=False)
#session.reset()
#session.run()
DMD.Wait()
pc=count.read(Xpix*Ypix-1,10) #nombre de valeur à lire dans le tampon et temps maximum passé à tester la presence d'une valeur dans le tampon
itime=it.read(Xpix*Ypix-1,10)
count.stop() #On arrète les tampons
it.stop()
pc.data.append(0) #La méthode .read retourne une liste des valeurs lu appelée "data" et le nombre d'élements encore présents dans le tampon. Ici on ajoute un zéro à la data car la première valeure lors de la mesure n'a pa été prise en compte
itime.data.append(0)
for i in range(Xpix):
for j in range(Ypix):
pici[i][j]= pc.data[Ypix*i+j] # On fait de la liste data une image
pict[i][j]= itime.data[Ypix*i+j]
return(pici/pict)
def FourElementAverage():
FifoData = []
print("Enter one integer element at a time.\n")
print("Data will be sent to the FPGA via FIFO, and the rounded integer average will be returned.\n")
i = 0
#Loop to accept integer inputs
while i < 4:
try:
FifoData.append( int( input("Enter a number: ".format(i + 1)) ) )
i += 1
except: #Repeat loop iteration if invalid entry
print("Invalid input. Please enter an integer. ")
pass
print("Computing the average of {0}, {1}, {2}, and {3}...".format(FifoData[0], FifoData[1], FifoData[2], FifoData[3]))
with Session(bitfile = bitfilepath, resource = targetname) as session:
session.reset() #Stop FPGA logic; put into default state
session.run() #Run FPGA logic
Average = session.registers['4-Element Average'] #Indicator for the average value
Host_Target_FIFO = session.fifos['Elements To Average'] #Obtain the host to target FIFO
Host_Target_FIFO.start() #Start the FIFO
for j in range(4):
Host_Target_FIFO.write(FifoData[j], timeout_ms = 100) #Writes numeric data to the FIFO, 100 ms timeout
data = Average.read()
print("The integer average is {0:.2f}".format(data))
def ChassisTemperature():
with Session(bitfile = bitfilepath, resource = targetname) as session:
session.reset() #Stop FPGA logic; put into default state
session.run() #Run FPGA logic
temp_indicator = session.registers['Chassis Temperature']
temperature = temp_indicator.read() / 4 #Divide temperature by 4 to convert FPGA -> Celsius
print("The Internal Chassis Temperature is {0:.1f} C".format(temperature))
class Nifpga(object):
""" National Instruments FPGA that controls the lasers via an OTF.
"""
# indicate bitfile path
# simple test with one laser line 561 nm and writing a list of values to this
# bitfile = 'C:\\Users\\sCMOS-1\\qudi-cbs\\hardware\\fpga\\FPGA\\FPGA Bitfiles\\FPGAv0_FPGATarget_FPGAlasercontrol_pdDEc3yii+w.lvbitx'
# multicolor imaging bitfile
bitfile = 'C:\\Users\\sCMOS-1\\qudi-cbs\\hardware\\fpga\\FPGA\\FPGA Bitfiles\\FPGAv0_FPGATarget_FPGAtriggercamer_u12WjFsC0U8.lvbitx'
resource = 'RIO0'
def __init__(self, *args, **kwargs):
super().__init__()
def on_activate(self):
""" Required initialization steps when module is called."""
self.session = Session(bitfile=self.bitfile, resource=self.resource)
print(self.session._registers)
# self.n_lines = self.session.registers['N']
# self.laser_control = self.session.registers['561 Laser Power']
def on_deactivate(self):
""" Required deactivation steps. """
self.session.close()
print('session closed')
# def apply_voltage_values(self):
# self.session.reset()
#
# print(self.n_lines.read())
# self.n_lines.write(5)
# print(self.n_lines.read())
#
# data = [4, 0, 4, 2, 0]
# conv_values = [self.convert_value(item) for item in data]
# print(conv_values)
# self.laser_control.write(conv_values)
# self.session.run()
def convert_value(self, value):
""" helper function: fpga needs int16 (-32768 to + 32767) data format: do rescaling of value to apply in percent of max value
apply min function to limit the allowed range """
return min(int(value / 100 * (2**15 - 1)),
36767) # set to maximum in case value > 100
def init_fpga(self, device, loop_rate):
self.session = Session(self.full_bitpath,
device,
reset_if_last_session_on_exit=True)
self.session.download()
self.session.run()
print(self.session.fpga_vi_state)
self.write_fifo_object = self.session.fifos['DMA_WRITE']
self.read_fifo_object = self.session.fifos['DMA_READ']
self.loop_timer = self.session.registers['Loop Rate (usec)']
self.fpga_start_control = self.session.registers['Start']
self.fpga_rtsi = self.session.registers['Write to RTSI']
self.fpga_ex_timing = self.session.registers['Use External Timing']
self.fpga_irq = self.session.registers['Generate IRQ']
self.loop_timer.write(loop_rate)
self.fpga_rtsi.write(False)
self.fpga_ex_timing.write(False)
self.fpga_irq.write(False)
def LedSequence():
with Session(bitfile = bitfilepath, resource = targetname) as session:
session.reset() #Stop FPGA logic; put into default state
session.run() #Run FPGA logic
LED_control = session.registers['User Fpga Led']
for i in range(3):
LED_control.write(1) #green
time.sleep(1) #1 second wait
LED_control.write(2) #orange
time.sleep(1)
LED_control.write(0)
print("LED Sequence {0} of 3 Completed".format(i + 1))
def WhiteGaussianNoise():
#Generate log file. This will replace an existing file of the same name.
log_file = open("log.txt", "w")
#DMA FIFO Variables:
Number_Acquire = 40
Fifo_Timeout = Number_Acquire*10
#IRQ Variables:
irqTimeout = 1000
irq = 0 #IRQ 0 is the default IRQ if unwired
with Session(bitfile = bitfilepath, resource = targetname) as session:
session.reset() #Stop FPGA logic; put into default state
session.run() #Run FPGA logic
Noise_length = session.registers['White Noise Data Length'] #Assign control to set # of White noise data points
White_Gaussian_FIFO = session.fifos['White Gaussian Noise'] #Assign Target to Host FIFO
White_Gaussian_FIFO.start() #Start the FIFO
#Wait on IRQ to find if FPGA is ready
irq_status = session.wait_on_irqs(irq, irqTimeout)
if irq_status.timed_out == True:
print("Timed out while waiting for interrupt.")
#If IRQ asserted:
if irq in irq_status.irqs_asserted:
print("IRQ 0 was asserted.")
Noise_length.write(Number_Acquire)
#Acknowledge IRQ
session.acknowledge_irqs(irq_status.irqs_asserted)
#Read FIFO data into Fifo_Data array
Fifo_Data = White_Gaussian_FIFO.read(Number_Acquire, timeout_ms = Fifo_Timeout)
#Normalize, log and print data
for i in range(0, Number_Acquire):
RMS_val = int (Fifo_Data.data[i]) / 6000.0 #Divide by 6000, since that is the RMS
log_file.write("{0:.2f}\n".format(RMS_val)) #Add data point to the log file
print(RMS_val)
else:
print("IRQ 0 was not asserted.")
log_file.close()
def main(RIO, RX_PORT, TX_PORT):
profiling = False
if profiling:
enable_thread_profiling()
bitfilename = os.path.abspath(
os.path.join(os.path.dirname(__file__),
'../Builds/GFDM FPGA Toplevel for 40MHz USRP.lvbitx'))
assert os.path.exists(bitfilename), "Bitfile not found as usual position!"
rxFifoName = 'Needed Resources.grsc\\RX.Data out.T2H'
txFifoName = 'Needed Resources.grsc\\TX.data in.H2T'
with Session(bitfilename, RIO) as session:
try:
txFifo = session.fifos[txFifoName]
txFifo.configure(10000000)
txFifo.start()
rxFifo = session.fifos[rxFifoName]
rxFifo.configure(10000000)
rxFifo.start()
stopEvent = threading.Event()
TX = threading.Thread(target=txThread,
args=(stopEvent, txFifo, RX_PORT))
TX.start()
RX = threading.Thread(target=rxThread,
args=(stopEvent, rxFifo, TX_PORT))
RX.start()
while True:
time.sleep(0.1)
finally:
stopEvent.set()
TX.join()
RX.join()
if profiling:
get_thread_stats().dump_stats(filename='profile2.prof')
def main(RIO, basePacketLength, numBytes):
bitfilename = os.path.abspath(
os.path.join(os.path.dirname(__file__),
'../Builds/GFDM FPGA Toplevel for 40MHz USRP.lvbitx'))
assert os.path.exists(bitfilename), "Bitfile not found as usual position!"
rxFifoName = 'Needed Resources.grsc\\RX.Data out.T2H'
txFifoName = 'Needed Resources.grsc\\TX.data in.H2T'
packetLen = int((numBytes // basePacketLength) * basePacketLength)
with Session(bitfilename, RIO) as session:
txFifo = session.fifos[txFifoName]
txFifo.configure(10000000)
txFifo.start()
rxFifo = session.fifos[rxFifoName]
rxFifo.configure(10000000)
rxFifo.start()
while True:
oneTransmission(packetLen, txFifo, rxFifo)
time.sleep(0.1)
from nifpga import Session bitfilepath = "MyBitfile.lvbitx" targetname = "10.1.128.157/RIO0" with Session(bitfile = bitfilepath, resource = targetname) as session: # Reset stops the logic on the FPGA and puts it in the default state. # May substitute reset with download if your bitfile doesn't support it. session.reset() # Add Initialization code here! # Write initial values to controls while the FPGA logic is stopped. # Start the logic on the FPGA session.run() Noise_length = session.registers['White Noise Data Length'] Noise_length.write(10) White_Gaussian_FIFO = session.fifos['White Gaussian Noise'] White_Gaussian_FIFO.start() #Start the FIFO Fifo_Data = White_Gaussian_FIFO.read(10, timeout_ms = 500) #Normalize (Divide by 6000, since that is the RMS) and Print data print(Fifo_Data.data / 6000)
# -*- coding: utf-8 -*-
"""
Created on Wed Feb 5 12:30:39 2020
@author: strobe
"""
from nifpga import Session
with Session(
"C:/Users/strobe/Desktop/example_digitizer_vi/FPGA Bitfiles/PXIe_5775_KU040.lvbitx",
"RIO0") as session:
my_control = session.registers['My Control']
my_indicator = session.registers['My Indicator']
my_control.write(4)
data = my_indicator.read()
print(data) # prints 16
def on_activate(self):
""" Required initialization steps when module is called."""
self.session = Session(bitfile=self.default_bitfile,
resource=self.resource)
# self.laser1_control = self.session.registers[self._registers[0]]
# self.laser2_control = self.session.registers[self._registers[1]]
# self.laser3_control = self.session.registers[self._registers[2]]
# self.laser4_control = self.session.registers[self._registers[3]]
# # maybe think of replacing the hardcoded version of assigning the registers to an identifier by something more dynamic
# self.session.reset()
# for i in range(len(self._registers)):
# self.apply_voltage(0, self._registers[i]) # set initial value to each channel
#
# self.QPD_X_read = self.session.registers[self._registers_qpd[0]]
# self.QPD_Y_read = self.session.registers[self._registers_qpd[1]]
# self.QPD_I_read = self.session.registers[self._registers_qpd[2]]
# self.Integration_time_us = self.session.registers[self._registers_qpd[3]]
# self.Duration_ms = self.session.registers[self._registers_qpd[4]]
# self.Task = self.session.registers[self._registers_qpd[5]]
#
# self.Task.write(False)
# self.session.run()
# Initialize registers dictionnary according to the type of experiment selected
if self._wavelengths is not None:
self.laser1_control = self.session.registers[
self._registers_laser[0]]
self.laser2_control = self.session.registers[
self._registers_laser[1]]
self.laser3_control = self.session.registers[
self._registers_laser[2]]
self.laser4_control = self.session.registers[
self._registers_laser[3]]
self.update = self.session.registers[self._registers_laser[4]]
self.session.reset()
for i in range(len(self._registers_laser) - 1):
self.apply_voltage(0, self._registers_laser[i]
) # set initial value to each channel
if self._registers_qpd is not None:
self.QPD_X_read = self.session.registers[self._registers_qpd[0]]
self.QPD_Y_read = self.session.registers[self._registers_qpd[1]]
self.QPD_I_read = self.session.registers[self._registers_qpd[2]]
self.Counter = self.session.registers[self._registers_qpd[3]]
self.Duration_ms = self.session.registers[self._registers_qpd[4]]
self.Stop = self.session.registers[self._registers_general[0]]
self.Integration_time_us = self.session.registers[
self._registers_general[1]]
self.Reset_counter = self.session.registers[
self._registers_general[2]]
self.setpoint = self.session.registers[
self._registers_autofocus[0]]
self.P = self.session.registers[self._registers_autofocus[1]]
self.I = self.session.registers[self._registers_autofocus[2]]
self.reset = self.session.registers[self._registers_autofocus[3]]
self.autofocus = self.session.registers[
self._registers_autofocus[4]]
self.ref_axis = self.session.registers[
self._registers_autofocus[5]]
self.output = self.session.registers[self._registers_autofocus[6]]
self.Stop.write(False)
self.Integration_time_us.write(10)
self.session.run()
def start_task_session(self, bitfile):
""" loads a bitfile used for a specific task """
self.session = Session(bitfile=bitfile, resource=self.resource)
class Nifpga(Base, LaserControlInterface, FPGAInterface):
""" National Instruments FPGA that controls the lasers via an OTF.
Example config for copy-paste:
nifpga:
module.Class: 'fpga.ni_fpga.Nifpga'
resource: 'RIO0'
default_bitfile: 'C:\\Users\\sCMOS-1\\qudi-cbs\\hardware\\fpga\\FPGA\\FPGA Bitfiles\\FPGAv0_FPGATarget_FPGAlasercontrol_mLrb7Qjptmw.lvbitx'
wavelengths:
- '405 nm'
- '488 nm'
- '561 nm'
- '640 nm'
registers:
- '405'
- '488'
- '561'
- '640'
registers_qpd:
- 'x'
- 'y'
- 'i'
# registers represent something like the channels.
# The link between registers and the physical channel is made in the labview file from which the bitfile is generated.
"""
# config
resource = ConfigOption('resource', None, missing='error')
default_bitfile = ConfigOption('default_bitfile', None, missing='error')
_wavelengths = ConfigOption('wavelengths', None, missing='warn')
_registers_laser = ConfigOption('registers_laser', None, missing='warn')
_registers_qpd = ConfigOption('registers_qpd', None, missing='warn')
_registers_autofocus = ConfigOption('registers_autofocus',
None,
missing='warn')
_registers_general = ConfigOption('registers_general',
None,
missing='warn')
def __init__(self, config, **kwargs):
super().__init__(config=config, **kwargs)
def on_activate(self):
""" Required initialization steps when module is called."""
self.session = Session(bitfile=self.default_bitfile,
resource=self.resource)
# self.laser1_control = self.session.registers[self._registers[0]]
# self.laser2_control = self.session.registers[self._registers[1]]
# self.laser3_control = self.session.registers[self._registers[2]]
# self.laser4_control = self.session.registers[self._registers[3]]
# # maybe think of replacing the hardcoded version of assigning the registers to an identifier by something more dynamic
# self.session.reset()
# for i in range(len(self._registers)):
# self.apply_voltage(0, self._registers[i]) # set initial value to each channel
#
# self.QPD_X_read = self.session.registers[self._registers_qpd[0]]
# self.QPD_Y_read = self.session.registers[self._registers_qpd[1]]
# self.QPD_I_read = self.session.registers[self._registers_qpd[2]]
# self.Integration_time_us = self.session.registers[self._registers_qpd[3]]
# self.Duration_ms = self.session.registers[self._registers_qpd[4]]
# self.Task = self.session.registers[self._registers_qpd[5]]
#
# self.Task.write(False)
# self.session.run()
# Initialize registers dictionnary according to the type of experiment selected
if self._wavelengths is not None:
self.laser1_control = self.session.registers[
self._registers_laser[0]]
self.laser2_control = self.session.registers[
self._registers_laser[1]]
self.laser3_control = self.session.registers[
self._registers_laser[2]]
self.laser4_control = self.session.registers[
self._registers_laser[3]]
self.update = self.session.registers[self._registers_laser[4]]
self.session.reset()
for i in range(len(self._registers_laser) - 1):
self.apply_voltage(0, self._registers_laser[i]
) # set initial value to each channel
if self._registers_qpd is not None:
self.QPD_X_read = self.session.registers[self._registers_qpd[0]]
self.QPD_Y_read = self.session.registers[self._registers_qpd[1]]
self.QPD_I_read = self.session.registers[self._registers_qpd[2]]
self.Counter = self.session.registers[self._registers_qpd[3]]
self.Duration_ms = self.session.registers[self._registers_qpd[4]]
self.Stop = self.session.registers[self._registers_general[0]]
self.Integration_time_us = self.session.registers[
self._registers_general[1]]
self.Reset_counter = self.session.registers[
self._registers_general[2]]
self.setpoint = self.session.registers[
self._registers_autofocus[0]]
self.P = self.session.registers[self._registers_autofocus[1]]
self.I = self.session.registers[self._registers_autofocus[2]]
self.reset = self.session.registers[self._registers_autofocus[3]]
self.autofocus = self.session.registers[
self._registers_autofocus[4]]
self.ref_axis = self.session.registers[
self._registers_autofocus[5]]
self.output = self.session.registers[self._registers_autofocus[6]]
self.Stop.write(False)
self.Integration_time_us.write(10)
self.session.run()
def on_deactivate(self):
""" Required deactivation steps. """
for i in range(len(self._registers_laser) - 1):
self.apply_voltage(
0, self._registers_laser[i]
) # make sure to switch the lasers off before closing the session
self.Stop.write(True)
self.session.close()
def read_qpd(self):
""" read QPD signal and return a list containing the X,Y position of the spot, the SUM signal,
the number of counts (iterations) since the session was launched and the duration of each iteration
"""
X = self.QPD_X_read.read()
Y = self.QPD_Y_read.read()
I = self.QPD_I_read.read()
count = self.Counter.read()
d = self.Duration_ms.read()
return [X, Y, I, count, d]
def reset_qpd_counter(self):
self.Reset_counter.write(True)
def update_pid_gains(self, p_gain, i_gain):
self.P.write(p_gain)
self.I.write(i_gain)
def init_pid(self, p_gain, i_gain, setpoint, ref_axis):
self.reset_qpd_counter()
self.setpoint.write(setpoint)
self.P.write(p_gain)
self.I.write(i_gain)
if ref_axis == 'X':
self.ref_axis.write(True)
elif ref_axis == 'Y':
self.ref_axis.write(False)
self.reset.write(True)
self.autofocus.write(True)
sleep(0.1)
self.reset.write(False)
def read_pid(self):
pid_output = self.output.read()
return pid_output
def stop_pid(self):
self.autofocus.write(False)
def apply_voltage(self, voltage, channel):
""" Writes a voltage to the specified channel.
@param: any numeric type, (recommended int) voltage: percent of maximal volts to be applied
if value < 0 or value > 100, value will be rescaled to be in the allowed range
@param: str channel: register name corresponding to the physical channel (link made in labview bitfile), example '405'
@returns: None
"""
# maybe think of replacing the hardcoded version of comparing channels with registers by something more dynamic
value = max(0, voltage)
conv_value = self.convert_value(value)
if channel == self._registers_laser[0]: # '405'
self.laser1_control.write(conv_value)
elif channel == self._registers_laser[1]: # '488'
self.laser2_control.write(conv_value)
elif channel == self._registers_laser[2]: # '561'
self.laser3_control.write(conv_value)
elif channel == self._registers_laser[3]: # '640'
self.laser4_control.write(conv_value)
else:
pass
self.update.write(True)
def convert_value(self, value):
""" helper function: fpga needs int16 (-32768 to + 32767) data format: do rescaling of value to apply in percent of max value
apply min function to limit the allowed range """
return min(int(value / 100 * (2**15 - 1)),
32767) # set to maximum in case value > 100
def read_values(self):
""" for tests - returns the (converted) values applied to the registers """
return self.laser1_control.read(), self.laser2_control.read(
), self.laser3_control.read(), self.laser4_control.read()
def get_dict(self):
""" Retrieves the register name (and the corresponding voltage range???) for each analog output from the
configuration file and associates it to the laser wavelength which is controlled by this channel.
@returns: laser_dict
"""
laser_dict = {}
for i, item in enumerate(
self._wavelengths
): # use any of the lists retrieved as config option, just to have an index variable
label = 'laser{}'.format(
i + 1
) # create a label for the i's element in the list starting from 'laser1'
dic_entry = {
'label': label,
'wavelength': self._wavelengths[i],
'channel': self._registers_laser[i]
}
# 'ao_voltage_range': self._ao_voltage_ranges[i]
laser_dict[dic_entry['label']] = dic_entry
return laser_dict
### new 3 march 2021 test with tasks
## these methods must be callable from the lasercontrol logic
def close_default_session(self):
""" This method is called before another bitfile than the default one shall be loaded
(in this version it actually does the same as on_deactivate (we could also just call this method .. but this might evolve)
"""
for i in range(len(self._registers_laser)):
self.apply_voltage(
0, self._registers_laser[i]
) # make sure to switch the lasers off before closing the session
self.session.close()
def restart_default_session(self):
""" This method allows to restart the default session"""
self.on_activate()
#or is it sufficient to just call self.session = Session(bitfile=self.default_bitfile, resource=self.resource) and session run ?
def start_task_session(self, bitfile):
""" loads a bitfile used for a specific task """
self.session = Session(bitfile=bitfile, resource=self.resource)
def end_task_session(self):
self.session.close()
# methods associated to a specific bitfile
def run_test_task_session(self, data):
"""
associated bitfile 'C:\\Users\\sCMOS-1\\qudi-cbs\\hardware\\fpga\\FPGA\\FPGA Bitfiles\\FPGAv0_FPGATarget_FPGAlasercontrol_pdDEc3yii+w.lvbitx'
@param: list data: values to be applied to the output (in % of max intensity) """
#using for a simple test the FPGA_laser_control_Qudi bitfile (control only for the 561 nm laser)
n_lines = self.session.registers['N']
laser_control = self.session.registers['561 Laser Power']
self.session.reset()
print(n_lines.read())
n_lines.write(5)
print(n_lines.read())
conv_values = [self.convert_value(item) for item in data]
print(conv_values)
laser_control.write(conv_values)
self.session.run()
def run_multicolor_imaging_task_session(self, z_planes, wavelength, values,
num_laserlines, exposure_time_ms):
"""
associated bitfile 'C:\\Users\\sCMOS-1\\qudi-cbs\\hardware\\fpga\\FPGA\\FPGA Bitfiles\\FPGAv0_FPGATarget_FPGAtriggercamer_u12WjFsC0U8.lvbitx'
@param: int z_planes: number of z planes
@param: int list wavelength: list containing the number of the laser line to be addressed: 0: BF, 1: 405, 2: 488, 3: 561, 4: 640
@param: int list values: intensity in per cent to be applied to the line given at the same index in the wavelength list
"""
num_lines = self.session.registers[
'N laser lines'] # number of laser lines
num_z_pos = self.session.registers[
'N Z positions'] # number of z positions
num_images_acquired = self.session.registers[
'Images acquired'] # indicator register how many images have been acquired
laser_lines = self.session.registers[
'Laser lines'] # list containing numbers of the laser lines which should be addressed
laser_power = self.session.registers[
'Laser power'] # list containing the intensity in % to apply (to the element at the same index in laser_lines list
stop = self.session.registers['stop']
exposure = self.session.registers[
'exposure_time_ms'] # integer indicating the exposure time of the camera in ms
self.QPD_X_read = self.session.registers[self._registers_qpd[0]]
self.QPD_Y_read = self.session.registers[self._registers_qpd[1]]
self.QPD_I_read = self.session.registers[self._registers_qpd[2]]
self.Counter = self.session.registers[self._registers_qpd[3]]
self.Duration_ms = self.session.registers[self._registers_qpd[4]]
self.Stop = self.session.registers[self._registers_general[0]]
self.Integration_time_us = self.session.registers[
self._registers_general[1]]
self.Reset_counter = self.session.registers[self._registers_general[2]]
self.setpoint = self.session.registers[self._registers_autofocus[0]]
self.P = self.session.registers[self._registers_autofocus[1]]
self.I = self.session.registers[self._registers_autofocus[2]]
self.reset = self.session.registers[self._registers_autofocus[3]]
self.autofocus = self.session.registers[self._registers_autofocus[4]]
self.ref_axis = self.session.registers[self._registers_autofocus[5]]
self.output = self.session.registers[self._registers_autofocus[6]]
self.Stop.write(False)
self.Integration_time_us.write(10)
self.session.reset()
conv_values = [self.convert_value(item) for item in values]
num_lines.write(num_laserlines)
print(num_laserlines)
num_z_pos.write(z_planes)
print(z_planes)
laser_lines.write(wavelength)
print(wavelength)
laser_power.write(conv_values)
print(conv_values)
stop.write(False)
print("exposure time = " + str(exposure_time_ms))
exposure_time_ms = int(exposure_time_ms * 1000 * 2)
exposure.write(exposure_time_ms)
self.session.run() # wait_until_done=True
num_imgs = num_images_acquired.read()
print(f'number images acquired: {num_imgs}')
from nifpga import Session
# This module can read and write values to the FPGA session that is initialized by LabVIEW
# The cRIO controller must be connected and the Labview bitfile must be in the same directory as this module
# Open a reference to the running FPGA session which is already initialized in LabVIEW
session = Session(bitfile="PythonMotorTest_FPGATarget_FPGA_EBn6Kf8AHrE.lvbitx",
resource="RIO://172.22.11.2/RIO0")
torque_to_CAN = session.registers['torqueToCAN']
speed_to_CAN = session.registers['speedToCAN']
direction_to_CAN = session.registers['directionToCAN']
inverter_enable_to_CAN = session.registers['inverter_EnableToCAN']
inverter_discharge_to_CAN = session.registers['inverter_DischargeToCAN']
speed_mode_enable_to_CAN = session.registers['speed_Mode_EnableToCAN']
commanded_torque_limit_to_CAN = session.registers[
'commanded_Torque_LimitToCAN']
in_time_to_CAN = session.registers['in_timeToCAN']
program_enable_to_CAN = session.registers['program_enable']
command_mode_curve_to_CAN = session.registers['command_mode_curve']
# Write function arguments to FPGA for LabVIEW to read and process
def send_command(torque, speed, direction, inv_enable, inv_discharge,
speed_mode, torque_limit, in_time):
torque_to_CAN.write(torque)
speed_to_CAN.write(speed)
direction_to_CAN.write(direction)
inverter_enable_to_CAN.write(inv_enable)
inverter_discharge_to_CAN.write(inv_discharge)
def reset_all_rollouts(self):
with Session(bitfile="SCPC-lv-noFIFO_FPGATarget_FPGAmainepos_1XvgQEcJVeE.lvbitx", resource="rio://10.157.23.150/RIO0") as session:
act_Rj1=session.registers['Mod3/AO0']
enc_Rj1=session.registers['Rj1']
act_Rj2=session.registers['Mod3/AO1']
enc_Rj2=session.registers['Rj2']
act_Lj1=session.registers['Mod3/AO3']
enc_Lj1=session.registers['Lj1']
act_Lj2=session.registers['Mod3/AO4']
enc_Lj2=session.registers['Lj2']
sen_f=session.registers['fsensor']
sen_e=session.registers['fencoder']
des_l = 0.08
des_th = 0.
des_p = np.array([[np.min([np.max([des_l, -self.l_limit]), self.l_limit])], [np.min([np.max([des_th, -self.th_limit]), self.th_limit])]])#0.7854
des_p_R = np.array([des_p[0]/2.0, des_p[1]])
des_p_L = des_p_R
r = np.array([[0.5*1.0], [1.0]])
new_goal = np.random.random()*np.pi/3.0 - np.pi/9.0
for i in range(100):
Re1 = enc_Rj1.read()
Re2 = enc_Rj2.read()
Le1 = enc_Lj1.read()
Le2 = enc_Lj2.read()
f_sensor = 5.1203 * sen_f.read() - 5.2506
e_sensor = (((sen_e.read()) - (f_sensor / 100.0 * 0.15)) -2.9440)/0.0148
Rj = self.R_j_inv * self.R_e * np.array([[Re1-self.offset[0]],[-Re2+self.offset[1]]]) * np.pi/180.0
Lj = self.R_j_inv_L * self.R_e * np.array([[Le1-self.offset[2]],[Le2-self.offset[3]]]) * np.pi/180.0
xR = self.L1 * np.cos(Rj[0,0] + np.pi/2.0) + self.L2 * np.cos(Rj[0,0]-Rj[1,0] + np.pi/2.0)
yR = self.L1 * np.sin(Rj[0,0] + np.pi/2.0) + self.L2 * np.sin(Rj[0,0]-Rj[1,0] + np.pi/2.0)
xL = self.L1 * np.cos(Lj[0,0] + np.pi/2.0) + self.L2 * np.cos(Lj[0,0]+Lj[1,0] + np.pi/2.0)
yL = self.L1 * np.sin(Lj[0,0] + np.pi/2.0) + self.L2 * np.sin(Lj[0,0]+Lj[1,0] + np.pi/2.0)
P_R = np.array([xR, yR])
P_L = np.array([xL, yL])
Prel_R = self.Pc_R - P_R
Prel_L = self.Pc_L - P_L
l_R = np.sqrt(Prel_R[0]*Prel_R[0] + Prel_R[1]*Prel_R[1])
l_L = np.sqrt(Prel_L[0]*Prel_L[0] + Prel_L[1]*Prel_L[1])
p_R = np.array([[l_R],[np.arctan2(-Prel_R[1],-Prel_R[0])]])
p_L = np.array([[l_L],[np.arctan2(Prel_L[1],Prel_L[0])]])
change_l_R = np.clip(des_l - p_R[0,0], -self.l_step_limit, self.l_step_limit)
change_th_R = np.clip(des_th - p_R[1,0], -8.0*self.th_step_limit, self.th_step_limit)
change_l_L = np.clip(des_l - p_L[0,0], -self.l_step_limit, self.l_step_limit)
change_th_L = np.clip(des_th - p_L[1,0], -8.0*self.th_step_limit, self.th_step_limit)
des_p_R = np.array([[np.min([np.max([p_R[0,0] + change_l_R, -self.l_limit/2.0]), self.l_limit/2.0])], [np.min([np.max([p_R[1,0] + change_th_R, -self.th_limit]), self.th_limit])]])
des_p_L = np.array([[np.min([np.max([p_L[0,0] + change_l_L, -self.l_limit/2.0]), self.l_limit/2.0])], [np.min([np.max([p_L[1,0] + change_th_L, -self.th_limit]), self.th_limit])]])
Jp_R = np.matrix([[-Prel_R[0]/l_R, -Prel_R[1]/l_R],[Prel_R[1]/l_R/l_R, -Prel_R[0]/l_R/l_R]])
Jp_L = np.matrix([[-Prel_L[0]/l_L, -Prel_L[1]/l_L],[Prel_L[1]/l_L/l_L, -Prel_L[0]/l_L/l_L]])
Jp_inv_R = np.matrix([[Jp_R[1,1] / (Jp_R[0,0]*Jp_R[1,1] - Jp_R[0,1]*Jp_R[1,0]), -Jp_R[0,1] / (Jp_R[0,0]*Jp_R[1,1] - Jp_R[0,1]*Jp_R[1,0])], [-Jp_R[1,0] / (Jp_R[0,0]*Jp_R[1,1] - Jp_R[0,1]*Jp_R[1,0]), Jp_R[0,0] / (Jp_R[0,0]*Jp_R[1,1] - Jp_R[0,1]*Jp_R[1,0])]])
Jp_inv_L = np.matrix([[Jp_L[1,1] / (Jp_L[0,0]*Jp_L[1,1] - Jp_L[0,1]*Jp_L[1,0]), -Jp_L[0,1] / (Jp_L[0,0]*Jp_L[1,1] - Jp_L[0,1]*Jp_L[1,0])], [-Jp_L[1,0] / (Jp_L[0,0]*Jp_L[1,1] - Jp_L[0,1]*Jp_L[1,0]), Jp_L[0,0] / (Jp_L[0,0]*Jp_L[1,1] - Jp_L[0,1]*Jp_L[1,0])]])
J_R = np.matrix([[-yR, self.L2 * np.cos(Rj[0,0]-Rj[1,0])],
[xR, self.L2 * np.sin(Rj[0,0]-Rj[1,0])]])
J_L = np.matrix([[-yL, -self.L2 * np.cos(Lj[0,0]+Lj[1,0])],
[xL, -self.L2 * np.sin(Lj[0,0]+Lj[1,0])]])
J_inv_R = np.matrix([[J_R[1,1] / (J_R[0,0]*J_R[1,1] - J_R[0,1]*J_R[1,0]), -J_R[0,1] / (J_R[0,0]*J_R[1,1] - J_R[0,1]*J_R[1,0])], [-J_R[1,0] / (J_R[0,0]*J_R[1,1] - J_R[0,1]*J_R[1,0]), J_R[0,0] / (J_R[0,0]*J_R[1,1] - J_R[0,1]*J_R[1,0])]])
J_inv_L = np.matrix([[J_L[1,1] / (J_L[0,0]*J_L[1,1] - J_L[0,1]*J_L[1,0]), -J_L[0,1] / (J_L[0,0]*J_L[1,1] - J_L[0,1]*J_L[1,0])], [-J_L[1,0] / (J_L[0,0]*J_L[1,1] - J_L[0,1]*J_L[1,0]), J_L[0,0] / (J_L[0,0]*J_L[1,1] - J_L[0,1]*J_L[1,0])]])
max_k_R = np.transpose(J_inv_R) * self.max_kj_R * J_inv_R
max_k_L = np.transpose(J_inv_L) * self.max_kj_L * J_inv_L
max_kp_R = np.transpose(Jp_inv_R) * max_k_R * Jp_inv_R
max_kp_L = np.transpose(Jp_inv_L) * max_k_L * Jp_inv_L
max_kp_R[0,1] = 0.0
max_kp_R[1,0] = 0.0
max_kp_L[0,1] = 0.0
max_kp_L[1,0] = 0.0
des_Fp_R = max_kp_R * (r * (des_p_R - p_R))
des_Fp_L = max_kp_L * (r * (des_p_L - p_L))
des_F_R = np.transpose(Jp_R) * des_Fp_R
des_F_L = np.transpose(Jp_L) * des_Fp_L
des_tau_R = np.transpose(J_R) * des_F_R if Rj[1,0] < 0 else np.array([[0.0],[-1.0]])
des_tau_L = np.transpose(J_L) * des_F_L if Lj[1,0] < 0 else np.array([[0.0],[-1.0]])
des_tau_R = np.clip(des_tau_R, -5., 5.)
des_tau_L = np.clip(des_tau_L, -5., 5.)
des_mR = (np.transpose(self.R_j_inv)*des_tau_R / (2*self.Ksc) + self.R_j * Rj) / self.Rm
des_mL = (np.transpose(self.R_j_inv_L)*des_tau_L / (2*self.Ksc) + self.R_j_L * Lj) / self.Rm
# Position control mode
act_Rj1.write(np.min([np.max([des_mR[0,0] * 180.0 / np.pi * 0.117258, -10.0]),10.0]))
act_Rj2.write(np.min([np.max([des_mR[1,0] * 180.0 / np.pi * 0.117541, -10.0]),10.0]))
act_Lj1.write(np.min([np.max([des_mL[0,0] * 180.0 / np.pi * 0.117729, -10.0]),10.0]))
act_Lj2.write(np.min([np.max([des_mL[1,0] * 180.0 / np.pi * 0.117679, -10.0]),10.0]))
time.sleep(0.004)
observation = np.array([[p_R[0,0] * 10 - 1.0, p_L[0,0] * 10 - 1.0, p_R[1,0], p_L[1,0],
((e_sensor -2.9440)/0.0148* np.pi / 180.0 - p_R[1,0]) , ((e_sensor -2.9440)/0.0148* np.pi / 180.0 - p_L[1,0]),
(new_goal - (e_sensor -2.9440)/0.0148 * np.pi / 180.0),
des_Fp_R[0,0] * 0.1, des_Fp_L[0,0] * 0.1,
0.0, 0.0, 0.0, 0.0,
self.stiffness, self.stiffness_lim]])
self.obs_dict = dict(observation = observation,
achieved_goal = np.array([[e_sensor * np.pi/ 180.0, 0.0, 0.0, 0.0, 0.0, des_Fp_R[0,0] * 0.1, des_Fp_L[0,0] * 0.1]]),
desired_goal = np.array([[new_goal, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]),
)
self.initial_o = self.obs_dict['observation']
self.initial_ag = self.obs_dict['achieved_goal']
self.g = self.obs_dict['desired_goal']
self.stiffness = 1.0
self.stiffness_lim = 1.0
def on_activate(self):
""" Required initialization steps when module is called."""
self.session = Session(bitfile=self.bitfile, resource=self.resource)
print(self.session._registers)
class Codec(object):
def __init__(self, key_manager, first_slot, is_fpga):
self.key_manager = key_manager
self.is_fpga = is_fpga
if self.is_fpga:
self.session = Session("real.lvbitx", "RIO0") #Session("real.lvbitx", "PXI1Slot2")
if(first_slot):
self.key = self.session.registers['Key1']
self.input = self.session.registers['Input1']
self.output = self.session.registers['Output1']
self.irq = 1
else:
self.key = self.session.registers['Key2']
self.input = self.session.registers['Input2']
self.output = self.session.registers['Output2']
self.irq = 2
def faster_slow_xor(self,aa,bb):
b = numpy.array(bb)
numpy.bitwise_xor(numpy.array(aa), b, b)
return b.tolist()
def xor(self, v1, v2):
result = []
if self.is_fpga:
self.key.write(v1)
self.input.write(v2)
irq_status = self.session.wait_on_irqs(self.irq, timeout_ms)
if self.irq in irq_status.irqs_asserted:
data = self.output.read()
return data
else:
return self.faster_slow_xor(v1, v2)
return None
def encode(self, data):
result = b""
key = self.key_manager.current_key()
if key is None:
return None
if len(data) > self.key_manager.buffer_size:
print "MORE"
while len(data) > 0:
count = self.key_manager.block_size if len(data) > self.key_manager.block_size else len(data)
if count + key.curpos > key.size:
key = self.key_manager.next_key()
if count > key.size:
count = key.size
for x in range(0, 8):
result += struct.pack('B', key.num[x])
result += struct.pack('I', key.curpos)
result += struct.pack('I', count)
towork = [ord(i) for i in data[:count]]
tokey = [key.key[i] for i in range(key.curpos, key.curpos+count)]
if count < self.key_manager.block_size:
for n in range(count, self.key_manager.block_size):
towork.append(0)
tokey.append(0)
for x in self.xor(towork, tokey)[:count]:
result += struct.pack('B', x)
#for x in range(0, count):
#print ord(data[x])
#print key.key[key.curpos+x]
#result += struct.pack('B', self.xor(ord(data[x]), key.key[key.curpos+x]))
key.curpos += count
data = data[count:]
return result
def decode(self, data):
result = b""
errors = 0
while len(data) > 16:
#print ""
#print len(data)
num = data[0:8]
curpos, = struct.unpack('I', data[8:12])
count, = struct.unpack('I', data[12:16])
key = self.key_manager.get_key_by_num(num)
if key is not None:
if count > len(data) - 16:
errors = 2
break
towork = [struct.unpack('B', data[i+16])[0] for i in range(0, count)]
tokey = [key.key[i] for i in range(curpos, curpos+count)]
if count < self.key_manager.block_size:
for n in range(count, self.key_manager.block_size):
towork.append(0)
tokey.append(0)
for x in self.xor(towork, tokey)[:count]:
result += chr(x)
#for x in range(0, count):
#val, = struct.unpack('B', data[x+16])
#result += chr(self.xor(val, key.key[curpos+x]))
#print chr(val ^ key.key[curpos+x])
#print key.key[curpos+x]
data = data[count+16:]
else:
errors = 1
data = b''
break
return {'result': result,'diff': len(data),'errors': errors}
write_values = {}
for i in range(1, write_count):
write_packet = write_packets['packet{}'.format(i)]
iteration_writes = {}
for j in range(write_packet.definition['channel_count']):
valuestr = input('{}: '.format(
write_packet.definition['name{}'.format(j)]))
channel_values = valuestr.split(',')
for k, value in enumerate(channel_values):
iteration_writes['{},{}'.format(
write_packet.definition['name{}'.format(j)], k)] = value
write_values['packet{}'.format(i)] = iteration_writes
device = input(
'Please input the name of your FPGA board as it appears in NI-MAX: ')
with Session(vsfpga.full_bitpath, device) as sesh:
read_fifo = sesh.fifos['DMA_READ']
write_fifo = sesh.fifos['DMA_WRITE']
loop_timer = sesh.registers['Loop Rate (usec)']
start = sesh.registers['Start']
rtsi = sesh.registers['Write to RTSI']
ex_timing = sesh.registers['Use External Timing']
irq = sesh.registers['Generate IRQ']
loop_timer.write(1000)
rtsi.write(False)
ex_timing.write(False)
irq.write(False)
start.write(True)
packed_reads = {}
def generate_rollouts(self):
"""Performs `rollout_batch_size` rollouts in parallel for time horizon `T` with the current
policy acting on it accordingly.
"""
self.reset_all_rollouts()
# compute observations
o = np.empty((self.rollout_batch_size, self.dims['o']), np.float32) # observations
ag = np.empty((self.rollout_batch_size, self.dims['g']), np.float32) # achieved goals
o[:] = self.initial_o
ag[:] = self.initial_ag
# generate episodes
obs, achieved_goals, acts, goals, successes, successes2 = [], [], [], [], [], []
dones = []
info_values = [np.empty((self.T-1, self.rollout_batch_size, self.dims['info_' + key]), np.float32) for key in self.info_keys]
Qs = []
Fs = []
Ks = []
with Session(bitfile="SCPC-lv-noFIFO_FPGATarget_FPGAmainepos_1XvgQEcJVeE.lvbitx", resource="rio://10.157.23.150/RIO0") as session:
act_Rj1=session.registers['Mod3/AO0']
enc_Rj1=session.registers['Rj1']
act_Rj2=session.registers['Mod3/AO1']
enc_Rj2=session.registers['Rj2']
act_Lj1=session.registers['Mod3/AO3']
enc_Lj1=session.registers['Lj1']
act_Lj2=session.registers['Mod3/AO4']
enc_Lj2=session.registers['Lj2']
sen_f=session.registers['fsensor']
sen_e=session.registers['fencoder']
emergency = False
Re1 = enc_Rj1.read()
Re2 = enc_Rj2.read()
Le1 = enc_Lj1.read()
Le2 = enc_Lj2.read()
f_sensor = 5.1203 * sen_f.read() - 5.2506
e_sensor = (((sen_e.read()) - (f_sensor / 100.0 * 0.15)) -2.9440)/0.0148
Rj = self.R_j_inv * self.R_e * np.array([[Re1-self.offset[0]],[-Re2+self.offset[1]]]) * np.pi/180.0
Lj = self.R_j_inv_L * self.R_e * np.array([[Le1-self.offset[2]],[Le2-self.offset[3]]]) * np.pi/180.0
Prev_Rj = Rj
Prev_Lj = Lj
xR = self.L1 * np.cos(Rj[0,0] + np.pi/2.0) + self.L2 * np.cos(Rj[0,0]-Rj[1,0] + np.pi/2.0)
yR = self.L1 * np.sin(Rj[0,0] + np.pi/2.0) + self.L2 * np.sin(Rj[0,0]-Rj[1,0] + np.pi/2.0)
xL = self.L1 * np.cos(Lj[0,0] + np.pi/2.0) + self.L2 * np.cos(Lj[0,0]+Lj[1,0] + np.pi/2.0)
yL = self.L1 * np.sin(Lj[0,0] + np.pi/2.0) + self.L2 * np.sin(Lj[0,0]+Lj[1,0] + np.pi/2.0)
P_R = np.array([xR, yR])
P_L = np.array([xL, yL])
Prel_R = self.Pc_R - P_R
Prel_L = self.Pc_L - P_L
l_R = np.sqrt(Prel_R[0]*Prel_R[0] + Prel_R[1]*Prel_R[1])
l_L = np.sqrt(Prel_L[0]*Prel_L[0] + Prel_L[1]*Prel_L[1])
p_R = np.array([[l_R],[np.arctan2(-Prel_R[1],-Prel_R[0])]])
p_L = np.array([[l_L],[np.arctan2(Prel_L[1],Prel_L[0])]])
for t in range(self.T):
policy_output = self.policy.get_actions(
o, ag, self.g,
compute_Q=self.compute_Q,
noise_eps=self.noise_eps if not self.exploit else 0.,
random_eps=self.random_eps if not self.exploit else 0.,
use_target_net=self.use_target_net)
if self.compute_Q:
u, Q = policy_output
Qs.append(Q)
Fs.append(f_sensor)
Ks.append(self.stiffness)
else:
u = policy_output
if u.ndim == 1:
# The non-batched case should still have a reasonable shape.
u = u.reshape(1, -1)
o_new = np.empty((self.rollout_batch_size, self.dims['o']))
ag_new = np.empty((self.rollout_batch_size, self.dims['g']))
success = np.zeros(self.rollout_batch_size)
success2 = np.zeros(self.rollout_batch_size)
# compute new states and observations
self.stiffness_lim = np.clip(self.stiffness_lim + 0.2 * u[0][3], 0.1, 1.0)
self.stiffness = np.clip(self.stiffness + 0.2 * u[0][2], 0, self.stiffness_lim)
u[0][0] = np.clip(u[0][0], -self.l_step_limit*14.0, self.l_step_limit*14.0)
u[0][1] = np.clip(u[0][1], -8.0*self.th_step_limit*2.0, self.th_step_limit*2.0)
if emergency == False:
vel_R = Rj - Prev_Rj
vel_L = Lj - Prev_Lj
if vel_R[0,0] > self.vel_limit or vel_R[0,0] < -self.vel_limit or vel_L[0,0] > self.vel_limit or vel_L[0,0] < -self.vel_limit:
emergency = True
r = np.array([[self.stiffness], [1.0]]) * 0.5
print("***************Robot going insane! Safety on!***************")
else:
r = np.array([[self.stiffness], [1.0]])
else:
r = np.array([[self.stiffness], [1.0]]) * 0.5
des_p_R = np.array([[np.min([np.max([p_R[0,0] + u[0][0]/14.0, -self.l_limit/2.0]), self.l_limit/4.0])], [np.min([np.max([p_R[1,0] + u[0][1]/2.0, -self.th_limit]), self.th_limit])]])
des_p_L = np.array([[np.min([np.max([p_L[0,0] + u[0][0]/14.0, -self.l_limit/2.0]), self.l_limit/4.0])], [np.min([np.max([p_L[1,0] + u[0][1]/2.0, -self.th_limit]), self.th_limit])]])
Jp_R = np.matrix([[-Prel_R[0]/l_R, -Prel_R[1]/l_R],[Prel_R[1]/l_R/l_R, -Prel_R[0]/l_R/l_R]])
Jp_L = np.matrix([[-Prel_L[0]/l_L, -Prel_L[1]/l_L],[Prel_L[1]/l_L/l_L, -Prel_L[0]/l_L/l_L]])
Jp_inv_R = np.matrix([[Jp_R[1,1] / (Jp_R[0,0]*Jp_R[1,1] - Jp_R[0,1]*Jp_R[1,0]), -Jp_R[0,1] / (Jp_R[0,0]*Jp_R[1,1] - Jp_R[0,1]*Jp_R[1,0])], [-Jp_R[1,0] / (Jp_R[0,0]*Jp_R[1,1] - Jp_R[0,1]*Jp_R[1,0]), Jp_R[0,0] / (Jp_R[0,0]*Jp_R[1,1] - Jp_R[0,1]*Jp_R[1,0])]])
Jp_inv_L = np.matrix([[Jp_L[1,1] / (Jp_L[0,0]*Jp_L[1,1] - Jp_L[0,1]*Jp_L[1,0]), -Jp_L[0,1] / (Jp_L[0,0]*Jp_L[1,1] - Jp_L[0,1]*Jp_L[1,0])], [-Jp_L[1,0] / (Jp_L[0,0]*Jp_L[1,1] - Jp_L[0,1]*Jp_L[1,0]), Jp_L[0,0] / (Jp_L[0,0]*Jp_L[1,1] - Jp_L[0,1]*Jp_L[1,0])]])
J_R = np.matrix([[-yR, self.L2 * np.cos(Rj[0,0]-Rj[1,0])],
[xR, self.L2 * np.sin(Rj[0,0]-Rj[1,0])]])
J_L = np.matrix([[-yL, -self.L2 * np.cos(Lj[0,0]+Lj[1,0])],
[xL, -self.L2 * np.sin(Lj[0,0]+Lj[1,0])]])
J_inv_R = np.matrix([[J_R[1,1] / (J_R[0,0]*J_R[1,1] - J_R[0,1]*J_R[1,0]), -J_R[0,1] / (J_R[0,0]*J_R[1,1] - J_R[0,1]*J_R[1,0])], [-J_R[1,0] / (J_R[0,0]*J_R[1,1] - J_R[0,1]*J_R[1,0]), J_R[0,0] / (J_R[0,0]*J_R[1,1] - J_R[0,1]*J_R[1,0])]])
J_inv_L = np.matrix([[J_L[1,1] / (J_L[0,0]*J_L[1,1] - J_L[0,1]*J_L[1,0]), -J_L[0,1] / (J_L[0,0]*J_L[1,1] - J_L[0,1]*J_L[1,0])], [-J_L[1,0] / (J_L[0,0]*J_L[1,1] - J_L[0,1]*J_L[1,0]), J_L[0,0] / (J_L[0,0]*J_L[1,1] - J_L[0,1]*J_L[1,0])]])
max_kj_R = np.transpose(self.R_j) * np.matrix([[2*self.Ksc, 0],[0, 2*self.Ksc]]) * self.R_j
max_kj_L = np.transpose(self.R_j_L) * np.matrix([[2*self.Ksc, 0],[0, 2*self.Ksc]]) * self.R_j_L
max_k_R = np.transpose(J_inv_R) * max_kj_R * J_inv_R
max_k_L = np.transpose(J_inv_L) * max_kj_L * J_inv_L
max_kp_R = np.transpose(Jp_inv_R) * max_k_R * Jp_inv_R
max_kp_L = np.transpose(Jp_inv_L) * max_k_L * Jp_inv_L
max_kp_R[0,1] = 0.0
max_kp_R[1,0] = 0.0
max_kp_L[0,1] = 0.0
max_kp_L[1,0] = 0.0
des_Fp_R = max_kp_R * (r * (des_p_R - p_R)) * 0.9
des_Fp_L = max_kp_L * (r * (des_p_L - p_L)) * 0.9
des_F_R = np.transpose(Jp_R) * des_Fp_R
des_F_L = np.transpose(Jp_L) * des_Fp_L
des_tau_R = np.transpose(J_R) * des_F_R
des_tau_L = np.transpose(J_L) * des_F_L
if Rj[1,0] > -0.2: des_tau_R += np.array([[0.0],[-0.05]])
if Lj[1,0] > -0.2: des_tau_L += np.array([[0.0],[-0.05]])
if Rj[1,0] < -1.8: des_tau_R += np.array([[0.0],[0.05]])
if Lj[1,0] < -1.8: des_tau_L += np.array([[0.0],[0.05]])
if Rj[0,0] > 0: des_tau_R += np.array([[-0.05],[0.0]])
if Lj[0,0] < 0: des_tau_L += np.array([[0.05],[0.0]])
if Rj[0,0] < -0.8: des_tau_R += np.array([[0.05],[0.0]])
if Lj[0,0] > 0.8: des_tau_L += np.array([[-0.05],[0.0]])
des_mR = (np.transpose(self.R_j_inv)*des_tau_R / (2*self.Ksc) + self.R_j * Rj) / self.Rm
des_mL = (np.transpose(self.R_j_inv_L)*des_tau_L / (2*self.Ksc) + self.R_j_L * Lj) / self.Rm
act_Rj1.write(np.min([np.max([des_mR[0,0] * 180.0 / np.pi * 0.117258, -10.0]),10.0]))
act_Rj2.write(np.min([np.max([des_mR[1,0] * 180.0 / np.pi * 0.117541, -10.0]),10.0]))
act_Lj1.write(np.min([np.max([des_mL[0,0] * 180.0 / np.pi * 0.117729, -10.0]),10.0]))
act_Lj2.write(np.min([np.max([des_mL[1,0] * 180.0 / np.pi * 0.117679, -10.0]),10.0]))
time.sleep(0.004)
Re1 = enc_Rj1.read()
Re2 = enc_Rj2.read()
Le1 = enc_Lj1.read()
Le2 = enc_Lj2.read()
f_sensor = 5.1203 * sen_f.read() - 5.2506
e_sensor = (((sen_e.read()) - (f_sensor / 100.0 * 0.15)) -2.9440)/0.0148
Prev_Rj = Rj
Prev_Lj = Lj
Rj = self.R_j_inv * self.R_e * np.array([[Re1-self.offset[0]],[-Re2+self.offset[1]]]) * np.pi/180.0
Lj = self.R_j_inv_L * self.R_e * np.array([[Le1-self.offset[2]],[Le2-self.offset[3]]]) * np.pi/180.0
xR = self.L1 * np.cos(Rj[0,0] + np.pi/2.0) + self.L2 * np.cos(Rj[0,0]-Rj[1,0] + np.pi/2.0)
yR = self.L1 * np.sin(Rj[0,0] + np.pi/2.0) + self.L2 * np.sin(Rj[0,0]-Rj[1,0] + np.pi/2.0)
xL = self.L1 * np.cos(Lj[0,0] + np.pi/2.0) + self.L2 * np.cos(Lj[0,0]+Lj[1,0] + np.pi/2.0)
yL = self.L1 * np.sin(Lj[0,0] + np.pi/2.0) + self.L2 * np.sin(Lj[0,0]+Lj[1,0] + np.pi/2.0)
P_R = np.array([xR, yR])
P_L = np.array([xL, yL])
Prel_R = self.Pc_R - P_R
Prel_L = self.Pc_L - P_L
l_R = np.sqrt(Prel_R[0]*Prel_R[0] + Prel_R[1]*Prel_R[1])
l_L = np.sqrt(Prel_L[0]*Prel_L[0] + Prel_L[1]*Prel_L[1])
p_R = np.array([[l_R],[np.arctan2(-Prel_R[1],-Prel_R[0])]])
p_L = np.array([[l_L],[np.arctan2(Prel_L[1],Prel_L[0])]])
observation = np.array([[p_R[0,0] * 10 - 1.0, p_L[0,0] * 10 - 1.0, p_R[1,0], p_L[1,0],
((e_sensor -2.9440)/0.0148* np.pi / 180.0 - p_R[1,0]) , ((e_sensor -2.9440)/0.0148* np.pi / 180.0 - p_L[1,0]),
(self.g[0][0] - (e_sensor -2.9440)/0.0148 * np.pi / 180.0),
des_Fp_R[0,0] * 0.1, des_Fp_L[0,0] * 0.1,
vel_R[0,0], vel_R[1,0], vel_L[0,0], vel_L[1,0],
self.stiffness, self.stiffness_lim]])
obs_dict_new = dict(observation=observation,
achieved_goal=np.array([[((e_sensor -2.9440)/0.0148) * np.pi / 180.0, vel_R[0,0], vel_R[1,0], vel_L[0,0], vel_L[1,0], des_Fp_R[0,0] * 0.1, des_Fp_L[0,0] * 0.1]]),
desired_goal = self.g)
done = [False] if t < self.T-1 else [True]
info = [{
'is_success': self._is_success(obs_dict_new['achieved_goal'][0], obs_dict_new['desired_goal'][0]),
}]
o_new = obs_dict_new['observation']
ag_new = obs_dict_new['achieved_goal']
success = np.array([i.get('is_success', 0.0) for i in info])
success2 = (np.float32(f_sensor < self.object_fragility))
if any(done):
# here we assume all environments are done is ~same number of steps, so we terminate rollouts whenever any of the envs returns done
# trick with using vecenvs is not to add the obs from the environments that are "done", because those are already observations
# after a reset
break
for i, info_dict in enumerate(info):
for idx, key in enumerate(self.info_keys):
# print(info_values[idx][t, i])
# print(info[i][key])
# print(info_values)
# print(info)
info_values[idx][t, i] = info[i][key]
# if np.isnan(o_new).any():
# self.logger.warn('NaN caught during rollout generation. Trying again...')
# self.reset_all_rollouts()
# return self.generate_rollouts()
dones.append(done)
obs.append(o.copy())
achieved_goals.append(ag.copy())
successes.append(success.copy())
successes2.append(success2.copy())
# print(o.copy())
# print(o_new)
# successes3.append(success3.copy())
acts.append(u.copy())
goals.append(self.g.copy())
o[...] = o_new
ag[...] = ag_new
# print("--------------------New Rollout--------------------")
obs.append(o.copy())
achieved_goals.append(ag.copy())
episode = dict(o=obs,
u=acts,
g=goals,
ag=achieved_goals)
for key, value in zip(self.info_keys, info_values):
episode['info_{}'.format(key)] = value
# stats
successful = np.array(successes)[-1, :]
successful2 = np.array(successes2)
assert successful.shape == (self.rollout_batch_size,)
success_rate = np.mean(successful)
success_rate2 = np.mean(successful2.mean(axis=0))
success_rate3 = np.mean(successful2.min(axis=0) * successful)
self.success_history.append(success_rate)
self.success_history2.append(success_rate2)
self.success_history3.append(success_rate3)
if self.compute_Q:
self.Q_history.append(np.mean(Qs))
self.F_history.append(np.mean(Fs))
self.K_history.append(np.mean(Ks))
self.n_episodes += self.rollout_batch_size
return convert_episode_to_batch_major(episode)
import os
import sys
import numpy as np
from nifpga import Session
from Calibration.Encoder import *
from ProofSignals.Signals import *
with Session("Mybitfile.lvbitx", "rio://172.22.11.2/RIO0") as Session:
Session.reset()
Session.run()
qd = Session.registers['qd']
qr = Session.registers['qr']
ResetPID = Session.registers['Reset PID']
ResetEncoder = Session.registers['Reset Enc']
PythonMode = Session.registers['Python Mode']
# PID Gains for Python Mode
P = Session.registers['P']
I = Session.registers['I']
D = Session.registers['D']
# Gains
PythonMode.write(True)
P.write(np.uint16(15))
I.write(np.uint16(0))
D.write(np.uint16(8))
print("Simple Pendulum by means an FPGA")
class VeriStandFPGA(object):
"""
DMA FIFO info pulled from an .fpgaconfig file
config_file is the file name of the XML that defines this FIFO
Direction is a string stating read or write.
"""
def __init__(self, filepath):
"""
Create a fpga_config object. The filepath is the path to the bitfile you wish to interact with.
This creates the fpga configuration with a number of read and write packets.
:param filepath:
"""
self.filepath = filepath
self.tree = ET.parse(self.filepath)
self.root = self.tree.getroot()
self.read_packets = None
self.write_packets = None
self.session = None
self.write_fifo_object = None
self.read_fifo_object = None
for child in self.root:
if child.tag == 'DMA_Read':
self.read_packets = int(child[0].text)
self.read_fifo_tag = child
elif child.tag == 'DMA_Write':
self.write_packets = int(child[0].text)
self.write_fifo_tag = child
elif child.tag == 'Bitfile':
self.bitfile = child.text
self.folder = ntpath.split(self.filepath)
self.full_bitpath = self.folder[0] + '\\{}'.format(self.bitfile)
if self.read_packets is None:
raise ConfigError(message='No DMA_Read tag present')
elif self.write_packets is None:
raise ConfigError(message='No DMA_Write tag present')
self.read_packet_list = []
self.write_packet_list = []
self.channel_value_table = {}
for pack_index in range(self.read_packets):
self.read_packet_list.append(
self._create_packet('read', pack_index + 1))
for chan_index in range(self.read_packet_list[pack_index].
definition['channel_count']):
self.channel_value_table[self.read_packet_list[
pack_index].definition['name{}'.format(chan_index)]] = 0
for pack_index in range(self.write_packets):
self.write_packet_list.append(
self._create_packet('write', pack_index + 1))
for chan_index in range(self.write_packet_list[pack_index].
definition['channel_count']):
self.channel_value_table[self.write_packet_list[
pack_index].definition['name{}'.format(chan_index)]] = 0
def init_fpga(self, device, loop_rate):
self.session = Session(self.full_bitpath,
device,
reset_if_last_session_on_exit=True)
self.session.download()
self.session.run()
print(self.session.fpga_vi_state)
self.write_fifo_object = self.session.fifos['DMA_WRITE']
self.read_fifo_object = self.session.fifos['DMA_READ']
self.loop_timer = self.session.registers['Loop Rate (usec)']
self.fpga_start_control = self.session.registers['Start']
self.fpga_rtsi = self.session.registers['Write to RTSI']
self.fpga_ex_timing = self.session.registers['Use External Timing']
self.fpga_irq = self.session.registers['Generate IRQ']
self.loop_timer.write(loop_rate)
self.fpga_rtsi.write(False)
self.fpga_ex_timing.write(False)
self.fpga_irq.write(False)
def start_fpga_main_loop(self):
self.fpga_start_control.write(True)
def stop_fpga(self):
self.session.reset()
self.session.close()
def set_channel(self, channel_name, value):
self.channel_value_table[channel_name] = value
def get_channel(self, channel_name):
return self.channel_value_table[channel_name]
def vs_read_fifo(self, timeout):
if self.read_fifo_object is None:
raise ConfigError('Session not initialized. Please first call the'
' VeriStandFPGA.init fpga method before reading')
else:
read_tup = self.read_fifo_object.read(
number_of_elements=self.read_packets, timeout_ms=timeout)
data = read_tup.data
for i, u64 in enumerate(data):
this_packet = self.read_packet_list[i]
read_vals = this_packet._unpack(u64)
for key in read_vals:
self.channel_value_table[key] = read_vals[key]
def vs_write_fifo(self, timeout):
if self.write_fifo_object is None:
raise ConfigError(
'Session not initialized. '
'Please first call the VeriStandFPGA.init_fpga method before writing'
)
else:
write_list = []
for current_packet in self.write_packet_list:
packet_vals = []
for channel in current_packet:
packet_vals.append(
self.channel_value_table[channel['name']])
write_list.append(current_packet._pack(packet_vals))
self.write_fifo_object.write(data=write_list, timeout_ms=timeout)
def _create_packet(self, direction, index):
"""
:param direction:
:param index:
:return:
"""
if index == 1 and direction.lower() == 'read':
this_packet = FirstReadPacket()
else:
this_packet = Packet(self, direction, index)
return this_packet
def __del__(self):
try:
self.stop_fpga()
except:
print('Configuration closed')
print('FPGA hardware session closed.')
print('{} configuration closed'.format(self.bitfile))