Source code for pyEDScorbotTool.pyAER
from datetime import time
from os import read
import os
import sys
import tkinter as tk
from tkinter import ttk,messagebox
from tkinter.ttk import Progressbar
from tkinter.constants import X
from tkinter import simpledialog
# import usb.core
# import usb.util
# import usb.backend.libusb1
import json
from tkinter import filedialog
import datetime
import time
import logging
import numpy as np
import pickle as P
import paho.mqtt.client as mqtt
import subprocess
import matplotlib.pyplot as plt
#from visualization import *
from tkinter import scrolledtext
from .utils.transformations import count_to_angle as c_to_a
from .utils.transformations import cont_to_xyz as c_to_xyz
from .utils.transformations import angles_to_json as a_to_j
from .utils.transformations import angles_to_refs as a_to_r
from .utils.transformations import angles_to_xyz as a_to_xyz
from .utils.transformations import pad_trajectory
from .utils.transformations import omegas_to_angles as w_to_a
from .utils.visualization.xyz import plot3d
from .utils.visualization.angles import plotangles
from .utils.visualization.counters import plotcounters
import pandas
def on_connect(client, userdata, flags, rc):
global traj_name
global n
print("Connected with result code "+str(rc))
# Subscribing in on_connect() means that if we lose the connection and
# reconnect then subscriptions will be renewed.
client.subscribe("EDScorbot/trajectory")
topic = "/EDScorbot/commands"
# The callback for when a PUBLISH message is received from the server.
def on_message(client, userdata, msg):
parsed = msg.payload.decode('utf8').lstrip('[').rstrip(']').split(',')
#print(parsed)
#t.update()
#global i
#i+=1
j1 = int(parsed[0])
j2 = int(parsed[1])
j3 = int(parsed[2])
j4 = int(parsed[3])
j5 = int(parsed[4])
j6 = int(parsed[5])
ts = int(parsed[6])
iter = int(parsed[7])
userdata['pos_data'].append([j1,j2,j3,j4,j5,j6,ts])
if int(iter) < 0:
arr = np.array(userdata['pos_data'])
userdata['pos_data'] = []
#name of folder in localhost where to save the data
savename = filedialog.asksaveasfilename()
np.save(savename,arr[:-1])
#np.save("output_data.npy",arr[:-1])
userdata['progressbar'].stop()
#name of ouput file in remote server
out_fname = userdata['filename'][:-5] + "_out_cont.json"
cmd = "scp -i /media/HDD/home/enrique/Proyectos/SMALL/zynq/zynq root@192.168.1.115:/home/root/refs_out_cont.json {}".format(os.path.join(savename,"out_cont.json"))
#cmd = "scp -i /media/HDD/home/enrique/Proyectos/SMALL/zynq/zynq {} root@192.168.1.115:/home/root/{}".format(filename.name,real_name)
os.system(cmd)
#sys.exit()
if userdata['visible'] == True and iter > 0:
userdata['textbox'].insert(tk.END,msg.topic+" "+str(msg.payload) + "\n")
userdata['progressbar'].step()
print(msg.topic+" "+str(msg.payload))
[docs]class pyEDScorbotTool:
'''
py-EDScorbotTool software, replacement of jAER filter for EDScorbot
This class is used for establishing a communication with ED-Scorbot
Robot in order to be able to control it via neuromorphic control, also called SPID
:ivar self.d: Dictionary in which there are stored the variables that allow for SPID configuration. Every input of the graphical interface corresponds to a variable that is stored in this dictionary. It contains three other dictionaries: Motor Config, Joints and Scan Parameters, which in turn hold the corresponding variables. The keys for the dictionaries are "Motor Config", "Joints" and "Scan Parameters", respectively.
:ivar self.visible: Boolean variable that indicates whether the graphical interface should be rendered or not
:ivar self.root: Root of the graphical interface's window
:ivar self.checked_usb: Variable that holds the state of the checkbox that indicates whether USB is enabled or not.
'''
def __init__(self,visible=True):
'''
Constructor
Initializes GUI by creating the root Tk object, sets icon of the app,
initializes data structures to hold the value of all variables that
are displayed, creating a dictionary (self.d) to access their values
and sets constants and handles needed for USB connection
'''
#Initialize GUI: create root Tk object
self.visible = visible
self.root = tk.Tk()
if self.visible:
#self.root = tk.Tk()
#Set the icon
self.root.iconphoto(False,tk.PhotoImage(file="atc.png"))
else:
self.root.withdraw()
#Create dictionaries where the interface data will be stored
self.d = {}
self.d["Motor Config"] = {}
self.d["Joints"] = {}
self.d["Scan Parameters"] = {}
self.d["Dynapse2"] = {}
self.j1 = -1
self.j2 = -1
self.j3 = -1
self.j4 = -1
self.j5 = -1
self.j6 = -1
#Standalone variable to control if USB is enabled
self.checked_usb = False
#Set USB constants needed
self.VID = 0x10c4
self.PID = 0x0000
self.ENDPOINT_OUT = 0x02
self.ENDPOINT_IN = 0x81
self.PACKET_LENGTH = 64
self.filename = ""
#Handle for USB connection
self.dev = None
#Array for storing data
self.array = []
#Variable to record data or not
self.record = False
#Variable to check if we keep updating the recordings
self.updating = True
logging.basicConfig(filemode='w',level=logging.INFO)
#Progress bar
#self.self.pb = None
return
[docs] def millis_now(self):
'''
This function returns the time at the moment of the call in milliseconds
'''
return (time.time()*1000) #time.time() returns seconds, so mult. by 1000 to get ms
[docs] def alert(self,text):
'''
This function creates a messagebox with the text parameter as data
Useful to alert the user that something has gone wrong;
to indicate successful procedures, please just print to console using print()
Args:
text (str): Text to be displayed in the box
'''
if self.visible:
messagebox.showinfo(message=text)
else:
print(text)
[docs] def render_motor(self, motor_number, row, col):
'''
Create the inputs for 1 motor
Each variable created is stored in the "Motor Config" dictionary
and can be accessed directly using the name that appears on the graphical interface
Args:
motor_number (int): Number of the motor to be rendered
row (int): Row of the grid in which the inputs will be displayed
col (int): Column of the grid in which the inputs will be displayed
Returns:
Labelframe in which the inputs are rendered
'''
labels = ["EI_FD_bank3_18bits", "PD_FD_bank3_22bits",
"PI_FD_bank3_18bits", "leds", "ref", "spike_expansor","ref_freq_divider"]
labelframe = None
labelframe = ttk.LabelFrame(
self.root, text="Motor " + str(motor_number))
labelframe.grid(
column=col, row=row, sticky=(tk.N, tk.W), padx=5, pady=5)
EI_FD_bank3_18bits = tk.IntVar()
PF_FD_bank3_22bits = tk.IntVar()
PI_FD_bank3_18bits = tk.IntVar()
leds = tk.IntVar()
ref = tk.IntVar()
spike_expansor = tk.IntVar()
spike_freq_divider = tk.IntVar()
variables = [EI_FD_bank3_18bits, PF_FD_bank3_22bits,
PI_FD_bank3_18bits, leds, ref, spike_expansor, spike_freq_divider,labelframe]
for var, row_, label in zip(variables, range(1, len(variables)), labels):
labeltext = label + "_M" + str(motor_number)
self.d["Motor Config"][labeltext] = var
if self.visible:
ttk.Entry(labelframe, width=7, textvariable=var).grid(
column=2, row=row_, sticky=(tk.W))
ttk.Label(labelframe, text=labeltext).grid(
column=1, row=row_, sticky=(tk.W))
# ttk.Scale(labelframe,from_=0,to=65535,orient=tk.HORIZONTAL,variable=var).grid(column=3,row=row,sticky=(tk.W))
return labelframe
[docs] def render_joints(self, row, col):
'''
Create joints text entries
These entries are read-only, as they will contain the joints measures read from the encoders
Each variable created is stored in the "Joints" dictionary
and can be accessed directly using the name that appears on the graphical interface
Args:
row (int): Row of the grid in which the inputs will be displayed
col (int): Column of the grid in which the inputs will be displayed
Returns:
LabelFrame in which the entries are rendered
'''
labelframe=None
if self.visible:
labelframe = ttk.LabelFrame(self.root, text="Joint Sensors")
labelframe.grid(column=col, row=row, sticky=(
tk.N, tk.W), padx=5, pady=5)
j1 = tk.IntVar()
j2 = tk.IntVar()
j3 = tk.IntVar()
j4 = tk.IntVar()
j5 = tk.IntVar()
j6 = tk.IntVar()
j1_hex = tk.DoubleVar()
j2_hex = tk.DoubleVar()
j3_hex = tk.DoubleVar()
j4_hex = tk.DoubleVar()
j5_hex = tk.DoubleVar()
j6_hex = tk.DoubleVar()
variables = [j1, j2, j3, j4, j5, j6]
variables_hex = [j1_hex,j2_hex,j3_hex,j4_hex,j5_hex,j6_hex]
i = 1
for var,var_hex, row_ in zip(variables,variables_hex, range(1, len(variables)+1)):
labeltext = "J" + str(i) + "_sensor_value"
labeltext_hex = "J" + str(i) + "_sensor_value_hex"
self.d["Joints"][labeltext] = var
self.d["Joints"][labeltext_hex] = var_hex
if self.visible:
text = ttk.Entry(labelframe, width=7, textvariable=var)
text.grid(column=2, row=row_, sticky=tk.W)
text.config(state="readonly")
ttk.Label(labelframe, text=labeltext).grid(
column=1, row=row_, sticky=tk.W)
text_hex = ttk.Entry(labelframe,width=7,textvariable=var_hex)
text_hex.grid(column=3,row=row_)
text_hex.config(state="readonly")
i += 1
xyz_row = len(variables)+1
xyz = tk.StringVar()
text = ttk.Entry(labelframe, width=7, textvariable=xyz)
text.grid(column=2, row=xyz_row, sticky=tk.W)
text.config(state="readonly")
ttk.Label(labelframe, text="XYZ coordinates").grid(
column=1, row=xyz_row, sticky=tk.W)
self.d["Joints"]["XYZ coordinates"] = xyz
return labelframe
[docs] def render_scan_parameters(self, row, col):
'''
Create the Scan Parameters inputs
Each variable created is stored in the "Scan Parameters" dictionary
and can be accessed directly using the name that appears on the graphical interface
Args:
row (int): Row of the grid in which the inputs will be displayed
col (int): Column of the grid in which the inputs will be displayed
Returns:
Labelframe in which the inputs are rendered
'''
labels = ["Final_Value", "Init_Value", "Step_Value", "Wait_Time"]
labelframe=None
if self.visible:
labelframe = ttk.LabelFrame(self.root, text="Scan Parameters")
labelframe.grid(column=col, row=row, sticky=(
tk.N, tk.W), padx=5, pady=5)
final_value = tk.IntVar()
init_value = tk.IntVar()
step_value = tk.IntVar()
wait_time = tk.IntVar()
variables = [final_value, init_value, step_value, wait_time]
for var, row_, label in zip(variables, range(1, len(variables)+1), labels):
labeltext = "scan_" + label
self.d["Scan Parameters"][labeltext] = var
if self.visible:
ttk.Entry(labelframe, width=7, textvariable=var).grid(
column=2, row=row_, sticky=tk.W)
ttk.Label(labelframe, text=labeltext).grid(
column=1, row=row_, sticky=tk.W)
return labelframe
[docs] def render_dynapse2(self, row, col):
'''
Create the Threshold and Reset counters paramters
Each variable created is stored in the "Dynapse2" dictionary
and can be accessed directly using the name that appears on the graphical interface
Args:
row (int): Row of the grid in which the inputs will be displayed
col (int): Column of the grid in which the inputs will be displayed
Returns:
Labelframe in which the inputs are rendered
'''
labels = ["Threshold1", "Reset1", "Threshold2", "Reset2"]
labelframe=None
if self.visible:
labelframe = ttk.LabelFrame(self.root, text="Dynapse2")
labelframe.grid(column=col, row=row, sticky=(
tk.N, tk.W), padx=5, pady=5)
th1_value = tk.IntVar()
rst1_value = tk.IntVar()
th2_value = tk.IntVar()
rst2_time = tk.IntVar()
variables = [th1_value, rst1_value, th2_value, rst2_time]
for var, row_, label in zip(variables, range(1, len(variables)+1), labels):
labeltext = label
self.d["Dynapse2"][labeltext] = var
if self.visible:
ttk.Entry(labelframe, width=7, textvariable=var).grid(
column=2, row=row_, sticky=tk.W)
ttk.Label(labelframe, text=labeltext).grid(
column=1, row=row_, sticky=tk.W)
return labelframe
[docs] def render_buttons(self, row, col):
'''
Create the buttons that will be bounded to all different usable actions
Args:
row (int): Row of the grid in which the buttons will be displayed
col (int): Column of the grid in which the buttons will be displayed
'''
if self.visible:
labelframe = ttk.LabelFrame(self.root, text="Actions")
labelframe.grid(column=col, row=row, sticky=(
tk.N, tk.W), padx=5, pady=5,rowspan=2)
ttk.Button(labelframe,text="Angles to JSON",command=self.traj_to_json).grid(row=1,column=1,sticky=(tk.W,tk.E))
ttk.Button(labelframe,text="W to Angles",command=self.w_to_angles).grid(row=2,column=1,sticky=(tk.W,tk.E))
ttk.Button(labelframe,text="Angles to XYZ",command=self.angles_to_xyz).grid(row=3,column=1,sticky=(tk.W,tk.E))
ttk.Button(labelframe,text="Count to XYZ (from npy)",command=self.count_to_xyz_npy).grid(row=4,column=1,sticky=(tk.W,tk.E))
ttk.Button(labelframe,text="Count to angles (from npy)",command=self.count_to_angles_npy).grid(row=5,column=1,sticky=(tk.W,tk.E))
ttk.Button(labelframe,text="Count to XYZ (from json)",command=self.count_to_xyz_json).grid(row=6,column=1,sticky=(tk.W,tk.E))
ttk.Button(labelframe,text="Count to angles (from json)",command=self.count_to_angles_json).grid(row=7,column=1,sticky=(tk.W,tk.E))
ttk.Button(labelframe,text="Send Trajectory",command=self.send_trajectory).grid(row=8,column=1,sticky=(tk.W,tk.E))
ttk.Button(labelframe,text="Plot 3D trajectory",command=self.plot_traj_3d).grid(row=9,column=1,sticky=(tk.W,tk.E))
ttk.Button(labelframe,text="Plot counters",command=self.plot_counters).grid(row=10,column=1,sticky=(tk.W,tk.E))
ttk.Button(labelframe,text="Plot angles",command=self.plot_angles).grid(row=1,column=2,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="ScanMotor6",command=self.scanMotor6).grid(row=2,column=2,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Search_Home",command=self.search_Home_all).grid(row=3,column=2,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send_Home",command=self.send_Home_all).grid(row=4,column=2,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="SendFPGAReset",command=self.SendFPGAReset).grid(row=5,column=2,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="SetAERIN_ref",command=self.SetAERIN_ref).grid(row=6,column=2,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="SetUSBSPI_ref",command=self.SetUSBSPI_ref).grid(row=7,column=2,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="SwitchOffLEDS",command=self.SwitchOffLEDS).grid(row=8,column=2,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="DumpConfig",command=self.dumpConfig).grid(row=9,column=2,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="LoadConfig",command=self.loadConfig).grid(row=10,column=2,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="ResetUSB",command=self.resetUSB).grid(row=1,column=3,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J1 Home",command=self.send_Home_J1).grid(row=2,column=3,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J2 Home",command=self.send_Home_J2).grid(row=3,column=3,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J3 Home",command=self.send_Home_J3).grid(row=4,column=3,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J4 Home",command=self.send_Home_J4).grid(row=5,column=3,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J5 Home",command=self.send_Home_J5).grid(row=6,column=3,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J6 Home",command=self.send_Home_J6).grid(row=7,column=3,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Search J1 Home",command=self.search_Home_J1).grid(row=8,column=3,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Search J2 Home",command=self.search_Home_J2).grid(row=9,column=3,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Search J3 Home",command=self.search_Home_J3).grid(row=10,column=3,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Search J4 Home",command=self.search_Home_J4).grid(row=1,column=4,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Search J5 Home",command=self.search_Home_J5).grid(row=2,column=4,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Search J6 Home",command=self.search_Home_J6).grid(row=3,column=4,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J1 ref",command=self.SendCommandJoint1_lite).grid(row=4,column=4,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J2 ref",command=self.SendCommandJoint2_lite).grid(row=5,column=4,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J3 ref",command=self.SendCommandJoint3_lite).grid(row=6,column=4,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J4 ref",command=self.SendCommandJoint4_lite).grid(row=7,column=4,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J5 ref",command=self.SendCommandJoint5_lite).grid(row=8,column=4,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Send J6 ref",command=self.SendCommandJoint6_lite).grid(row=9,column=4,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J1 counter",command=self.Reset_J1_pos).grid(row=10,column=4,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J2 counter",command=self.Reset_J2_pos).grid(row=1,column=5,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J3 counter",command=self.Reset_J3_pos).grid(row=2,column=5,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J4 counter",command=self.Reset_J4_pos).grid(row=3,column=5,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J5 counter",command=self.Reset_J5_pos).grid(row=4,column=5,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J6 counter",command=self.Reset_J6_pos).grid(row=5,column=5,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Start/Stop recording",command=self.toggle_record).grid(row=6,column=5,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="DYNAPSE2",command=self.send_dynapse2).grid(row=7,column=5,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J1 SPID",command=self.sendJ1FPGAReset).grid(row=8,column=5,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J2 SPID",command=self.sendJ2FPGAReset).grid(row=9,column=5,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J3 SPID",command=self.sendJ3FPGAReset).grid(row=10,column=5,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J4 SPID",command=self.sendJ4FPGAReset).grid(row=1,column=6,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J5 SPID",command=self.sendJ5FPGAReset).grid(row=2,column=6,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Reset J6 SPID",command=self.sendJ6FPGAReset).grid(row=3,column=6,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Trajectory",command=self.send_trajectory).grid(row=4,column=6,sticky=(tk.W,tk.E))
# ttk.Button(labelframe,text="Plot data",command=self.plot_data).grid(row=5,column=6,sticky=(tk.W,tk.E))
def plot_data(self):
filename = filedialog.askopenfile(mode="r")
#check box for 2d or 3d
data = np.load(filename.name,allow_pickle=True)
df = pandas.DataFrame(data)
self.plot(df,2)
def plot(self,data,dim):
if dim == 2:
#plot normally
plt.plot(data[0][:-1])
plt.show()
pass
elif dim == 3:
plot3d(data[0],data[1],data[2])
#plot3d()
pass
else:
#give error/return
pass
pass
def send_trajectory(self):
if self.checked_remote.get() == False:
self.alert("Remote mode must be activated")
return
filename = filedialog.askopenfile(mode="r")
real_name = filename.name.split("/")[-1]
n = simpledialog.askinteger("Trajectory sender","Number of points of trajectory (integer)")
cmd = "scp -i /media/HDD/home/enrique/Proyectos/SMALL/zynq/zynq {} root@192.168.1.115:/home/root/{}".format(filename.name,real_name)
os.system(cmd)
#cmd = "python3 mqtt/client_traj.py -t {} -n 500 &".format(real_name)
#################################
#MUST BE PARAMETERIZED CORRECTLY#
#################################
self.pb["maximum"] = n
msg = "[1,S,/home/root/{},{}]".format(real_name,n)
self.filename = real_name
self.mqtt_client.publish(self.topic,msg,qos=0)
#os.system(cmd)
i = 0
[docs] def render_progressbar(self,row,col):
'''
Create the progress bar to indicate trajectory execution %
Args:
row (int): Row of the grid in which the buttons will be displayed
col (int): Column of the grid in which the buttons will be displayed
'''
if self.visible:
labelframe = ttk.LabelFrame(self.root, text="Trajectory Execution")
labelframe.grid(column=col, row=row, sticky=(
tk.N, tk.W), padx=5, pady=5,rowspan=2)
self.pb = Progressbar(labelframe, orient='horizontal',mode='determinate',length=600,maximum=100)
# a = self.pb.getvar("maximum")
self.pb.grid(row=1,column=1)
# self.pb.setvar("maximum","100")
# a = self.pb.getvar("maximum")
# start_button = ttk.Button(
# labelframe,
# text='Start').grid(row=2,column=1)
def render_textbox(self,row,col):
if self.visible:
labelframe = ttk.LabelFrame(self.root, text="Information")
labelframe.grid(column=col, row=row, sticky=(
tk.N, tk.W), padx=5, pady=5)
self.textbox = scrolledtext.ScrolledText(labelframe,height=7,width=90)
self.textbox.see(tk.END)
self.textbox.grid(column=1,row=1,sticky=(tk.W,tk.N))
[docs] def render_usbEnable(self,row,col):
'''
Create the checkbox that enables opening USB devices
The value of the box is stored in the checked_usb
variable available in the class (self.checked_usb)
Args:
row (int): Row of the grid in which the checkbox will be displayed
col (int): Column of the grid in which the checkbox will be displayed
'''
if self.visible:
labelframe = ttk.LabelFrame(self.root, text="USB")
labelframe.grid(column=col, row=row, sticky=(
tk.N, tk.W), padx=5, pady=5)
checked_usb = tk.BooleanVar()
checked_remote = tk.BooleanVar()
if self.visible:
ttk.Checkbutton(labelframe,text="Open device",command=self.checkUSB,variable=checked_usb,onvalue=True,offvalue=False).grid(column=1,row=3,sticky=(tk.W))
ttk.Checkbutton(labelframe,text="Remote mode",command=self.checkRemote,variable=checked_remote,onvalue=True,offvalue=False).grid(column=2,row=3,sticky=(tk.W))
self.checked_usb = checked_usb
self.checked_remote = checked_remote
# def openUSB(self):
# '''
# Try to open USB connection with robot infrastructure
# This function tries to open a connection with
# the AERNode USB board and claim its interface
# to initiate communication and returns the connection
# to the device found
# Returns:
# Device handler if connection was successful
# None if the connection couldn't be established
# '''
# try:
# dev = usb.core.find(idVendor=self.VID,idProduct=self.PID)
# except usb.core.NoBackendError:
# be = usb.backend.libusb1.get_backend(find_library = lambda x:"/lib/x86_64-linux-gnu/libusb-1.0.so.0")
# dev = usb.core.find(idVendor=self.VID,idProduct=self.PID,backend=be)
# finally:
# #If the device can't be found, tell the user and end execution
# if dev is None:
# self.alert("Device not found, try again or check the connection")
# self.checked_usb.set(False)
# return None
# #If the device was found, set configuration to default, claim the
# #default interface and attach the handler to dev
# else:
# dev.set_configuration()
# usb.util.claim_interface(dev,0)
# print("Device found and initialized successfully")
# return dev
# def closeUSB(self):
# '''
# Close USB connection
# This function releases the interface claimed
# and then detaches the handle to the device
# '''
# if(self.dev != None):
# usb.util.release_interface(self.dev,0)
# usb.util.dispose_resources(self.dev)
# self.dev = None
# print("Device disconnected successfully")
[docs] def checkUSB(self):
'''
Check wether USB usage has been enabled or not
This function reads the checked_usb self variable to determine
whether USB connection has been enabled or not
If it has, then tries to connect to AERNode board and sets
self.dev to the device handlerconnect
If it hasn't, it disconnects from the device
'''
#Check if USB is enabled
if self.checked_usb.get() == False:
#If not, close the connection
self.closeUSB()
else:
#If USB is enabled, try to connect to AERNode board
self.checked_remote.set(False)
self.dev = self.openUSB()
[docs] def checkRemote(self):
'''
Check wether remote usage has been enabled or not
This function reads the checked_remote self variable to determine
whether remote connection has been enabled or not
If it has, then tries to connect to MQTT broker and sets
self.mqtt to the mqtt client
If it hasn't, it disconnects from the broker
'''
#Check if USB is enabled
if self.checked_remote.get() == False:
#If not, close the connection
self.close_mqtt()
else:
#If remote usage is enabled, try to connect to mqtt broker
self.checked_usb.set(False)
self.mqtt_client = self.open_mqtt("192.168.1.104")
#self.mqtt_client = self.open_mqtt("150.214.140.189")
def open_mqtt(self,ip):
client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message
d = {
'visible':self.visible,
'textbox':self.textbox,
'pos_data':[],
'progressbar':self.pb,
'filename':self.filename
}
self.topic = "/EDScorbot/commands"
client.user_data_set(d)
client.connect("192.168.1.104", 1883, 60)
client.loop_start()
return client
def close_mqtt(self):
self.mqtt_client.loop_stop()
self.mqtt_client.disconnect()
self.mqtt_client = None
pass
[docs] def dumpConfig(self):
'''
Dump current configuration
This function dumps current config (the one being
displayed in the GUI) to a JSON file named "config.json"
Generated config file is also printed in console
'''
# Create temporary dicts which will hold the values of each text entry
# This is necessary because these entries can be modified using the
# set() method of the variables linked to them, but these variables
# are not serializable directly because they are tkinter var objects,
# so we need just their value to be able to serialize them properly
d = {}
d["Motor Config"] = {}
d["Joints"] = {}
d["Scan Parameters"] = {}
d["Dynapse2"] = {}
for key in self.d["Motor Config"].keys():
d["Motor Config"][key] = self.d["Motor Config"][key].get()
for key in self.d["Scan Parameters"].keys():
d["Scan Parameters"][key] = self.d["Scan Parameters"][key].get()
for key in self.d["Dynapse2"].keys():
d["Dynapse2"][key] = self.d["Dynapse2"][key].get()
#Once we've collected all variables, dump dictionary d as json to config.json
with open('config.json','w') as f:
json.dump(d,f,indent=2)
#And print generated config in the console
jsondict = json.dumps(d,indent=2)
print("Settings generated: \n"+jsondict)
[docs] def loadConfig(self,visible=True):
'''
Load configuration in an extern .json file
This function allows users to load a json file containing
a valid configuration file, just as the ones generated with
the dumpConfig function
Also works without gui, just pass the correspondent argument
to the --config option
'''
#Open file dialog to select config file
filename = filedialog.askopenfile(mode="r")
#Read file contents and parse as JSON
obj = filename.read()
j = json.loads(obj)
if visible:
#Then try to fit said JSON in the app variables
try:
for key in self.d["Motor Config"].keys():
self.d["Motor Config"][key].set(j["Motor Config"][key])
for key in self.d["Scan Parameters"].keys():
self.d["Scan Parameters"][key].set(j["Scan Parameters"][key])
return
#If we catch a KeyError, the config is invalid, so alert the user and end execution
except KeyError:
self.alert("Invalid config file")
return
else:
d = {}
d["Motor Config"] = {}
d["Joints"] = {}
d["Scan Parameters"] = {}
for config,configdict in j.items():
for k,v in configdict.items():
d[config][k] = v
self.d = d
[docs] def test_load(self,visible=False):
'''
Load configuration in an extern .json file
This function allows users to load a json file containing
a valid configuration file, just as the ones generated with
the dumpConfig function
'''
#Open file dialog to select config file
filename = filedialog.askopenfile(mode="r")
#Read file contents and parse as JSON
obj = filename.read()
j = json.loads(obj)
if visible:
#Then try to fit said JSON in the app variables
try:
for key in self.d["Motor Config"].keys():
self.d["Motor Config"][key].set(j["Motor Config"][key])
for key in self.d["Scan Parameters"].keys():
self.d["Scan Parameters"][key].set(j["Scan Parameters"][key])
return
#If we catch a KeyError, the config is invalid, so alert the user and end execution
except KeyError:
self.alert("Invalid config file")
return
else:
d = {}
d["Motor Config"] = {}
d["Joints"] = {}
d["Scan Parameters"] = {}
for config,configdict in j.items():
for k,v in configdict.items():
d[config][k] = v
self.d = d
def render_cameras(self,row,col):
if self.visible:
labelframe = ttk.LabelFrame(self.root, text="Cameras")
labelframe.grid(column=col, row=row, sticky=(
tk.N, tk.W), padx=5, pady=5)
camera1_enabled = tk.BooleanVar()
camera2_enabled = tk.BooleanVar()
if self.visible:
ttk.Checkbutton(labelframe,text="Camera 1 (Front)",command=self.openCamera1,variable=camera1_enabled,onvalue=True,offvalue=False).grid(column=1,row=1,sticky=(tk.W))
ttk.Checkbutton(labelframe,text="Camera 2 (Side)",command=self.openCamera2,variable=camera2_enabled,onvalue=True,offvalue=False).grid(column=1,row=2,sticky=(tk.W))
self.cam1_enable = camera1_enabled
self.cam2_enable = camera2_enabled
[docs] def render_gui(self):
'''
Top level GUI routine
This function serves as a top routine for rendering all
GUI widgets. It calls each function that renders a component,
and assigns them a place in the top window.
Layout changes may be made here: just change the column and row
of the component to render in its call to adjust it to your own needs
'''
#Set window title
if self.visible:
self.root.title("ATCEDScorbotConfig")
#Set list of rendered frames
self.frame_list = []
i = 1
#Render motors in a 2x3 (rowXcolumns) fashion
for row in range(1, 3):
for col in range(1, 4):
frame = self.render_motor(i, row, col)
i += 1
self.frame_list.append(frame)
#Render the rest of the components
self.frame_list.append(self.render_joints(3, 1))
self.frame_list.append(self.render_scan_parameters(3, 2))
#self.render_joints_hex(4,1)
self.render_buttons(1, 4)
self.render_usbEnable(5,1)
self.render_dynapse2(3,3)
#self.render_cameras(4,2)
self.render_textbox(3,4)
self.render_progressbar(4,4)
self.init_config()
self.update()
#And call mainloop to display GUI
if self.visible:
self.root.mainloop()
def openCamera1(self):
if(self.cam1_enable.get()):
self.camera1 = cv2.VideoCapture('/dev/video0')
else:
try:
self.camera1.release()
cv2.destroyWindow('front')
except:
pass
def openCamera2(self):
if(self.cam2_enable.get()):
self.camera2 = cv2.VideoCapture('/dev/video2')
else:
try:
self.camera2.release()
cv2.destroyWindow('side')
except:
pass
def toggle_record(self,filename=None):
if self.record:
date = datetime.datetime.now()
if filename == None:
timeStamp = date.strftime("%Y_%b_%d_%H_%M_%S")
else:
timeStamp=filename
P.dump(self.array,open(timeStamp + '.pkl','wb'))
self.array = []
self.record = not self.record
def print_updates(self,):
self.j1 = self.execute_script("bash/readJoint.bash 1")
self.j2 = self.execute_script("bash/readJoint.bash 2")
self.j3 = self.execute_script("bash/readJoint.bash 3")
self.j4 = self.execute_script("bash/readJoint.bash 4")
self.j5 = self.execute_script("bash/readJoint.bash 5")
self.j6 = self.execute_script("bash/readJoint.bash 6")
print("Joint\tPosition\tHex\nJ1\t{}\t{}\nJ2\t{}\t{}\nJ3\t{}\t{}\nJ4\t{}\t{}\nJ5\t{}\t{}\nJ6\t{}\t{}\n"
.format(self.j1,hex(self.j1),self.j2,hex(self.j2),self.j3,hex(self.j3),self.j4,hex(self.j4),self.j5,hex(self.j5),self.j6,hex(self.j6))
,end='/r')
def update(self,ref=None):
if self.checked_usb.get():
self.j1 = self.Read_J1_gui()
self.j2 = self.Read_J2_gui()
self.j3 = self.Read_J3_gui()
self.j4 = self.Read_J4_gui()
self.j5 = self.Read_J5_gui()
self.j6 = self.Read_J6_gui()
#xyz = self.CalculateXYZ()
#self.d["Joints"]["XYZ coordinates"].set(xyz)
if self.record:
motors = self.d["Motor Config"]
ts = self.millis_now()
positions = [self.j1,self.j2,self.j3,self.j4,self.j5,self.j6]
aux = []
for i in range(6):
label = "ref_M"+str(i+1)
if ref == None:
data = [motors[label].get(),positions[i]]
else:
data = [motors[label].get(),positions[i],ref[i]]
aux.append(data)
aux.append(ts)
self.array.append(aux)
if self.updating:
self.root.after(1,self.update)
[docs] def ConfigureInit(self):
'''
Set current position as initial
This function sets the current position of each joint
as the initial position. This means that, after this
function is called, the current position will match
the position with a reference of 0
'''
if self.checked_remote:
msg = "[3,S,-1,-1]"
self.mqtt_client.publish(self.topic,msg)
elif (((self.dev and self.checked_usb.get()) == None) or (self.checked_usb.get() == False)):
self.alert("No device opened. Try checking USB option first")
return
else:
if self.checked_usb.get():
for i in range(0,6):
self.sendCommand16(0xF7,0x0000,0x0000,True) #
#Motor 1
self.sendCommand16(0x00,0x00,0x03,True) #Leds M1
self.sendCommand16(0x01,0x00,0x01,True) #spike gen freq divider
self.sendCommand16(0x03,0x00,0x0f,True) #I banks disabled M1
self.sendCommand16(0x03,0x00,0x03,True) #I banks enabled Bank3 M1
self.sendCommand16(0x02,0x00,0x00,True) #Ref M1 0
self.sendCommand16(0x04,((512>>8) & 0xFF),512 & 0xFF,True) #FD I&G bank 0 M1
self.sendCommand16(0x05,((512>>8) & 0xFF),512 & 0xFF,True) #FD I&G bank 1 M1
self.sendCommand16(0x06,((512>>8) & 0xFF),512 & 0xFF,True) #FD I&G bank 2 M1
self.sendCommand16(0x07,((self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get()>>8) & 0xFF),self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get() & 0xFF,True) #FD I&G bank 3 M1
self.sendCommand16(0x08,(0x00),(0x0f), True) #d banks disabled M1
self.sendCommand16(0x08,0x00,0x03,True) #I banks enabled Bank3 M1
self.sendCommand16(0x09, ((512 >> 8) & 0xFF),((512) & 0xFF), True) #FD I&G bank 0 M1
self.sendCommand16(0x0A, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M1
self.sendCommand16(0x0B, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M1
self.sendCommand16(0x0C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M1"].get() >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16(0x12, (0x00), (0x0), True); #spike expansor M1
self.sendCommand16( 0x12, ((self.d["Motor Config"]["spike_expansor_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M1"].get()) & 0xFF), True) #spike expansor M1
self.sendCommand16(0x13, (0x00), (0x0f), True); #d banks disabled M1
self.sendCommand16(0x13,0x00,0x03,True) #I banks enabled Bank3 M1
self.sendCommand16(0x14, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M1
self.sendCommand16(0x15, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M1
self.sendCommand16(0x16, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M1
self.sendCommand16(0x17, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get()) & 0xFF), True); #FD I&G bank 3 M1
self.sendCommand16(0x00, 0x00, 0x00, False); #LEDs M1 off
print("PI_FD_bank0_12bits_M1={} \t PI_FD_bank1_14bits_M1={} \t PI_FD_bank2_16bits_M1={} \t PI_FD_bank3_18bits_M1={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get()) +
"PD_FD_bank0_16bits_M1={} \t PD_FD_bank1_18bits_M1={} \t PD_FD_bank2_20bits_M1={} \t PD_FD_bank3_22bits_M1={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M1={} \t EI_FD_bank1_14bits_M1={} \t EI_FD_bank2_16bits_M1={} \t EI_FD_bank3_18bits_M1={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get()))
#Motor 2
self.sendCommand16( 0x20, (0x00), (0x03), True); #LEDs M2
self.sendCommand16( 0x21, 0x00, 0x01 ,True) #spike gen freq divider
self.sendCommand16( 0x23, (0x00), (0x0f), True); #I banks disabled M2
self.sendCommand16( 0x23,0x00,0x03,True) #I banks enabled Bank3 M1
self.sendCommand16( 0x22, (0x00), (0x00), True); #Ref M2 0
self.sendCommand16( 0x24, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M2
self.sendCommand16( 0x25, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M2
self.sendCommand16( 0x26, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M2
self.sendCommand16( 0x27, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get()) & 0xFF), True); #FD I&G bank 3 M2
self.sendCommand16( 0x28, (0x00), (0x0f), True); #I banks disabled M2
self.sendCommand16( 0x28,0x00,0x03,True) #I banks enabled Bank3 M1
self.sendCommand16( 0x29, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M2
self.sendCommand16( 0x2A, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M2
self.sendCommand16( 0x2B, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M2
self.sendCommand16( 0x2C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get()) & 0xFF), True); #FD I&G bank 3 M2
self.sendCommand16( 0x32, (0x00), (0x0), True); #spike expansor M2
self.sendCommand16( 0x32, ((self.d["Motor Config"]["spike_expansor_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M2"].get()) & 0xFF), True) #spike expansor M2
self.sendCommand16( 0x33, (0x00), (0x0f), True); #d banks disabled M1
self.sendCommand16( 0x33,0x00,0x03,True) #I banks enabled Bank3 M1
self.sendCommand16( 0x34, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M1
self.sendCommand16( 0x35, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M1
self.sendCommand16( 0x36, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M1
self.sendCommand16( 0x37, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get()) & 0xFF), True); #FD I&G bank 3 M1
self.sendCommand16( 0x20, 0, 0, False); #LEDs M2 off
print("PI_FD_bank0_12bits_M2={} \t PI_FD_bank1_14bits_M2={} \t PI_FD_bank2_16bits_M2={} \t PI_FD_bank3_18bits_M2={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get()) +
"PD_FD_bank0_16bits_M2={} \t PD_FD_bank1_18bits_M2={} \t PD_FD_bank2_20bits_M2={} \t PD_FD_bank3_22bits_M2={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M2={} \t EI_FD_bank1_14bits_M2={} \t EI_FD_bank2_16bits_M2={} \t EI_FD_bank3_18bits_M2={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get()))
#Motor 3
self.sendCommand16( 0x40, (0x00), (0x03), True); #LEDs M3
self.sendCommand16( 0x41, 0x00, 0x01 ,True) #spike gen freq divider
self.sendCommand16( 0x43, (0x00), (0x0f), True); #I banks disabled M3
self.sendCommand16( 0x43,0x00,0x03,True) #I banks enabled Bank3 M1
self.sendCommand16( 0x42, (0x00), (0x00), True); #Ref M3 0
self.sendCommand16( 0x44, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M3
self.sendCommand16( 0x45, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M3
self.sendCommand16( 0x46, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M3
self.sendCommand16( 0x47, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get()) & 0xFF), True); #FD I&G bank 3 M3
self.sendCommand16( 0x48, (0x00), (0x0f), True); #I banks disabled M3
self.sendCommand16( 0x48,0x00,0x03,True) #I banks enabled Bank3 M1
self.sendCommand16( 0x49, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M3
self.sendCommand16( 0x4A, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M3
self.sendCommand16( 0x4B, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M3
self.sendCommand16( 0x4C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get()) & 0xFF), True); #FD I&G bank 3 M3
self.sendCommand16( 0x52, (0x00), (0x0), True); #spike expansor M3
self.sendCommand16( 0x52, ((self.d["Motor Config"]["spike_expansor_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M3"].get()) & 0xFF), True) #spike expansor M3
self.sendCommand16( 0x53, (0x00), (0x0f), True); #d banks disabled M1
self.sendCommand16( 0x53,0x00,0x03,True) #I banks enabled Bank3 M1
self.sendCommand16( 0x54, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M1
self.sendCommand16( 0x55, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M1
self.sendCommand16( 0x56, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M1
self.sendCommand16( 0x57, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get()) & 0xFF), True); #FD I&G bank 3 M1
self.sendCommand16( 0x40, 0, 0, False); #LEDs M3 off
print("PI_FD_bank0_12bits_M3={} \t PI_FD_bank1_14bits_M3={} \t PI_FD_bank2_16bits_M3={} \t PI_FD_bank3_18bits_M3={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get()) +
"PD_FD_bank0_16bits_M3={} \t PD_FD_bank1_18bits_M3={} \t PD_FD_bank2_20bits_M3={} \t PD_FD_bank3_22bits_M3={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M3={} \t EI_FD_bank1_14bits_M3={} \t EI_FD_bank2_16bits_M3={} \t EI_FD_bank3_18bits_M3={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get()))
#Motor 4
self.sendCommand16( 0x60, (0x00), (0x03), True); #LEDs M4
self.sendCommand16( 0x61, 0x00, 0x01 ,True) #spike gen freq divider
self.sendCommand16( 0x62, (0x00), (0x00), True); #Ref M4 0
self.sendCommand16( 0x63, (0x00), (0x0f), True); #I banks disabled M4
self.sendCommand16( 0x64, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M4
self.sendCommand16( 0x65, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M4
self.sendCommand16( 0x66, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M4
self.sendCommand16( 0x67, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get()) & 0xFF), True); #FD I&G bank 3 M4
self.sendCommand16( 0x68, (0x00), (0x0f), True); #I banks disabled M4
self.sendCommand16( 0x69, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M4
self.sendCommand16( 0x6A, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M4
self.sendCommand16( 0x6B, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M4
self.sendCommand16( 0x6C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get()) & 0xFF), True); #FD I&G bank 3 M4
self.sendCommand16( 0x72, (0x00), (0x0), True); #spike expansor M4
self.sendCommand16( 0x73, (0x00), (0x0f), True); #d banks disabled M1
self.sendCommand16( 0x74, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M1
self.sendCommand16( 0x75, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M1
self.sendCommand16( 0x76, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M1
self.sendCommand16( 0x77, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get()) & 0xFF), True); #FD I&G bank 3 M1
self.sendCommand16( 0x60, 0, 0, False); #LEDs M4 off
print("PI_FD_bank0_12bits_M4={} \t PI_FD_bank1_14bits_M4={} \t PI_FD_bank2_16bits_M4={} \t PI_FD_bank3_18bits_M4={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get()) +
"PD_FD_bank0_16bits_M4={} \t PD_FD_bank1_18bits_M4={} \t PD_FD_bank2_20bits_M4={} \t PD_FD_bank3_22bits_M4={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M4={} \t EI_FD_bank1_14bits_M4={} \t EI_FD_bank2_16bits_M4={} \t EI_FD_bank3_18bits_M4={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get()))
#Motor 5
self.sendCommand16( 0x80, (0x00), (0x03), True); #LEDs M5
self.sendCommand16( 0x81, 0x00, 0x01 ,True) #spike gen freq divider
self.sendCommand16( 0x82, (0x00), (0x00), True); #Ref M5 0
self.sendCommand16( 0x83, (0x00), (0x0f), True); #I banks disabled M5
self.sendCommand16( 0x84, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M5
self.sendCommand16( 0x85, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M5
self.sendCommand16( 0x86, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M5
self.sendCommand16( 0x87, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get()) & 0xFF), True); #FD I&G bank 3 M5
self.sendCommand16( 0x88, (0x00), (0x0f), True); #I banks disabled M5
self.sendCommand16( 0x89, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M5
self.sendCommand16( 0x8A, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M5
self.sendCommand16( 0x8B, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M5
self.sendCommand16( 0x8C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get()) & 0xFF), True); #FD I&G bank 3 M5
self.sendCommand16( 0x92, (0x00), (0x0), True); #spike expansor M5
self.sendCommand16( 0x93, (0x00), (0x0f), True); #d banks disabled M1
self.sendCommand16( 0x94, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M1
self.sendCommand16( 0x95, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M1
self.sendCommand16( 0x96, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M1
self.sendCommand16( 0x97, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get()) & 0xFF), True); #FD I&G bank 3 M1
self.sendCommand16( 0x80, 0, 0, False); #LEDs M5 off
print("PI_FD_bank0_12bits_M5={} \t PI_FD_bank1_14bits_M5={} \t PI_FD_bank2_16bits_M5={} \t PI_FD_bank3_18bits_M5={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get()) +
"PD_FD_bank0_16bits_M5={} \t PD_FD_bank1_18bits_M5={} \t PD_FD_bank2_20bits_M5={} \t PD_FD_bank3_22bits_M5={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M5={} \t EI_FD_bank1_14bits_M5={} \t EI_FD_bank2_16bits_M5={} \t EI_FD_bank3_18bits_M5={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get()))
#Motor 6
self.sendCommand16( 0xA0, (0x00), (0x03), True); #LEDs M6
self.sendCommand16( 0xA1, 0x00, 0x01 ,True) #spike gen freq divider
self.sendCommand16( 0xA2, (0x00), (0x00), True); #Ref M6 0
self.sendCommand16( 0xA3, (0x00), (0x0f), True); #I banks disabled M6
self.sendCommand16( 0xA4, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M6
self.sendCommand16( 0xA5, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M6
self.sendCommand16( 0xA6, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M6
self.sendCommand16( 0xA7, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get()) & 0xFF), True); #FD I&G bank 3 M6
self.sendCommand16( 0xA8, (0x00), (0x0f), True); #I banks disabled M6
self.sendCommand16( 0xA9, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M6
self.sendCommand16( 0xAA, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M6
self.sendCommand16( 0xAB, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M6
self.sendCommand16( 0xAC, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get()) & 0xFF), True); #FD I&G bank 3 M6
self.sendCommand16( 0xB2, (0x00), (0x0), True); #spike expansor M6
self.sendCommand16( 0xB3, (0x00), (0x0f), True); #d banks disabled M1
self.sendCommand16( 0xB4, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 0 M1
self.sendCommand16( 0xB5, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 1 M1
self.sendCommand16( 0xB6, ((512 >> 8) & 0xFF), ((512) & 0xFF), True); #FD I&G bank 2 M1
self.sendCommand16( 0xB7, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get()) & 0xFF), True); #FD I&G bank 3 M1
self.sendCommand16( 0xA0, 0, 0, False); #LEDs M6 off
print("PI_FD_bank0_12bits_M6={} \t PI_FD_bank1_14bits_M6={} \t PI_FD_bank2_16bits_M6={} \t PI_FD_bank3_18bits_M6={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get()) +
"PD_FD_bank0_16bits_M6={} \t PD_FD_bank1_18bits_M6={} \t PD_FD_bank2_20bits_M6={} \t PD_FD_bank3_22bits_M6={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M6={} \t EI_FD_bank1_14bits_M6={} \t EI_FD_bank2_16bits_M6={} \t EI_FD_bank3_18bits_M6={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get()))
print("Sending USB SPI")
print(i)
return
[docs] def SendCommandJoint1(self,ref):
'''
Send reference to 1st joint
This function allows to send a reference
to the 1st joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
#EI_FD_bank0_12bits_M1 = 512
#EI_FD_bank0_14bits_M1 = 512
#EI_FD_bank0_16bits_M1 = 512
#PI_FD_bank0_12bits_M1 = 512
#PI_FD_bank0_14bits_M1 = 512
#PI_FD_bank0_16bits_M1 = 512
self.sendCommand16( 0, (0x00), ((self.d["Motor Config"]["leds_M1"].get()) & 0xFF), True) #LEDs M1
self.sendCommand16(0x01,((self.d["Motor Config"]["ref_freq_divider_M1"].get() >> 8) & 0xFF),(self.d["Motor Config"]["ref_freq_divider_M1"].get() & 0xFF),True) #spike gen freq divider
self.sendCommand16( 0x03, (0x00), ((3)&0xFF), True) #I banks disabled M1
# self.sendCommand16( 0x04, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M1
# self.sendCommand16( 0x05, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M1
# self.sendCommand16( 0x06, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M1
self.sendCommand16( 0x07, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x08, (0x00), ((3)&0xFF), True) #D banks disabled M1 #it was (512)
# self.sendCommand16( 0x09, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M1
# self.sendCommand16( 0x0A, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M1
# self.sendCommand16( 0x0B, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M1
self.sendCommand16( 0x0C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x12, ((self.d["Motor Config"]["spike_expansor_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M1"].get()) & 0xFF), True) #spike expansor M1
self.sendCommand16( 0x13, (0x00), ((3)&0xFF), True) #EI bank enabled M1 EI_bank_select_M1 = 3
# self.sendCommand16( 0x14, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M1
# self.sendCommand16( 0x15, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M1
# self.sendCommand16( 0x16, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M1
self.sendCommand16( 0x17, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
#self.sendCommand16( 0, 0, 0, false) #LEDs M1 off
self.sendCommand16( 0x02, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M1 0
self.sendCommand16( 0x02, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M1 0
self.sendCommand16( 0x02, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M1 0
pass
[docs] def SendCommandJoint2(self,ref):
'''
Send reference to 2nd joint
This function allows to send a reference
to the 2nd joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
#EI_FD_bank0_12bits_M2 = 512
#EI_FD_bank0_14bits_M2 = 512
#EI_FD_bank0_16bits_M2 = 512
#PI_FD_bank0_12bits_M2 = 512
#PI_FD_bank0_14bits_M2 = 512
#PI_FD_bank0_16bits_M2 = 512
self.sendCommand16( 0x20, (0x00), ((self.d["Motor Config"]["leds_M2"].get()) & 0xFF), True) #LEDs M2
self.sendCommand16( 0x21, ((self.d["Motor Config"]["ref_freq_divider_M1"].get() >> 8) & 0xFF),(self.d["Motor Config"]["ref_freq_divider_M1"].get() & 0xFF),True) #spike gen freq divider
self.sendCommand16( 0x23, (0x00), ((3)&0xFF), True) #I banks disabled M2
# self.sendCommand16( 0x24, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M2
# self.sendCommand16( 0x25, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M2
# self.sendCommand16( 0x26, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M2
self.sendCommand16( 0x27, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M2
self.sendCommand16( 0x28, (0x00), ((3)&0xFF), True) #D banks disabled M2 #it was (512)
# self.sendCommand16( 0x29, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M2
# self.sendCommand16( 0x2A, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M2
# self.sendCommand16( 0x2B, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M2
self.sendCommand16( 0x2C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M2
self.sendCommand16( 0x32, ((self.d["Motor Config"]["spike_expansor_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M2"].get()) & 0xFF), True) #spike expansor M2
self.sendCommand16( 0x33, (0x00), ((3)&0xFF), True) #EI bank enabled M2 EI_bank_select_M2 = 3
# self.sendCommand16( 0x34, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M2
# self.sendCommand16( 0x35, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M2
# self.sendCommand16( 0x36, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M2
self.sendCommand16( 0x37, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M2
#self.sendCommand16( 0, 0, 0, false) #LEDs M2 off
self.sendCommand16( 0x22, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M2 0
self.sendCommand16( 0x22, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M2 0
self.sendCommand16( 0x22, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M2 0
pass
[docs] def SendCommandJoint3(self,ref):
'''
Send reference to 3rd joint
This function allows to send a reference
to the 3rd joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
#EI_FD_bank0_12bits_M3 = 512
#EI_FD_bank0_14bits_M3 = 512
#EI_FD_bank0_16bits_M3 = 512
#PI_FD_bank0_12bits_M3 = 512
#PI_FD_bank0_14bits_M3 = 512
#PI_FD_bank0_16bits_M3 = 512
self.sendCommand16( 0x40, (0x00), ((self.d["Motor Config"]["leds_M3"].get()) & 0xFF), True) #LEDs M3
self.sendCommand16( 0x41, ((self.d["Motor Config"]["ref_freq_divider_M1"].get() >> 8) & 0xFF),(self.d["Motor Config"]["ref_freq_divider_M1"].get() & 0xFF),True) #spike gen freq divider
self.sendCommand16( 0x43, (0x00), ((3)&0xFF), True) #I banks disabled M3
# self.sendCommand16( 0x44, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M3
# self.sendCommand16( 0x45, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M3
# self.sendCommand16( 0x46, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M3
self.sendCommand16( 0x47, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M3
self.sendCommand16( 0x48, (0x00), ((3)&0xFF), True) #D banks disabled M3 #it was (512)
# self.sendCommand16( 0x49, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M3
# self.sendCommand16( 0x4A, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M3
# self.sendCommand16( 0x4B, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M3
self.sendCommand16( 0x4C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M3
self.sendCommand16( 0x52, ((self.d["Motor Config"]["spike_expansor_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M3"].get()) & 0xFF), True) #spike expansor M3
self.sendCommand16( 0x53, (0x00), ((3)&0xFF), True) #EI bank enabled M3 EI_bank_select_M3 = 3
# self.sendCommand16( 0x54, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M3
# self.sendCommand16( 0x55, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M3
# self.sendCommand16( 0x56, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M3
self.sendCommand16( 0x57, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M3
#self.sendCommand16( 0, 0, 0, false) #LEDs M3 off
self.sendCommand16( 0x42, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M3 0
self.sendCommand16( 0x42, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M3 0
self.sendCommand16( 0x42, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M3 0
pass
[docs] def SendCommandJoint4(self,ref):
'''
Send reference to 4th joint
This function allows to send a reference
to the 4th joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
#EI_FD_bank0_12bits_M4 = 512
#EI_FD_bank0_14bits_M4 = 512
#EI_FD_bank0_16bits_M4 = 512
#PI_FD_bank0_12bits_M4 = 512
#PI_FD_bank0_14bits_M4 = 512
#PI_FD_bank0_16bits_M4 = 512
self.sendCommand16( 0x60, (0x00), ((self.d["Motor Config"]["leds_M4"].get()) & 0xFF), True) #LEDs M4
self.sendCommand16( 0x61, ((self.d["Motor Config"]["ref_freq_divider_M1"].get() >> 8) & 0xFF),(self.d["Motor Config"]["ref_freq_divider_M1"].get() & 0xFF),True) #spike gen freq divider
self.sendCommand16( 0x63, (0x00), ((3)&0xFF), True) #I banks disabled M4
# self.sendCommand16( 0x64, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M4
# self.sendCommand16( 0x65, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M4
# self.sendCommand16( 0x66, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M4
self.sendCommand16( 0x67, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x68, (0x00), ((3)&0xFF), True) #D banks disabled M4
# self.sendCommand16( 0x69, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M4
# self.sendCommand16( 0x6A, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M4
# self.sendCommand16( 0x6B, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M4
self.sendCommand16( 0x6C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x72, ((self.d["Motor Config"]["spike_expansor_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M4"].get()) & 0xFF), True) #spike expansor M4
self.sendCommand16( 0x73, (0x00), ((3)&0xFF), True) #EI bank enabled M4 EI_bank_select_M4 = 3
# self.sendCommand16( 0x74, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M4
# self.sendCommand16( 0x75, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M4
# self.sendCommand16( 0x76, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M4
self.sendCommand16( 0x77, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
#self.sendCommand16( 0, 0, 0, false) #LEDs M4 off
self.sendCommand16( 0x62, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M4 0
self.sendCommand16( 0x62, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M4 0
self.sendCommand16( 0x62, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M4 0
pass
[docs] def SendCommandJoint5(self,ref):
'''
Send reference to 5th joint
This function allows to send a reference
to the 5th joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
#EI_FD_bank0_12bits_M5 = 512
#EI_FD_bank0_14bits_M5 = 512
#EI_FD_bank0_16bits_M5 = 512
#PI_FD_bank0_12bits_M5 = 512
#PI_FD_bank0_14bits_M5 = 512
#PI_FD_bank0_16bits_M5 = 512
self.sendCommand16( 0x80, (0x00), ((self.d["Motor Config"]["leds_M5"].get()) & 0xFF), True) #LEDs M5
self.sendCommand16( 0x81, ((self.d["Motor Config"]["ref_freq_divider_M1"].get() >> 8) & 0xFF),(self.d["Motor Config"]["ref_freq_divider_M1"].get() & 0xFF),True) #spike gen freq divider
self.sendCommand16( 0x83, (0x00), ((3)&0xFF), True) #I banks disabled M5
# self.sendCommand16( 0x84, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M5
# self.sendCommand16( 0x85, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M5
# self.sendCommand16( 0x86, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M5
self.sendCommand16( 0x87, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x88, (0x00), ((3)&0xFF), True) #D banks disabled M5
# self.sendCommand16( 0x89, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M5
# self.sendCommand16( 0x8A, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M5
# self.sendCommand16( 0x8B, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M5
self.sendCommand16( 0x8C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x92, ((self.d["Motor Config"]["spike_expansor_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M5"].get()) & 0xFF), True) #spike expansor M5
self.sendCommand16( 0x93, (0x00), ((3)&0xFF), True) #EI bank enabled M5 EI_bank_select_M5 = 3
# self.sendCommand16( 0x94, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M5
# self.sendCommand16( 0x95, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M5
# self.sendCommand16( 0x96, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M5
self.sendCommand16( 0x97, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
#self.sendCommand16( 0, 0, 0, false) #LEDs M5 off
self.sendCommand16( 0x82, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M5 0
self.sendCommand16( 0x82, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M5 0
self.sendCommand16( 0x82, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M5 0
pass
[docs] def SendCommandJoint6(self,ref):
'''
Send reference to 6th joint
This function allows to send a reference
to the 6th joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
#EI_FD_bank0_12bits_M6 = 512
#EI_FD_bank0_14bits_M6 = 512
#EI_FD_bank0_16bits_M6 = 512
#PI_FD_bank0_12bits_M6 = 512
#PI_FD_bank0_14bits_M6 = 512
#PI_FD_bank0_16bits_M6 = 512
self.sendCommand16( 0xA0, (0x00), ((self.d["Motor Config"]["leds_M6"].get()) & 0xFF), True) #LEDs M6
self.sendCommand16( 0xA1, ((self.d["Motor Config"]["ref_freq_divider_M1"].get() >> 8) & 0xFF),(self.d["Motor Config"]["ref_freq_divider_M1"].get() & 0xFF),True) #spike gen freq divider
self.sendCommand16( 0xA3, (0x00), ((3)&0xFF), True) #I banks disabled M6
# self.sendCommand16( 0xA4, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M6
# self.sendCommand16( 0xA5, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M6
# self.sendCommand16( 0xA6, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M6
self.sendCommand16( 0xA7, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
self.sendCommand16( 0xA8, (0x00), ((3)&0xFF), True) #D banks disabled M6
# self.sendCommand16( 0xA9, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M6
# self.sendCommand16( 0xAA, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M6
# self.sendCommand16( 0xAB, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M6
self.sendCommand16( 0xAC, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
self.sendCommand16( 0xB2, ((self.d["Motor Config"]["spike_expansor_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M6"].get()) & 0xFF), True) #spike expansor M6
self.sendCommand16( 0xB3, (0x00), ((3)&0xFF), True) #EI bank enabled M6 EI_bank_select_M6 = 3
# self.sendCommand16( 0xB4, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 0 M6
# self.sendCommand16( 0xB5, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 1 M6
# self.sendCommand16( 0xB6, ((512 >> 8) & 0xFF), ((512) & 0xFF), True) #FD I&G bank 2 M6
self.sendCommand16( 0xB7, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
#self.sendCommand16( 0, 0, 0, false) #LEDs M6 off
self.sendCommand16( 0xA2, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M6 0
self.sendCommand16( 0xA2, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M6 0
self.sendCommand16( 0xA2, ((ref >> 8) & 0xFF), ((ref) & 0xFF), True) #Ref M6 0
pass
[docs] def ConfigureSPID(self):
'''
Program SPID parameters and command a movement to all the joints
It calls sendCommandJoint1-6 and sends
the parameters needed to configure the SPID controller
as well as the reference specified
'''
if (((self.dev and self.checked_usb.get()) == None) or (self.checked_usb.get() == False)):
self.alert("No device opened. Try checking USB option first")
return
else:
if self.checked_usb.get():
for i in range(0,6):
self.SendCommandJoint1(self.d["Motor Config"]["ref_M1"].get())
print("ref_M1:",self.d["Motor Config"]["ref_M1"].get())
print("PI_FD_bank0_12bits_M1={} \t PI_FD_bank1_14bits_M1={} \t PI_FD_bank2_16bits_M1={} \t PI_FD_bank3_18bits_M1={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get()) +
"PD_FD_bank0_16bits_M1={} \t PD_FD_bank1_18bits_M1={} \t PD_FD_bank2_20bits_M1={} \t PD_FD_bank3_22bits_M1={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M1={} \t EI_FD_bank1_14bits_M1={} \t EI_FD_bank2_16bits_M1={} \t EI_FD_bank3_18bits_M1={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get()))
self.SendCommandJoint2(self.d["Motor Config"]["ref_M2"].get())
print("ref_M2:",self.d["Motor Config"]["ref_M2"].get())
print("PI_FD_bank0_12bits_M2={} \t PI_FD_bank1_14bits_M2={} \t PI_FD_bank2_16bits_M2={} \t PI_FD_bank3_18bits_M2={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get()) +
"PD_FD_bank0_16bits_M2={} \t PD_FD_bank1_18bits_M2={} \t PD_FD_bank2_20bits_M2={} \t PD_FD_bank3_22bits_M2={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M2={} \t EI_FD_bank1_14bits_M2={} \t EI_FD_bank2_16bits_M2={} \t EI_FD_bank3_18bits_M2={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get()))
self.SendCommandJoint3(self.d["Motor Config"]["ref_M3"].get())
print("ref_M3:",self.d["Motor Config"]["ref_M3"].get())
print("PI_FD_bank0_12bits_M3={} \t PI_FD_bank1_14bits_M3={} \t PI_FD_bank2_16bits_M3={} \t PI_FD_bank3_18bits_M3={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get()) +
"PD_FD_bank0_16bits_M3={} \t PD_FD_bank1_18bits_M3={} \t PD_FD_bank2_20bits_M3={} \t PD_FD_bank3_22bits_M3={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M3={} \t EI_FD_bank1_14bits_M3={} \t EI_FD_bank2_16bits_M3={} \t EI_FD_bank3_18bits_M3={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get()))
self.SendCommandJoint4(self.d["Motor Config"]["ref_M4"].get())
print("ref_M4:",self.d["Motor Config"]["ref_M4"].get())
print("PI_FD_bank0_12bits_M4={} \t PI_FD_bank1_14bits_M4={} \t PI_FD_bank2_16bits_M4={} \t PI_FD_bank3_18bits_M4={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get()) +
"PD_FD_bank0_16bits_M4={} \t PD_FD_bank1_18bits_M4={} \t PD_FD_bank2_20bits_M4={} \t PD_FD_bank3_22bits_M4={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M4={} \t EI_FD_bank1_14bits_M4={} \t EI_FD_bank2_16bits_M4={} \t EI_FD_bank3_18bits_M4={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get()))
self.SendCommandJoint5(self.d["Motor Config"]["ref_M5"].get())
print("ref_M5:",self.d["Motor Config"]["ref_M5"].get())
print("PI_FD_bank0_12bits_M5={} \t PI_FD_bank1_14bits_M5={} \t PI_FD_bank2_16bits_M5={} \t PI_FD_bank3_18bits_M5={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get()) +
"PD_FD_bank0_16bits_M5={} \t PD_FD_bank1_18bits_M5={} \t PD_FD_bank2_20bits_M5={} \t PD_FD_bank3_22bits_M5={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M5={} \t EI_FD_bank1_14bits_M5={} \t EI_FD_bank2_16bits_M5={} \t EI_FD_bank3_18bits_M5={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get()))
self.SendCommandJoint6(self.d["Motor Config"]["ref_M6"].get())
print("ref_M6:",self.d["Motor Config"]["ref_M6"].get())
print("PI_FD_bank0_12bits_M6={} \t PI_FD_bank1_14bits_M6={} \t PI_FD_bank2_16bits_M6={} \t PI_FD_bank3_18bits_M6={}\n"
.format(512,512,512,self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get()) +
"PD_FD_bank0_16bits_M6={} \t PD_FD_bank1_18bits_M6={} \t PD_FD_bank2_20bits_M6={} \t PD_FD_bank3_22bits_M6={}\n"
.format(512,512,512,512) +
"EI_FD_bank0_12bits_M6={} \t EI_FD_bank1_14bits_M6={} \t EI_FD_bank2_16bits_M6={} \t EI_FD_bank3_18bits_M6={}\n"
.format(512,512,512,self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get()))
print("Sending USB SPI")
print(i)
[docs] def ConfigureSPID_allJoints(self):
'''
Same as ConfigureSPID but it doesn't print the information in the console
'''
#Check if USB device is initialized
if(self.dev == None):
self.alert("There is no opened device. Try opening one first")
return
if self.checked_usb.get():
for i in range (0,2):
self.SendCommandJoint1(self.d["Motor Config"]["ref_M1"].get())
self.SendCommandJoint2(self.d["Motor Config"]["ref_M2"].get())
self.SendCommandJoint3(self.d["Motor Config"]["ref_M3"].get())
self.SendCommandJoint4(self.d["Motor Config"]["ref_M4"].get())
self.SendCommandJoint5(self.d["Motor Config"]["ref_M5"].get())
self.SendCommandJoint6(self.d["Motor Config"]["ref_M6"].get())
print("Sending USB SPI")
print(i)
[docs] def sendCommand16(self, cmd, data1, data2, spiEnable):
'''
Send 2 bytes of data and an address via USB to robot's infrastructure
This function allows to send 2 bytes of data to
a specific address to the AERNode board via USB
Args:
cmd (int): Address to which data will be sent
data1 (byte): Byte 1 of data
data2 (byte): Byte 2 of data
spiEnable (bool): Legacy option not to break anything, always True in this program
'''
#Check if USB device is initialized
if(self.dev == None):
self.alert("There is no opened device. Try opening one first")
return
else:
#Allocate data buffer as a byte array
dataBuffer = bytearray(self.PACKET_LENGTH)
#Then fill it for the first transfer
dataBuffer[0] = ord('A')
dataBuffer[1] = ord('T')
dataBuffer[2] = ord('C')
dataBuffer[3] = 0x01 # Command always 1 for SPI upload.
dataBuffer[4] = 0x02 # Data length always 2 for 2 bytes.
dataBuffer[5] = 0x00
dataBuffer[6] = 0x00
dataBuffer[7] = 0x00
dataBuffer[8] = cmd
dataBuffer[9] = 0 if spiEnable else 1
#Library determines automatically transfer type via endpoint addresses
#so no need to specify bulk or interrupt
written = self.dev.write(self.ENDPOINT_OUT,dataBuffer)
#If the amount of bytes written is not the expected, raise a warning
if(written != self.PACKET_LENGTH):
print("Failed to transfer whole packet")
#Fill buffer for the second transfer,
#which contains the actual data
dataBuffer[0] = cmd
dataBuffer[1] = data1
dataBuffer[2] = data2
written = self.dev.write(self.ENDPOINT_OUT,dataBuffer)
#And check again for a correct transfer
if(written != self.PACKET_LENGTH):
print("Failed to transfer whole packet")
[docs] def readSensor(self,sensor):
'''
Read a joint's position sensor
This function allows for a joint sensor
to be read, so that you are able to
know a joint's position in any given moment
Args:
sensor (int): The number of the sensor to be read, ranging from 1 to 6
Returns:
int: Sensor position if everything went well
or -1 if something went wrong
'''
if(self.dev == None):
self.alert("There is no opened device. Try opening one first")
return
else:
#Allocate data buffer as a byte array
dataBuffer = bytearray(self.PACKET_LENGTH)
#Then fill it for the first transfer
dataBuffer[0] = ord('A')
dataBuffer[1] = ord('T')
dataBuffer[2] = ord('C')
dataBuffer[3] = 0x02 # Command always 2 for reading operation
dataBuffer[4] = 64 # Data length always 3 for 3 bytes.
readBuffer = bytearray(self.PACKET_LENGTH)
written = self.dev.write(self.ENDPOINT_OUT,dataBuffer)
#Check for a correct transfer
if(written != self.PACKET_LENGTH):
print("Failed to transfer whole packet")
readBuffer = self.dev.read(self.ENDPOINT_IN,self.PACKET_LENGTH,10000)
if read==0:
print("Failed to receive whole packet")
if readBuffer[34] == sensor:
sensor_data = (0x0ff & readBuffer[35])*256 + readBuffer[36]
else:
sensor_data = -1
return sensor_data
[docs] def scanMotor1(self):
'''
Scan Motor 1
This function commands the robot to perform
the scan procedure defined by the 4 Scan Parameters
(available in the dictionary as "Scan Parameters")
These functions (scanMotor1-6) can be used to obtain data
that can be later be displayed in different graphs
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
# Convert ms time into clock cycles.
scan_Init_Value = self.d["Scan Parameters"]["scan_Init_Value"].get()
scan_Final_Value = self.d["Scan Parameters"]["scan_Final_Value"].get()
scan_Step_Value = self.d["Scan Parameters"]["scan_Step_Value"].get()
scan_Wait_Time = self.d["Scan Parameters"]["scan_Wait_Time"].get()
motor = 1
if self.checked_usb.get():
#Fecha en str, formato: yyyy_MM_dd_HH_mm_ss
date = datetime.datetime.now()
timeStamp = date.strftime("%Y_%b_%d_%H_%M_%S")
#Abrir archivo de log con el nombre de la fecha
filename='./logs/Scan1_' + timeStamp + '.log'
logger_file = logging.FileHandler(filename)
logger = logging.getLogger("logger_J{}".format(motor))
logger.addHandler(logger_file)
logger.info("SMALL ED-Scorbot Joint1 Scan Log file")
#
self.sendCommand16( 0x03, (0x00), ((3)&0xFF), True) #I banks disabled M1 PI_bank_select_M1 = 3
self.sendCommand16( 0x07, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x08, (0x00), ((3)&0xFF), True) #D banks disabled M1 PD_bank_select_M1 = 3
self.sendCommand16( 0x0C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x12, ((self.d["Motor Config"]["spike_expansor_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M1"].get()) & 0xFF), True) #spike expansor M1
self.sendCommand16( 0x13, (0x00), ((3)&0xFF), True) #EI bank enabled M1 EI_bank_select_M1 = 3
self.sendCommand16( 0x17, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x02, ((scan_Init_Value >> 8) & 0xFF), ((scan_Init_Value) & 0xFF), True) #Ref M1 0
logger.info("Time\tM1 Ref\tJ1 Pos\tM2 Ref\tJ2 Pos\tM3 Ref\tJ3 Pos\tM4 Ref\tJ4 Pos\tM5 Ref\tJ5 Pos\tM6 Ref\tJ6 Pos\t")
start = self.millis_now()
now = self.millis_now()
while (abs(now-start) < 1500):
lap = self.millis_now()
while(abs(now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((self.millis_now()-start),scan_Init_Value,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
for j in range(0,5):
i = scan_Init_Value
while(i <= scan_Final_Value):
self.sendCommand16( 0x02, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M1 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i + scan_Step_Value
i = scan_Final_Value
while(i>=scan_Init_Value):
self.sendCommand16( 0x02, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M1 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i - scan_Step_Value
pass
[docs] def scanMotor2(self):
'''
Scan Motor 2
This function commands the robot to perform
the scan procedure defined by the 4 Scan Parameters
(available in the dictionary as "Scan Parameters")
These functions (scanMotor1-6) can be used to obtain data
that can be later be displayed in different graphs
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
# Convert ms time into clock cycles.
scan_Init_Value = self.d["Scan Parameters"]["scan_Init_Value"].get()
scan_Final_Value = self.d["Scan Parameters"]["scan_Final_Value"].get()
scan_Step_Value = self.d["Scan Parameters"]["scan_Step_Value"].get()
scan_Wait_Time = self.d["Scan Parameters"]["scan_Wait_Time"].get()
motor = 2
if self.checked_usb.get():
#Fecha en str, formato: yyyy_MM_dd_HH_mm_ss
date = datetime.datetime.now()
timeStamp = date.strftime("%Y_%b_%d_%H_%M_%S")
#Abrir archivo de log con el nombre de la fecha
filename='./logs/Scan2_' + timeStamp + '.log'
logger_file = logging.FileHandler(filename)
logger = logging.getLogger("logger_J{}".format(motor))
logger.addHandler(logger_file)
#
self.sendCommand16( 0x23, (0x00), ((3)&0xFF), True) #I banks disabled M2 PI_bank_select_M2 = 3
self.sendCommand16( 0x27, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M2
self.sendCommand16( 0x28, (0x00), ((3)&0xFF), True) #D banks disabled M2 PD_bank_select_M2 = 3
self.sendCommand16( 0x2C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M2
self.sendCommand16( 0x32, ((self.d["Motor Config"]["spike_expansor_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M2"].get()) & 0xFF), True) #spike expansor M2
self.sendCommand16( 0x33, (0x00), ((3)&0xFF), True) #EI bank enabled M2 EI_bank_select_M2 = 3
self.sendCommand16( 0x37, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M2
self.sendCommand16( 0x22, ((scan_Init_Value >> 8) & 0xFF), ((scan_Init_Value) & 0xFF), True) #Ref M2 0
logger.info("Time\tM2 Ref\tJ1 Pos\tM2 Ref\tJ2 Pos\tM3 Ref\tJ3 Pos\tM4 Ref\tJ4 Pos\tM5 Ref\tJ5 Pos\tM6 Ref\tJ6 Pos\t")
start = self.millis_now()
now = self.millis_now()
while (abs(now-start) < 1500):
lap = self.millis_now()
while(abs(now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((self.millis_now()-start),scan_Init_Value,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
for j in range(0,5):
i = scan_Init_Value
while(i <= scan_Final_Value):
self.sendCommand16( 0x22, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M2 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i + scan_Step_Value
i = scan_Final_Value
while(i>=scan_Init_Value):
self.sendCommand16( 0x22, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M2 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i - scan_Step_Value
pass
[docs] def scanMotor3(self):
'''
Scan Motor 3
This function commands the robot to perform
the scan procedure defined by the 4 Scan Parameters
(available in the dictionary as "Scan Parameters")
These functions (scanMotor1-6) can be used to obtain data
that can be later be displayed in different graphs
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
# Convert ms time into clock cycles.
scan_Init_Value = self.d["Scan Parameters"]["scan_Init_Value"].get()
scan_Final_Value = self.d["Scan Parameters"]["scan_Final_Value"].get()
scan_Step_Value = self.d["Scan Parameters"]["scan_Step_Value"].get()
scan_Wait_Time = self.d["Scan Parameters"]["scan_Wait_Time"].get()
motor = 3
if self.checked_usb.get():
#Fecha en str, formato: yyyy_MM_dd_HH_mm_ss
date = datetime.datetime.now()
timeStamp = date.strftime("%Y_%b_%d_%H_%M_%S")
#Abrir archivo de log con el nombre de la fecha
filename='./logs/Scan3_' + timeStamp + '.log'
logger_file = logging.FileHandler(filename)
logger = logging.getLogger("logger_J{}".format(motor))
logger.addHandler(logger_file)
#
self.sendCommand16( 0x43, (0x00), ((3)&0xFF), True) #I banks disabled M3 PI_bank_select_M3 = 3
self.sendCommand16( 0x47, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M3
self.sendCommand16( 0x48, (0x00), ((3)&0xFF), True) #D banks disabled M3 PD_bank_select_M3 = 3
self.sendCommand16( 0x4C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M3
self.sendCommand16( 0x52, ((self.d["Motor Config"]["spike_expansor_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M3"].get()) & 0xFF), True) #spike expansor M3
self.sendCommand16( 0x53, (0x00), ((3)&0xFF), True) #EI bank enabled M3 EI_bank_select_M3 = 3
self.sendCommand16( 0x57, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M3
self.sendCommand16( 0x42, ((scan_Init_Value >> 8) & 0xFF), ((scan_Init_Value) & 0xFF), True) #Ref M3 0
logger.info("Time\tM3 Ref\tJ1 Pos\tM3 Ref\tJ2 Pos\tM3 Ref\tJ3 Pos\tM4 Ref\tJ4 Pos\tM5 Ref\tJ5 Pos\tM6 Ref\tJ6 Pos\t")
start = self.millis_now()
now = self.millis_now()
while (abs(now-start) < 1500):
lap = self.millis_now()
while(abs(now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((self.millis_now()-start),scan_Init_Value,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
for j in range(0,5):
i = scan_Init_Value
while(i <= scan_Final_Value):
self.sendCommand16( 0x42, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M3 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i + scan_Step_Value
i = scan_Final_Value
while(i>=scan_Init_Value):
self.sendCommand16( 0x42, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M3 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i - scan_Step_Value
pass
[docs] def scanMotor4(self):
'''
Scan Motor 4
This function commands the robot to perform
the scan procedure defined by the 4 Scan Parameters
(available in the dictionary as "Scan Parameters")
These functions (scanMotor1-6) can be used to obtain data
that can be later be displayed in different graphs
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
# Convert ms time into clock cycles.
scan_Init_Value = self.d["Scan Parameters"]["scan_Init_Value"].get()
scan_Final_Value = self.d["Scan Parameters"]["scan_Final_Value"].get()
scan_Step_Value = self.d["Scan Parameters"]["scan_Step_Value"].get()
scan_Wait_Time = self.d["Scan Parameters"]["scan_Wait_Time"].get()
motor = 4
if self.checked_usb.get():
#Fecha en str, formato: yyyy_MM_dd_HH_mm_ss
date = datetime.datetime.now()
timeStamp = date.strftime("%Y_%b_%d_%H_%M_%S")
#Abrir archivo de log con el nombre de la fecha
filename='./logs/Scan4_' + timeStamp + '.log'
logger_file = logging.FileHandler(filename)
logger = logging.getLogger("logger_J{}".format(motor))
logger.addHandler(logger_file)
#
self.sendCommand16( 0x63, (0x00), ((3)&0xFF), True) #I banks disabled M4 PI_bank_select_M4 = 3
self.sendCommand16( 0x67, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x68, (0x00), ((3)&0xFF), True) #D banks disabled M4 PD_bank_select_M4 = 3
self.sendCommand16( 0x6C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x72, ((self.d["Motor Config"]["spike_expansor_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M4"].get()) & 0xFF), True) #spike expansor M4
self.sendCommand16( 0x73, (0x00), ((3)&0xFF), True) #EI bank enabled M4 EI_bank_select_M4 = 3
self.sendCommand16( 0x77, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x62, ((scan_Init_Value >> 8) & 0xFF), ((scan_Init_Value) & 0xFF), True) #Ref M4 0
logger.info("Time\tM4 Ref\tJ1 Pos\tM4 Ref\tJ2 Pos\tM4 Ref\tJ3 Pos\tM4 Ref\tJ4 Pos\tM5 Ref\tJ5 Pos\tM6 Ref\tJ6 Pos\t")
start = self.millis_now()
now = self.millis_now()
while (abs(now-start) < 1500):
lap = self.millis_now()
while(abs(now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((self.millis_now()-start),scan_Init_Value,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
for j in range(0,5):
i = scan_Init_Value
while(i <= scan_Final_Value):
self.sendCommand16( 0x62, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M4 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i + scan_Step_Value
i = scan_Final_Value
while(i>=scan_Init_Value):
self.sendCommand16( 0x62, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M4 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i - scan_Step_Value
pass
[docs] def scanMotor5(self):
'''
Scan Motor 5
This function commands the robot to perform
the scan procedure defined by the 4 Scan Parameters
(available in the dictionary as "Scan Parameters")
These functions (scanMotor1-6) can be used to obtain data
that can be later be displayed in different graphs
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
# Convert ms time into clock cycles.
scan_Init_Value = self.d["Scan Parameters"]["scan_Init_Value"].get()
scan_Final_Value = self.d["Scan Parameters"]["scan_Final_Value"].get()
scan_Step_Value = self.d["Scan Parameters"]["scan_Step_Value"].get()
scan_Wait_Time = self.d["Scan Parameters"]["scan_Wait_Time"].get()
motor = 5
if self.checked_usb.get():
#Fecha en str, formato: yyyy_MM_dd_HH_mm_ss
date = datetime.datetime.now()
timeStamp = date.strftime("%Y_%b_%d_%H_%M_%S")
#Abrir archivo de log con el nombre de la fecha
filename='./logs/Scan5_' + timeStamp + '.log'
logger_file = logging.FileHandler(filename)
logger = logging.getLogger("logger_J{}".format(motor))
logger.addHandler(logger_file)
#
self.sendCommand16( 0x83, (0x00), ((3)&0xFF), True) #I banks disabled M5 PI_bank_select_M5 = 3
self.sendCommand16( 0x87, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x88, (0x00), ((3)&0xFF), True) #D banks disabled M5 PD_bank_select_M5 = 3
self.sendCommand16( 0x8C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x92, ((self.d["Motor Config"]["spike_expansor_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M5"].get()) & 0xFF), True) #spike expansor M5
self.sendCommand16( 0x93, (0x00), ((3)&0xFF), True) #EI bank enabled M5 EI_bank_select_M5 = 3
self.sendCommand16( 0x97, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x82, ((scan_Init_Value >> 8) & 0xFF), ((scan_Init_Value) & 0xFF), True) #Ref M5 0
logger.info("Time\tM5 Ref\tJ1 Pos\tM5 Ref\tJ2 Pos\tM5 Ref\tJ3 Pos\tM5 Ref\tJ4 Pos\tM5 Ref\tJ5 Pos\tM6 Ref\tJ6 Pos\t")
start = self.millis_now()
now = self.millis_now()
while (abs(now-start) < 1500):
lap = self.millis_now()
while(abs(now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((self.millis_now()-start),scan_Init_Value,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
for j in range(0,5):
i = scan_Init_Value
while(i <= scan_Final_Value):
self.sendCommand16( 0x82, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M5 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i + scan_Step_Value
i = scan_Final_Value
while(i>=scan_Init_Value):
self.sendCommand16( 0x82, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M5 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i - scan_Step_Value
pass
[docs] def scanMotor6(self):
'''
Scan Motor 2
This function commands the robot to perform
the scan procedure defined by the 4 Scan Parameters
(available in the dictionary as "Scan Parameters")
These functions (scanMotor1-6) can be used to obtain data
that can be later be displayed in different graphs
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
# Convert ms time into clock cycles.
scan_Init_Value = self.d["Scan Parameters"]["scan_Init_Value"].get()
scan_Final_Value = self.d["Scan Parameters"]["scan_Final_Value"].get()
scan_Step_Value = self.d["Scan Parameters"]["scan_Step_Value"].get()
scan_Wait_Time = self.d["Scan Parameters"]["scan_Wait_Time"].get()
motor = 6
if self.checked_usb.get():
#Fecha en str, formato: yyyy_MM_dd_HH_mm_ss
date = datetime.datetime.now()
timeStamp = date.strftime("%Y_%b_%d_%H_%M_%S")
#Abrir archivo de log con el nombre de la fecha
filename='./logs/Scan6_' + timeStamp + '.log'
logger_file = logging.FileHandler(filename)
logger = logging.getLogger("logger_J{}".format(motor))
logger.addHandler(logger_file)
#
self.sendCommand16( 0xA3, (0x00), ((3)&0xFF), True) #I banks disabled M6 PI_bank_select_M6 = 3
self.sendCommand16( 0xA7, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
self.sendCommand16( 0xA8, (0x00), ((3)&0xFF), True) #D banks disabled M6 PD_bank_select_M6 = 3
self.sendCommand16( 0xAC, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
self.sendCommand16( 0xB2, ((self.d["Motor Config"]["spike_expansor_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M6"].get()) & 0xFF), True) #spike expansor M6
self.sendCommand16( 0xB3, (0x00), ((3)&0xFF), True) #EI bank enabled M6 EI_bank_select_M6 = 3
self.sendCommand16( 0xB7, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
self.sendCommand16( 0xA2, ((scan_Init_Value >> 8) & 0xFF), ((scan_Init_Value) & 0xFF), True) #Ref M6 0
logger.info("Time\tM6 Ref\tJ1 Pos\tM6 Ref\tJ2 Pos\tM6 Ref\tJ3 Pos\tM6 Ref\tJ4 Pos\tM6 Ref\tJ5 Pos\tM6 Ref\tJ6 Pos\t")
start = self.millis_now()
now = self.millis_now()
while (abs(now-start) < 1500):
lap = self.millis_now()
while(abs(now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((self.millis_now()-start),scan_Init_Value,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
for j in range(0,5):
i = scan_Init_Value
while(i <= scan_Final_Value):
self.sendCommand16( 0xA2, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M6 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i + scan_Step_Value
i = scan_Final_Value
while(i>=scan_Init_Value):
self.sendCommand16( 0xA2, ((i >> 8) & 0xFF), ((i) & 0xFF), True); #Ref M6 0
start2 = self.millis_now()
now = self.millis_now()
while (abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while((now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),0,self.Read_J2_pos(),0,self.Read_J3_pos(),0,self.Read_J4_pos(),0,self.Read_J5_pos(),0,self.Read_J6_pos()))
now = self.millis_now()
i = i - scan_Step_Value
pass
[docs] def Read_J1_pos(self):
'''
Read position of the first joint
This function makes combined use of sendCommand16 and
readSensor functions to retrieve J1 position
Returns:
int: Position of J1 or -1 if something went wrong
'''
j1_pos = -1
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
if self.checked_usb.get():
sensor_j1 = -1
for i in range(0,10):
self.sendCommand16( 0xF1, (0x00), (0x00), True);
self.sendCommand16( 0xF1, (0x00), (0x00), True);
sensor_j1 = self.readSensor(0x01)
if sensor_j1 > 0:
break
j1_pos = sensor_j1
return j1_pos
[docs] def Read_J2_pos(self):
'''
Read position of the second joint
This function makes combined use of sendCommand16 and
readSensor functions to retrieve J2 position
Returns:
int: Position of J2 or -1 if something went wrong
'''
j2_pos = -1
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
if self.checked_usb.get():
sensor_j2 = -1
for i in range(0,10):
self.sendCommand16( 0xF2, (0x00), (0x00), True);
self.sendCommand16( 0xF2, (0x00), (0x00), True);
sensor_j2 = self.readSensor(0x02)
if sensor_j2 > 0:
break
j2_pos = sensor_j2
return j2_pos
[docs] def Read_J3_pos(self):
'''
Read position of the third joint
This function makes combined use of sendCommand16 and
readSensor functions to retrieve J3 position
Returns:
int: Position of J3 or -1 if something went wrong
'''
j3_pos = -1
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
if self.checked_usb.get():
sensor_j3 = -1
for i in range(0,10):
self.sendCommand16( 0xF3, (0x00), (0x00), True);
self.sendCommand16( 0xF3, (0x00), (0x00), True);
sensor_j3 = self.readSensor(0x03)
if sensor_j3 > 0:
break
j3_pos = sensor_j3
return j3_pos
[docs] def Read_J4_pos(self):
'''
Read position of the fourth joint
This function makes combined use of sendCommand16 and
readSensor functions to retrieve J4 position
Returns:
int: Position of J4 or -1 if something went wrong
'''
j4_pos = -1
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
if self.checked_usb.get():
sensor_j4 = -1
for i in range(0,10):
self.sendCommand16( 0xF4, (0x00), (0x00), True);
self.sendCommand16( 0xF4, (0x00), (0x00), True);
sensor_j4 = self.readSensor(0x04)
if sensor_j4 > 0:
break
j4_pos = sensor_j4
return j4_pos
[docs] def Read_J5_pos(self):
'''
Read position of the fifth joint
This function makes combined use of sendCommand16 and
readSensor functions to retrieve J5 position
Returns:
int: Position of J5 or -1 if something went wrong
'''
j5_pos = -1
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
if self.checked_usb.get():
sensor_j5 = -1
for i in range(0,10):
self.sendCommand16( 0xF5, (0x00), (0x00), True);
self.sendCommand16( 0xF5, (0x00), (0x00), True);
sensor_j5 = self.readSensor(0x05)
if sensor_j5 > 0:
break
j5_pos = sensor_j5
return j5_pos
[docs] def Read_J6_pos(self):
'''
Read position of the sixth joint
This function makes combined use of sendCommand16 and
readSensor functions to retrieve J6 position
Returns:
int: Position of J6 or -1 if something went wrong
'''
j6_pos = -1
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
if self.checked_usb.get():
sensor_j6 = -1
for i in range(0,10):
self.sendCommand16( 0xF6, (0x00), (0x00), True);
self.sendCommand16( 0xF6, (0x00), (0x00), True);
sensor_j6 = self.readSensor(0x06)
if sensor_j6 > 0:
break
j6_pos = sensor_j6
return j6_pos
# def CalculateXYZ(self,njoints=4):
# j1 = self.count_to_angle(self.d["Joints"]["J1_sensor_value"].get())
# j2 = self.count_to_angle(self.d["Joints"]["J2_sensor_value"].get())
# j3 = self.count_to_angle(self.d["Joints"]["J3_sensor_value"].get())
# j4 = self.count_to_angle(self.d["Joints"]["J4_sensor_value"].get())
# j5 = self.count_to_angle(self.d["Joints"]["J5_sensor_value"].get())
# j6 = self.count_to_angle(self.d["Joints"]["J6_sensor_value"].get())
# l = [j1,j2,j3,j4,j5,j6]
# l2 = []
# for i in range(njoints):
# l2.append(l[i])
# xyz = inverse_kinematics(l2)
# return xyz
def Read_J1_gui(self):
pos = self.Read_J1_pos()
self.d["Joints"]["J1_sensor_value"].set(pos)
#self.d["Joints"]["J1_sensor_value_hex"].set(hex(pos))
self.d["Joints"]["J1_sensor_value_hex"].set(self.count_to_angle(1,pos))
return pos
def Read_J2_gui(self):
pos = self.Read_J2_pos()
self.d["Joints"]["J2_sensor_value"].set(pos)
#self.d["Joints"]["J2_sensor_value_hex"].set(hex(pos))
self.d["Joints"]["J2_sensor_value_hex"].set(self.count_to_angle(2,pos))
return pos
def Read_J3_gui(self):
pos = self.Read_J3_pos()
self.d["Joints"]["J3_sensor_value"].set(pos)
#self.d["Joints"]["J3_sensor_value_hex"].set(hex(pos))
self.d["Joints"]["J3_sensor_value_hex"].set(self.count_to_angle(3,pos))
return pos
def Read_J4_gui(self):
pos = self.Read_J4_pos()
self.d["Joints"]["J4_sensor_value"].set(pos)
#self.d["Joints"]["J4_sensor_value_hex"].set(hex(pos))
self.d["Joints"]["J4_sensor_value_hex"].set(self.count_to_angle(4,pos))
return pos
def Read_J5_gui(self):
pos = self.Read_J5_pos()
self.d["Joints"]["J5_sensor_value"].set(pos)
#self.d["Joints"]["J5_sensor_value_hex"].set(hex(pos))
self.d["Joints"]["J5_sensor_value_hex"].set(self.count_to_angle(5,pos))
return pos
def Read_J6_gui(self):
pos = self.Read_J6_pos()
self.d["Joints"]["J6_sensor_value"].set(pos)
#self.d["Joints"]["J6_sensor_value_hex"].set(hex(pos))
self.d["Joints"]["J6_sensor_value_hex"].set(self.count_to_angle(6,pos))
return pos
def Reset_J1_pos(self):
self.sendCommand16( 0xF1, (0x00), (0xF1), True)
def Reset_J2_pos(self):
self.sendCommand16( 0xF2, (0x00), (0xF2), True)
def Reset_J3_pos(self):
self.sendCommand16( 0xF3, (0x00), (0xF3), True)
def Reset_J4_pos(self):
self.sendCommand16( 0xF4, (0x00), (0xF4), True)
def Reset_J5_pos(self):
self.sendCommand16( 0xF5, (0x00), (0xF5), True)
def Reset_J6_pos(self):
self.sendCommand16( 0xF6, (0x00), (0xF6), True)
[docs] def SendFPGAReset(self):
'''
Command the FPGA to force an internal reset
This function tells the FPGA to reset, in order
to restore its initial configuration in case
something is not working properly
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
else:
if self.checked_usb.get():
for i in range(0,2):
self.sendCommand16( 0, (0x00), (0x00), True) #LEDs M1
self.sendCommand16( 0x03, (0x00), (0x0f), True) #I banks disabled M1
self.sendCommand16( 0x08, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0x12, (0x00), (0x0), True) #spike expansor M1
self.sendCommand16( 0x13, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0x20, (0x00), (0x00), True) #LEDs M2
self.sendCommand16( 0x23, (0x00), (0x0f), True) #I banks disabled M1
self.sendCommand16( 0x28, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0x32, (0x00), (0x0), True) #spike expansor M1
self.sendCommand16( 0x33, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0x40, (0x00), (0x00), True) #LEDs M3
self.sendCommand16( 0x43, (0x00), (0x0f), True) #I banks disabled M1
self.sendCommand16( 0x48, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0x52, (0x00), (0x0), True) #spike expansor M1
self.sendCommand16( 0x53, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0x60, (0x00), (0x00), True) #LEDs M4
self.sendCommand16( 0x63, (0x00), (0x0f), True) #I banks disabled M1
self.sendCommand16( 0x68, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0x72, (0x00), (0x0), True) #spike expansor M1
self.sendCommand16( 0x73, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0x80, (0x00), (0x00), True) #LEDs M5
self.sendCommand16( 0x83, (0x00), (0x0f), True) #I banks disabled M1
self.sendCommand16( 0x88, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0x92, (0x00), (0x0), True) #spike expansor M1
self.sendCommand16( 0x93, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0xA0, (0x00), (0x00), True) #LEDs M6
self.sendCommand16( 0xA3, (0x00), (0x0f), True) #I banks disabled M1
self.sendCommand16( 0xA8, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0xB2, (0x00), (0x0), True) #spike expansor M1
self.sendCommand16( 0xB3, (0x00), (0x0f), True) #d banks disabled M1
for i in range(0,2):
self.sendCommand16( 0, (0x00), (0xFF), True) #LEDs M1
self.sendCommand16( 0xff, (0xFF), (0xFF), True) #FPGA reset
self.sendCommand16( 0xfe, (0xFF), (0xFF), True) #FPGA reset
self.sendCommand16( 0xff, (0x00), (0x00), True) #FPGA reset
self.sendCommand16( 0xfe, (0x00), (0x00), True) #FPGA reset
for i in range(0,2):
self.SendCommandJoint1(0)
self.SendCommandJoint2(0)
self.SendCommandJoint3(0)
self.SendCommandJoint4(0)
self.SendCommandJoint5(0)
self.SendCommandJoint6(0)
[docs] def SendFPGAReset_joint(self, joint):
'''
Command the FPGA to force an internal reset
This function tells the FPGA to reset, in order
to restore its initial configuration in case
something is not working properly
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
else:
if self.checked_usb.get():
for i in range(0,2):
if(joint==1):
self.sendCommand16( 0, (0x00), (0x00), True) #LEDs M1
self.sendCommand16( 0x03, (0x00), (0x0f), True) #I banks disabled M1
self.sendCommand16( 0x08, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0x12, (0x00), (0x0), True) #spike expansor M1
self.sendCommand16( 0x13, (0x00), (0x0f), True) #d banks disabled M1
self.sendCommand16( 0, (0x00), (0xFF), True) #LEDs M1
# self.sendCommand16( 0xff, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xfe, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xff, (0x00), (0x00), True) #FPGA reset
# self.sendCommand16( 0xfe, (0x00), (0x00), True) #FPGA reset
self.sendCommand16( 0x03, (0x00), (0x03), True) #I bank enabled M1
self.sendCommand16( 0x08, (0x00), (0x03), True) #d bank enabled M1
self.sendCommand16( 0x13, (0x00), (0x03), True) #d bank enabled M1
self.sendCommand16( 0x12, ((self.d["Motor Config"]["spike_expansor_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M1"].get()) & 0xFF), True) #spike expansor M1
self.SendCommandJoint1(0)
if(joint==2):
self.sendCommand16( 0x20, (0x00), (0x00), True) #LEDs M2
self.sendCommand16( 0x23, (0x00), (0x0f), True) #I banks disabled M2
self.sendCommand16( 0x28, (0x00), (0x0f), True) #d banks disabled M2
self.sendCommand16( 0x32, (0x00), (0x0), True) #spike expansor M2
self.sendCommand16( 0x33, (0x00), (0x0f), True) #d banks disabled M2
self.sendCommand16( 0, (0x00), (0xFF), True) #LEDs M1
# self.sendCommand16( 0xff, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xfe, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xff, (0x00), (0x00), True) #FPGA reset
# self.sendCommand16( 0xfe, (0x00), (0x00), True) #FPGA reset
self.sendCommand16( 0x23, (0x00), (0x03), True) #I bank enable M2
self.sendCommand16( 0x28, (0x00), (0x03), True) #d bank enable M2
self.sendCommand16( 0x33, (0x00), (0x03), True) #d bank enable M2
self.sendCommand16( 0x32, ((self.d["Motor Config"]["spike_expansor_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M2"].get()) & 0xFF), True) #spike expansor M2
self.SendCommandJoint2(0)
if(joint==3):
self.sendCommand16( 0x40, (0x00), (0x00), True) #LEDs M3
self.sendCommand16( 0x43, (0x00), (0x0f), True) #I banks disabled M3
self.sendCommand16( 0x48, (0x00), (0x0f), True) #d banks disabled M3
self.sendCommand16( 0x52, (0x00), (0x0), True) #spike expansor M3
self.sendCommand16( 0x53, (0x00), (0x0f), True) #d banks disabled M3
self.sendCommand16( 0, (0x00), (0xFF), True) #LEDs M1
# self.sendCommand16( 0xff, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xfe, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xff, (0x00), (0x00), True) #FPGA reset
# self.sendCommand16( 0xfe, (0x00), (0x00), True) #FPGA reset
self.sendCommand16( 0x43, (0x00), (0x03), True) #I bank enable M3
self.sendCommand16( 0x48, (0x00), (0x03), True) #d bank enable M3
self.sendCommand16( 0x53, (0x00), (0x03), True) #d bank enable M3
self.sendCommand16( 0x52, ((self.d["Motor Config"]["spike_expansor_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M3"].get()) & 0xFF), True) #spike expansor M3
self.SendCommandJoint3(0)
if(joint==4):
self.sendCommand16( 0x60, (0x00), (0x00), True) #LEDs M4
self.sendCommand16( 0x63, (0x00), (0x0f), True) #I banks disabled M4
self.sendCommand16( 0x68, (0x00), (0x0f), True) #d banks disabled M4
self.sendCommand16( 0x72, (0x00), (0x0), True) #spike expansor M4
self.sendCommand16( 0x73, (0x00), (0x0f), True) #d banks disabled M4
self.sendCommand16( 0, (0x00), (0xFF), True) #LEDs M1
# self.sendCommand16( 0xff, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xfe, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xff, (0x00), (0x00), True) #FPGA reset
# self.sendCommand16( 0xfe, (0x00), (0x00), True) #FPGA reset
self.sendCommand16( 0x63, (0x00), (0x03), True) #I bank enable M4
self.sendCommand16( 0x68, (0x00), (0x03), True) #d bank enable M4
self.sendCommand16( 0x73, (0x00), (0x03), True) #d bank enable M4
self.sendCommand16( 0x72, ((self.d["Motor Config"]["spike_expansor_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M4"].get()) & 0xFF), True) #spike expansor M4
self.SendCommandJoint4(0)
if(joint==5):
self.sendCommand16( 0x80, (0x00), (0x00), True) #LEDs M5
self.sendCommand16( 0x83, (0x00), (0x0f), True) #I banks disabled M5
self.sendCommand16( 0x88, (0x00), (0x0f), True) #d banks disabled M5
self.sendCommand16( 0x92, (0x00), (0x0), True) #spike expansor M5
self.sendCommand16( 0x93, (0x00), (0x0f), True) #d banks disabled M5
self.sendCommand16( 0, (0x00), (0xFF), True) #LEDs M1
# self.sendCommand16( 0xff, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xfe, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xff, (0x00), (0x00), True) #FPGA reset
# self.sendCommand16( 0xfe, (0x00), (0x00), True) #FPGA reset
self.sendCommand16( 0x83, (0x00), (0x03), True) #I bank enable M5
self.sendCommand16( 0x88, (0x00), (0x03), True) #d bank enable M5
self.sendCommand16( 0x93, (0x00), (0x03), True) #d bank enable M5
self.sendCommand16( 0x92, ((self.d["Motor Config"]["spike_expansor_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M5"].get()) & 0xFF), True) #spike expansor M5
self.SendCommandJoint5(0)
if(joint==6):
self.sendCommand16( 0xA0, (0x00), (0x00), True) #LEDs M6
self.sendCommand16( 0xA3, (0x00), (0x0f), True) #I banks disabled M6
self.sendCommand16( 0xA8, (0x00), (0x0f), True) #d banks disabled M6
self.sendCommand16( 0xB2, (0x00), (0x0), True) #spike expansor M6
self.sendCommand16( 0xB3, (0x00), (0x0f), True) #d banks disabled M6
self.sendCommand16( 0, (0x00), (0xFF), True) #LEDs M1
# self.sendCommand16( 0xff, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xfe, (0xFF), (0xFF), True) #FPGA reset
# self.sendCommand16( 0xff, (0x00), (0x00), True) #FPGA reset
# self.sendCommand16( 0xfe, (0x00), (0x00), True) #FPGA reset
self.sendCommand16( 0xa3, (0x00), (0x03), True) #I bank enable M6
self.sendCommand16( 0xa8, (0x00), (0x03), True) #d bank enable M6
self.sendCommand16( 0xb3, (0x00), (0x03), True) #d bank enable M6
self.sendCommand16( 0xb2, ((self.d["Motor Config"]["spike_expansor_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M6"].get()) & 0xFF), True) #spike expansor M6
self.SendCommandJoint6(0)
def sendJ1FPGAReset(self):
self.SendFPGAReset_joint(1)
def sendJ2FPGAReset(self):
self.SendFPGAReset_joint(2)
def sendJ3FPGAReset(self):
self.SendFPGAReset_joint(3)
def sendJ4FPGAReset(self):
self.SendFPGAReset_joint(4)
def sendJ5FPGAReset(self):
self.SendFPGAReset_joint(5)
def sendJ6FPGAReset(self):
self.SendFPGAReset_joint(6)
def ScanAllMotor(self):
# '''
# Perform scan procedure on all 6 joints
# This function commands the robot to find
# its base position on all 6 joints at
# the same time
# '''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
else:
scan_Init_Value = self.d["Scan Parameters"]["scan_Init_Value"].get()
scan_Final_Value = self.d["Scan Parameters"]["scan_Final_Value"].get()
scan_Step_Value = self.d["Scan Parameters"]["scan_Step_Value"].get()
scan_Wait_Time = self.d["Scan Parameters"]["scan_Wait_Time"].get()
if self.checked_usb.get():
#Fecha en str, formato: yyyy_MM_dd_HH_mm_ss
date = datetime.datetime.now()
timeStamp = date.strftime("%Y_%b_%d_%H_%M_%S")
#Abrir archivo de log con el nombre de la fecha
filename='./logs/ScanAllmotor_' + timeStamp + '.log'
logger_file = logging.FileHandler(filename)
logger = logging.getLogger("logger_allmotors")
logger.addHandler(logger_file)
iSIV = scan_Init_Value
iSFV = scan_Final_Value
iSSV = scan_Step_Value
if scan_Init_Value > 200:
iSIV = 200
elif scan_Init_Value < -200:
iSIV = -200
if scan_Final_Value > 200:
iSFV = 200
elif scan_Final_Value < -200:
iSFV = -200
if scan_Step_Value > 200:
iSSV = 200
elif scan_Step_Value < -200:
iSSV = -200
self.sendCommand16( 0x03, (0x00), ((3)&0xFF), True) #I banks disabled M1 PI_bank_select_M1 = 3
self.sendCommand16( 0x07, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x08, (0x00), ((3)&0xFF), True) #D banks disabled M1 PD_bank_select_M1 = 3
self.sendCommand16( 0x0C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x12, ((self.d["Motor Config"]["spike_expansor_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M1"].get()) & 0xFF), True) #spike expansor M1
self.sendCommand16( 0x13, (0x00), ((3)&0xFF), True) #EI bank enabled M1 EI_bank_select_M1 = 3
self.sendCommand16( 0x17, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x02, ((iSIV >> 8) & 0xFF), ((iSIV) & 0xFF), True) #Ref M1 0
self.sendCommand16( 0x23, (0x00), ((3)&0xFF), True) #I banks disabled M1 PI_bank_select_M2 = 3
self.sendCommand16( 0x27, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x28, (0x00), ((3)&0xFF), True) #D banks disabled M1 PD_bank_select_M2 = 3
self.sendCommand16( 0x2C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x32, ((self.d["Motor Config"]["spike_expansor_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M2"].get()) & 0xFF), True) #spike expansor M1
self.sendCommand16( 0x33, (0x00), ((3)&0xFF), True) #EI bank enabled M1 EI_bank_select_M2 = 3
self.sendCommand16( 0x37, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x22, ((iSIV >> 8) & 0xFF), ((iSIV) & 0xFF), True) #Ref M1 0
self.sendCommand16( 0x43, (0x00), ((3)&0xFF), True) #I banks disabled M1 PI_bank_select_M3 = 3
self.sendCommand16( 0x47, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x48, (0x00), ((3)&0xFF), True) #D banks disabled M1 PD_bank_select_M3 = 3
self.sendCommand16( 0x4C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x52, ((self.d["Motor Config"]["spike_expansor_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M3"].get()) & 0xFF), True) #spike expansor M1
self.sendCommand16( 0x53, (0x00), ((3)&0xFF), True) #EI bank enabled M1 EI_bank_select_M2 = 3
self.sendCommand16( 0x57, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x42, ((iSIV >> 8) & 0xFF), ((iSIV) & 0xFF), True) #Ref M1 0
self.sendCommand16( 0x63, (0x00), ((3)&0xFF), True) #I banks disabled M4 PI_bank_select_M4 = 3
self.sendCommand16( 0x67, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x68, (0x00), ((3)&0xFF), True) #D banks disabled M4 PD_bank_select_M4 = 3
self.sendCommand16( 0x6C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x72, ((self.d["Motor Config"]["spike_expansor_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M4"].get()) & 0xFF), True) #spike expansor M4
self.sendCommand16( 0x73, (0x00), ((3)&0xFF), True) #EI bank enabled M4 EI_bank_select_M4 = 3
self.sendCommand16( 0x77, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x62, ((iSIV >> 8) & 0xFF), ((iSIV) & 0xFF), True)
'''
self.sendCommand16( 0x83, (0x00), ((3)&0xFF), True) #I banks disabled M5 PI_bank_select_M5 = 3
self.sendCommand16( 0x87, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x88, (0x00), ((3)&0xFF), True) #D banks disabled M5 PD_bank_select_M5 = 3
self.sendCommand16( 0x8C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x92, ((self.d["Motor Config"]["spike_expansor_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M5"].get()) & 0xFF), True) #spike expansor M5
self.sendCommand16( 0x93, (0x00), ((3)&0xFF), True) #EI bank enabled M5 EI_bank_select_M5 = 3
self.sendCommand16( 0x97, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x82, ((iSIV >> 8) & 0xFF), ((iSIV) & 0xFF), True) #Ref M5 0
self.sendCommand16( 0xA3, (0x00), ((3)&0xFF), True) #I banks disabled M6 PI_bank_select_M6 = 3
self.sendCommand16( 0xA7, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
self.sendCommand16( 0xA8, (0x00), ((3)&0xFF), True) #D banks disabled M6 PD_bank_select_M6 = 3
self.sendCommand16( 0xAC, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
self.sendCommand16( 0xB2, ((self.d["Motor Config"]["spike_expansor_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M6"].get()) & 0xFF), True) #spike expansor M6
self.sendCommand16( 0xB3, (0x00), ((3)&0xFF), True) #EI bank enabled M6 EI_bank_select_M6 = 3
self.sendCommand16( 0xB7, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
self.sendCommand16( 0xA2, ((iSIV >> 8) & 0xFF), ((iSIV) & 0xFF), True) #Ref M6 0
'''
logger.info("Time\tM1 Ref\tJ1 Pos\tM2 Ref\tJ2 Pos\tM3 Ref\tJ3 Pos\tM4 Ref\tJ4 Pos\tM5 Ref\tJ5 Pos\tM6 Ref\tJ6 Pos\t")
start = self.millis_now()
now = self.millis_now()
while(abs(now-start) < 1500):
lap = self.millis_now()
while(abs(now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((self.millis_now() - start),scan_Init_Value,self.Read_J1_pos(),scan_Init_Value,self.Read_J2_pos(),scan_Init_Value,self.Read_J3_pos(),scan_Init_Value,self.Read_J4_pos(),scan_Init_Value,self.Read_J5_pos(),scan_Init_Value,self.Read_J6_pos()))
now = self.millis_now()
for j in range(0,5):
i = iSIV
while( i <= iSFV):
#self.sendCommand16( 0xA2, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M6 I
#self.sendCommand16( 0x82, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M5 I
self.sendCommand16( 0x62, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M4 I
self.sendCommand16( 0x42, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M3 I
self.sendCommand16( 0x22, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M2 I
self.sendCommand16( 0x02, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M1 I
start2 = self.millis_now()
now = self.millis_now()
while(abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while(abs(now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),i,self.Read_J2_pos(),i,self.Read_J3_pos(),i,self.Read_J4_pos(),i,self.Read_J5_pos(),i,self.Read_J6_pos()))
i = i + iSSV
i = iSFV
while(i >= iSIV):
#self.sendCommand16( 0xA2, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M6 I
#self.sendCommand16( 0x82, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M5 I
self.sendCommand16( 0x62, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M4 I
self.sendCommand16( 0x42, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M3 I
self.sendCommand16( 0x22, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M2 I
self.sendCommand16( 0x02, ((i >> 8) & 0xFF), ((i) & 0xFF), True) #Ref M1 I
start2 = self.millis_now()
now = self.millis_now()
while(abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while(abs(now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),i,self.Read_J1_pos(),i,self.Read_J2_pos(),i,self.Read_J3_pos(),i,self.Read_J4_pos(),i,self.Read_J5_pos(),i,self.Read_J6_pos()))
now = self.millis_now()
i = i - iSSV
[docs] def SetAERIN_ref(self):
'''
Not used
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
else:
if self.checked_usb.get():
self.sendCommand16(0xF0,(0xFF),(0xFF), True)
self.sendCommand16(0xF0,(0xFF),(0xFF), True)
[docs] def SetUSBSPI_ref(self):
'''
Not used
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
else:
if self.checked_usb.get():
self.sendCommand16(0xF0,(0x00),(0x00), True)
self.sendCommand16(0xF0,(0x00),(0x00), True)
[docs] def resetUSB(self):
'''
Reset USB connection
This function resets USB connection by closing
and then reopening the device
'''
self.closeUSB()
self.dev = self.openUSB()
self.checked_usb.set(True)
[docs] def ConfigureLeds(self):
'''
Under development
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
else:
if self.checked_usb.get():
self.sendCommand16( 0, (0x00), ((self.d["Motor Config"]["leds_M1"].get()) & 0xFF), True) #LEDs M1
self.sendCommand16( 0x20, (0x00), ((self.d["Motor Config"]["leds_M2"].get()) & 0xFF), True) #LEDs M2
self.sendCommand16( 0x40, (0x00), ((self.d["Motor Config"]["leds_M3"].get()) & 0xFF), True) #LEDs M3
self.sendCommand16( 0x60, (0x00), ((self.d["Motor Config"]["leds_M4"].get()) & 0xFF), True) #LEDs M4
self.sendCommand16( 0x80, (0x00), ((self.d["Motor Config"]["leds_M5"].get()) & 0xFF), True) #LEDs M5
self.sendCommand16( 0xA0, (0x00), ((self.d["Motor Config"]["leds_M6"].get()) & 0xFF), True) #LEDs M6
[docs] def SwitchOffLEDS(self):
'''
Under development
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
else:
if self.checked_usb.get():
self.sendCommand16( 0, 0, 0, False) #LEDs M1 off
self.sendCommand16( 0x20, 0, 0, False) #LEDs M2 off
self.sendCommand16( 0x40, 0, 0, False) #LEDs M3 off
self.sendCommand16( 0x60, 0, 0, False) #LEDs M4 off
self.sendCommand16( 0x80, 0, 0, False) #LEDs M5 off
self.sendCommand16( 0xA0, 0, 0, False) #LEDs M6 off
[docs] def Draw8xy(self):
'''
Under development
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
else:
if self.checked_usb.get():
scan_Wait_Time = self.d["Scan Parameters"]["scan_Wait_Time"]
refsM1 = [0,-200,0,200,0]
refsM2 = [0,-50,0,-50,0]
refsM3 = [0, -200, 0, -200, 0]
refsM4 = [0, -200, 0, -200, 0]
date = datetime.datetime.now()
timeStamp = date.strftime("%Y_%b_%d_%H_%M_%S")
#Abrir archivo de log con el nombre de la fecha
filename='./logs/8xy' + timeStamp + '.log'
logger_file = logging.FileHandler(filename)
logger = logging.getLogger("logger_8xy")
logger.addHandler(logger_file)
logger.info("CITEC ED-BioRob Print 8 x,y Log file") #Se usa esta funcion??
self.sendCommand16( 0x03, (0x00), ((3)&0xFF), True) #I banks disabled M1
self.sendCommand16( 0x07, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x08, (0x00), ((3)&0xFF), True) #D banks disabled M1
self.sendCommand16( 0x0C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x12, ((self.d["Motor Config"]["spike_expansor_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M1"].get()) & 0xFF), True) #spike expansor M1
self.sendCommand16( 0x13, (0x00), ((3)&0xFF), True) #EI bank enabled M1
self.sendCommand16( 0x17, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x02, ((refsM1[0] >> 8) & 0xFF), ((refsM1[0]) & 0xFF), True) #Ref M1 0
self.sendCommand16( 0x23, (0x00), ((3)&0xFF), True) #I banks disabled M1
self.sendCommand16( 0x27, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x28, (0x00), ((3)&0xFF), True) #D banks disabled M1
self.sendCommand16( 0x2C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x32, ((self.d["Motor Config"]["spike_expansor_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M2"].get()) & 0xFF), True) #spike expansor M1
self.sendCommand16( 0x33, (0x00), ((3)&0xFF), True) #EI bank enabled M1
self.sendCommand16( 0x37, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x22, ((refsM2[0] >> 8) & 0xFF), ((refsM2[0]) & 0xFF), True) #Ref M1 0
self.sendCommand16( 0x43, (0x00), ((3)&0xFF), True) #I banks disabled M1
self.sendCommand16( 0x47, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x48, (0x00), ((3)&0xFF), True) #D banks disabled M1
self.sendCommand16( 0x4C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x52, ((self.d["Motor Config"]["spike_expansor_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M3"].get()) & 0xFF), True) #spike expansor M1
self.sendCommand16( 0x53, (0x00), ((3)&0xFF), True) #EI bank enabled M1
self.sendCommand16( 0x57, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x42, ((refsM3[0] >> 8) & 0xFF), ((refsM3[0]) & 0xFF), True) #Ref M1 0
self.sendCommand16( 0x63, (0x00), ((3)&0xFF), True) #I banks disabled M4
self.sendCommand16( 0x67, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x68, (0x00), ((3)&0xFF), True) #D banks disabled M4
self.sendCommand16( 0x6C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x72, ((self.d["Motor Config"]["spike_expansor_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M4"].get()) & 0xFF), True) #spike expansor M4
self.sendCommand16( 0x73, (0x00), ((3)&0xFF), True) #EI bank enabled M4
self.sendCommand16( 0x77, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x62, ((refsM4[0] >> 8) & 0xFF), ((refsM4[0]) & 0xFF), True) #Ref M4 0
logger.info("Time\tM1 Ref\tJ1 Pos\tM2 Ref\tJ2 Pos\tM3 Ref\tJ3 Pos\tM4 Ref\tJ4 Pos\t")
start = self.millis_now()
now = self.millis_now()
while(abs(now-start)< 3000):
lap = self.millis_now()
while(abs(now-lap)<100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((self.millis_now()-start),refsM1[0],self.Read_J1_pos(),refsM2[0],self.Read_J2_pos(),refsM3[0],self.Read_J3_pos(),refsM4[0],self.Read_J4_pos()))
now = self.millis_now()
for j in range(0,2):
for i in range(0,5):
self.sendCommand16( 0x62, ((refsM4[i] >> 8) & 0xFF), ((refsM4[i]) & 0xFF), True) #Ref M4 0
self.sendCommand16( 0x42, ((refsM3[i] >> 8) & 0xFF), ((refsM3[i]) & 0xFF), True) #Ref M4 0
self.sendCommand16( 0x22, ((refsM2[i] >> 8) & 0xFF), ((refsM2[i]) & 0xFF), True) #Ref M4 0
self.sendCommand16( 0x02, ((refsM1[i] >> 8) & 0xFF), ((refsM1[i]) & 0xFF), True) #Ref M4 0
start2 = self.millis_now()
now = self.millis_now()
while(abs(now-start2) < scan_Wait_Time):
lap = self.millis_now()
while(abs(now-lap) < 100):
now = self.millis_now()
logger.info("{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t,{}\t".format((now-start),refsM1[i],self.Read_J1_pos(),refsM2[i],self.Read_J2_pos(),refsM3[i],self.Read_J3_pos(),refsM4[i],self.Read_J4_pos()))
now = self.millis_now()
[docs] def search_Joint_home(self,JOINTNUM,pol):
'''
Implement the method of searching a joint's home.
First it moves the joint until it hits one
of its limits, then progressively moves the
joint in the opposite direction until
the controller receives the home signal
from the joint
Args:
JOINTNUM (int): Number of the joint we want to find home position for
pol (int): 1 or -1, depending on the joint
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.sendCommand16(0xF7,0xff,0xff,True) #enable counter reset when microswitch is hit
old_sj = 0x20000/4 #32768
sj = 0x20000/4 #32768
addr_j = 0x02 #2
inc_j = -50*pol
if JOINTNUM == 1:
self.SendCommandJoint1(inc_j)
elif JOINTNUM == 2:
self.SendCommandJoint2(inc_j)
elif JOINTNUM == 3:
self.SendCommandJoint3(inc_j)
elif JOINTNUM == 4:
self.SendCommandJoint4(inc_j)
elif JOINTNUM == 5:
self.SendCommandJoint5(inc_j)
elif JOINTNUM == 6:
self.SendCommandJoint6(inc_j)
time.sleep(2)
if JOINTNUM == 1:
sj = self.Read_J1_pos()
elif JOINTNUM == 2:
sj = self.Read_J2_pos()
elif JOINTNUM == 3:
sj = self.Read_J3_pos()
elif JOINTNUM == 4:
sj = self.Read_J4_pos()
elif JOINTNUM == 5:
sj = self.Read_J5_pos()
elif JOINTNUM == 6:
sj = self.Read_J6_pos()
while( abs(sj - old_sj) != 0 ):
inc_j = inc_j - 50*pol
if JOINTNUM == 1:
self.SendCommandJoint1(inc_j)
elif JOINTNUM == 2:
self.SendCommandJoint2(inc_j)
elif JOINTNUM == 3:
self.SendCommandJoint3(inc_j)
elif JOINTNUM == 4:
self.SendCommandJoint4(inc_j)
elif JOINTNUM == 5:
self.SendCommandJoint5(inc_j)
elif JOINTNUM == 6:
self.SendCommandJoint6(inc_j)
time.sleep(2)
old_sj = sj
if JOINTNUM == 1:
sj = self.Read_J1_pos()
elif JOINTNUM == 2:
sj = self.Read_J2_pos()
elif JOINTNUM == 3:
sj = self.Read_J3_pos()
elif JOINTNUM == 4:
sj = self.Read_J4_pos()
elif JOINTNUM == 5:
sj = self.Read_J5_pos()
elif JOINTNUM == 6:
sj = self.Read_J6_pos()
if( abs(sj - old_sj) < 0x5):
break
#self.SendFPGAReset()
self.SendFPGAReset_joint(JOINTNUM)
if JOINTNUM == 1:
self.Reset_J1_pos()
elif JOINTNUM == 2:
self.Reset_J2_pos()
elif JOINTNUM == 3:
self.Reset_J3_pos()
elif JOINTNUM == 4:
self.Reset_J4_pos()
elif JOINTNUM == 5:
self.Reset_J5_pos()
elif JOINTNUM == 6:
self.Reset_J6_pos()
if JOINTNUM == 1:
inc_j = 350*pol
self.SendCommandJoint1(inc_j)
elif JOINTNUM == 2:
inc_j = 400*pol
self.SendCommandJoint2(inc_j)
elif JOINTNUM == 3:
inc_j = 200*pol
self.SendCommandJoint3(inc_j)
elif JOINTNUM == 4:
inc_j = 10*pol
self.SendCommandJoint4(inc_j)
elif JOINTNUM == 5:
self.SendCommandJoint5(inc_j)
elif JOINTNUM == 6:
self.SendCommandJoint6(inc_j)
time.sleep(2)
if JOINTNUM == 1:
sj = self.Read_J1_pos()
elif JOINTNUM == 2:
sj = self.Read_J2_pos()
elif JOINTNUM == 3:
sj = self.Read_J3_pos()
elif JOINTNUM == 4:
sj = self.Read_J4_pos()
elif JOINTNUM == 5:
sj = self.Read_J5_pos()
elif JOINTNUM == 6:
sj = self.Read_J6_pos()
old_sj = sj + 1000
while( abs(old_sj - sj) > 200):
old_sj = sj
if JOINTNUM == 1:
sj = self.Read_J1_pos()
elif JOINTNUM == 2:
sj = self.Read_J2_pos()
elif JOINTNUM == 3:
sj = self.Read_J3_pos()
elif JOINTNUM == 4:
sj = self.Read_J4_pos()
elif JOINTNUM == 5:
sj = self.Read_J5_pos()
elif JOINTNUM == 6:
sj = self.Read_J6_pos()
while(abs(sj - (0x20000/4)) > 0x400):
inc_j = inc_j + (10*pol)
if JOINTNUM == 1:
self.SendCommandJoint1(inc_j)
elif JOINTNUM == 2:
self.SendCommandJoint2(inc_j)
elif JOINTNUM == 3:
self.SendCommandJoint3(inc_j)
elif JOINTNUM == 4:
self.SendCommandJoint4(inc_j)
elif JOINTNUM == 5:
self.SendCommandJoint5(inc_j)
elif JOINTNUM == 6:
self.SendCommandJoint6(inc_j)
time.sleep(1.5)
old_sj = sj
if JOINTNUM == 1:
sj = self.Read_J1_pos()
elif JOINTNUM == 2:
sj = self.Read_J2_pos()
elif JOINTNUM == 3:
sj = self.Read_J3_pos()
elif JOINTNUM == 4:
sj = self.Read_J4_pos()
elif JOINTNUM == 5:
sj = self.Read_J5_pos()
elif JOINTNUM == 6:
sj = self.Read_J6_pos()
if (abs(sj - (0x20000/4)) < 0x400 and abs(old_sj - sj) > 0x300):
self.SendFPGAReset_joint(JOINTNUM)
#self.ConfigureInit()
#self.ConfigureSPID_allJoints()
self.sendCommand16(0xF7,0x00,0x00,True) #disable counter reset when microswitch is hit
[docs] def search_Home_J1(self):
'''
Search Home position for joint 1
This function uses search_Joint_home function to
search the home position of joint 1
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.search_Joint_home(1,1)
[docs] def search_Home_J2(self):
'''
Search Home position for joint 2
This function uses search_Joint_home function to
search the home position of joint 2
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.search_Joint_home(2,-1)
[docs] def search_Home_J3(self):
'''
Search Home position for joint 3
This function uses search_Joint_home function to
search the home position of joint 3
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.search_Joint_home(3,-1)
[docs] def search_Home_J4(self):
'''
Search Home position for joint 4
This function uses search_Joint_home function to
search the home position of joint 4
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.search_Joint_home(4,-1)
[docs] def search_Home_J5(self):
'''
Search Home position for joint 5
This function uses search_Joint_home function to
search the home position of joint 5
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.search_Joint_home(5,-1)
[docs] def search_Home_J6(self):
'''
Search Home position for joint 6
This function uses search_Joint_home function to
search the home position of joint 6
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.search_Joint_home(6,-1)
[docs] def search_Home_all(self):
'''
Search Home position for all joints
This function uses search_Joint_home function to
search the home position of all joints
Be careful: this function won't stop execution until
it has completely finished rendering the graphical
interface stuck in the process, so bear that in mind when using it
'''
if self.checked_remote:
msg = "[4,S,-1,-1]"
self.mqtt_client.publish(self.topic,msg)
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.search_Joint_home(1,1)
self.search_Joint_home(2,-1)
self.search_Joint_home(3,-1)
self.search_Joint_home(4,-1)
self.search_Joint_home(5,-1)
self.search_Joint_home(6,-1)
#doGo_Home separado en 6
[docs] def send_Home_J1(self):
'''
Send Joint 1 to current home position
This function uses sendCommand16 to send
the first joint to its current home position,
which is specified by passing '0' as reference
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.sendCommand16( 0x03, (0x00), ((3)&0xFF), True) #I banks disabled M1 PI_bank_select_M1 = 3
self.sendCommand16( 0x07, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x08, (0x00), ((3)&0xFF), True) #D banks disabled M1 PD_bank_select_M1 = 3
self.sendCommand16( 0x0C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x12, ((self.d["Motor Config"]["spike_expansor_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M1"].get()) & 0xFF), True) #spike expansor M1
self.sendCommand16( 0x13, (0x00), ((3)&0xFF), True) #EI bank enabled M1 EI_bank_select_M1 = 3
self.sendCommand16( 0x17, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M1"].get()) & 0xFF), True) #FD I&G bank 3 M1
self.sendCommand16( 0x02, (0), (0), True) #Ref M1 0
#Go to home position
self.sendCommand16( 0x02, (( 0 >> 8) & 0xFF),((0) & 0xFF), True) # Ref M1 0
pass
[docs] def send_Home_J2(self):
'''
Send Joint 2 to current home position
This function uses sendCommand16 to send
the second joint to its current home position,
which is specified by passing '0' as reference
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.sendCommand16( 0x23, (0x00), ((3)&0xFF), True) #I banks disabled M2 PI_bank_select_M2 = 3
self.sendCommand16( 0x27, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M2
self.sendCommand16( 0x28, (0x00), ((3)&0xFF), True) #D banks disabled M2 PD_bank_select_M2 = 3
self.sendCommand16( 0x2C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M2
self.sendCommand16( 0x32, ((self.d["Motor Config"]["spike_expansor_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M2"].get()) & 0xFF), True) #spike expansor M2
self.sendCommand16( 0x33, (0x00), ((3)&0xFF), True) #EI bank enabled M2 EI_bank_select_M2 = 3
self.sendCommand16( 0x37, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M2"].get()) & 0xFF), True) #FD I&G bank 3 M2
self.sendCommand16( 0x22, (0), (0), True) #Ref M2 0
#Go to home position
self.sendCommand16( 0x22, (( 0 >> 8) & 0xFF),((0) & 0xFF), True) # Ref M2 0
pass
[docs] def send_Home_J3(self):
'''
Send Joint 3 to current home position
This function uses sendCommand16 to send
the third joint to its current home position,
which is specified by passing '0' as reference
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.sendCommand16( 0x43, (0x00), ((3)&0xFF), True) #I banks disabled M3 PI_bank_select_M3 = 3
self.sendCommand16( 0x47, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M3
self.sendCommand16( 0x48, (0x00), ((3)&0xFF), True) #D banks disabled M3 PD_bank_select_M3 = 3
self.sendCommand16( 0x4C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M3
self.sendCommand16( 0x52, ((self.d["Motor Config"]["spike_expansor_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M3"].get()) & 0xFF), True) #spike expansor M3
self.sendCommand16( 0x53, (0x00), ((3)&0xFF), True) #EI bank enabled M3 EI_bank_select_M3 = 3
self.sendCommand16( 0x57, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M3"].get()) & 0xFF), True) #FD I&G bank 3 M3
self.sendCommand16( 0x42, (0), (0), True) #Ref M3 0
#Go to home position
self.sendCommand16( 0x42, (( 0 >> 8) & 0xFF),((0) & 0xFF), True) # Ref M3 0
pass
[docs] def send_Home_J4(self):
'''
Send Joint 4 to current home position
This function uses sendCommand16 to send
the fourth joint to its current home position,
which is specified by passing '0' as reference
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.sendCommand16( 0x63, (0x00), ((3)&0xFF), True) #I banks disabled M4 PI_bank_select_M4 = 3
self.sendCommand16( 0x67, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x68, (0x00), ((3)&0xFF), True) #D banks disabled M4 PD_bank_select_M4 = 3
self.sendCommand16( 0x6C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x72, ((self.d["Motor Config"]["spike_expansor_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M4"].get()) & 0xFF), True) #spike expansor M4
self.sendCommand16( 0x73, (0x00), ((3)&0xFF), True) #EI bank enabled M4 EI_bank_select_M4 = 3
self.sendCommand16( 0x77, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M4"].get()) & 0xFF), True) #FD I&G bank 3 M4
self.sendCommand16( 0x62, (0), (0), True) #Ref M4 0
#Go to home position
self.sendCommand16( 0x62, (( 0 >> 8) & 0xFF),((0) & 0xFF), True) # Ref M4 0
pass
[docs] def send_Home_J5(self):
'''
Send Joint 5 to current home position
This function uses sendCommand16 to send
the fifth joint to its current home position,
which is specified by passing '0' as reference
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.sendCommand16( 0x83, (0x00), ((3)&0xFF), True) #I banks disabled M5 PI_bank_select_M5 = 3
self.sendCommand16( 0x87, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x88, (0x00), ((3)&0xFF), True) #D banks disabled M5 PD_bank_select_M5 = 3
self.sendCommand16( 0x8C, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x92, ((self.d["Motor Config"]["spike_expansor_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M5"].get()) & 0xFF), True) #spike expansor M5
self.sendCommand16( 0x93, (0x00), ((3)&0xFF), True) #EI bank enabled M5 EI_bank_select_M5 = 3
self.sendCommand16( 0x97, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M5"].get()) & 0xFF), True) #FD I&G bank 3 M5
self.sendCommand16( 0x82, (0), (0), True) #Ref M5 0
#Go to home position
self.sendCommand16( 0x82, (( 0 >> 8) & 0xFF),((0) & 0xFF), True) # Ref M5 0
pass
[docs] def send_Home_J6(self):
'''
Send Joint 6 to current home position
This function uses sendCommand16 to send
the sixth joint to its current home position,
which is specified by passing '0' as reference
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.sendCommand16( 0xA3, (0x00), ((3)&0xFF), True) #I banks disabled M6 PI_bank_select_M6 = 3
self.sendCommand16( 0xA7, ((self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PI_FD_bank3_18bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
self.sendCommand16( 0xA8, (0x00), ((3)&0xFF), True) #D banks disabled M6 PD_bank_select_M6 = 3
self.sendCommand16( 0xAC, ((self.d["Motor Config"]["PD_FD_bank3_22bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["PD_FD_bank3_22bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
self.sendCommand16( 0xB2, ((self.d["Motor Config"]["spike_expansor_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["spike_expansor_M6"].get()) & 0xFF), True) #spike expansor M6
self.sendCommand16( 0xB3, (0x00), ((3)&0xFF), True) #EI bank enabled M6 EI_bank_select_M6 = 3
self.sendCommand16( 0xB7, ((self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["EI_FD_bank3_18bits_M6"].get()) & 0xFF), True) #FD I&G bank 3 M6
self.sendCommand16( 0xA2, (0), (0), True) #Ref M6 0
#Go to home position
self.sendCommand16( 0xA2, (( 0 >> 8) & 0xFF),((0) & 0xFF), True) # Ref M6 0
pass
#doGo_Home y con todo junto
[docs] def send_Home_all(self):
'''
Send Joint 1-6 to current home position
This function uses sendCommand16 to send
all joints to its current home position,
using send_Home_JX functions, with X
being the joint's number
'''
if self.dev==None:
self.alert("There is no opened device. Try opening one first")
return
self.send_Home_J1()
self.send_Home_J2()
self.send_Home_J3()
self.send_Home_J4()
self.send_Home_J5()
self.send_Home_J6()
[docs] def init_config(self):
'''
Load initial config
This function tries to load the initial configuration
for the program containing all the necessary values
that make the SPID control work properly. There must be
an "initial_config.json" file for this to work properly
and, in case you are missing it, you can download the file
directly from the latest master branch of the repository
'''
try:
f = open('./initial_config.json')
j = json.loads(f.read())
except FileNotFoundError:
self.alert("Initial config file missing. Please download initial_config.json from the repository if you want the default configuration")
try:
for key in self.d["Motor Config"].keys():
self.d["Motor Config"][key].set(j["Motor Config"][key])
for key in self.d["Scan Parameters"].keys():
self.d["Scan Parameters"][key].set(j["Scan Parameters"][key])
for key in self.d["Dynapse2"].keys():
self.d["Dynapse2"][key].set(j["Dynapse2"][key])
return
#If we cacth a KeyError, the config is invalid, so alert the user and end execution
except KeyError:
self.alert("Invalid config file")
return
[docs] @staticmethod
def ref_to_angle(motor,ref,strict=True):
"""
Convert reference of motor to angle
This function takes a motor and a reference and
returns the corresponding angle to said reference
for that specific motor
Args:
motor (int): Number of the motor (1-4)
ref (int): reference to be converted
strict (boolean): Whether to restrict angle values to ther maximum bounds or not (default is True)
Returns:
float: Angle that corresponds to the reference given for the given motor or the joint's limit if the calculated reference is above (or below) it
"""
f = lambda x:x
bounds = [[155,-155,],[85,-85],[112.5,-112.5],[90,-90]]
#############DEPRECATED################
# if motor == 1:
# f = lambda x: (-1/3)*x
# elif motor == 2:
# f = lambda x: ((-11/100)*x) + 53
# elif motor == 3:
# #To be characterised
# if ((ref >=-400) and (ref <= 200)):
# f = lambda x: ((-67/200)*x) + 11
# else:
# self.alert("Joint 3 out of range. Ref must be between 200 and -400")
# return
# elif motor == 4:
# f = lambda x:((9/50)*x) + 2
########################################
if motor == 1:
f = lambda x:-(1/3)*x
elif motor == 2:
f = lambda x:-(1/9.4)*x
elif motor == 3:
f = lambda x:-(1/-3.0)*x
elif motor == 4:
f = lambda x: -0.056780795*x
ret = f(ref)
if not(ret < bounds[motor-1][0] and ret > bounds[motor-1][1]) and strict:
ret = bounds[motor-1][0]*np.sign(ret)
return ret
[docs] @staticmethod
def angle_to_ref(motor,angle):
"""
Convert angle of motor to reference
This function takes a motor and an angle and
returns the corresponding reference to said angle
for that specific motor.
Args:
motor (int): Number of the motor (1-4)
angle (int): angle to be converted
strict (boolean): Whether to restrict reference values to ther maximum bounds or not (default is True)
Returns:
float: Reference that corresponds to the angle given for the given motor or the joint's limit if the calculated reference is above (or below) it
"""
f = lambda x:x
#bounds = [[400,-400],[700,-900],[300,-400],[1583,-1583]]
##############DEPRECATED#############
#These are the inverse of the functions that appear in ref_to_angle function
# if motor == 1:
# f = lambda x: (-3)*x #https://www.symbolab.com/solver/function-inverse-calculator/inverse%20f%5Cleft(x%5Cright)%3D-%5Cfrac%7B1%7D%7B3%7Dx
# elif motor == 2:
# f = lambda x: -(((100*x) - 5300)/11) #https://www.symbolab.com/solver/function-inverse-calculator/inverse%20f%5Cleft(x%5Cright)%3D%20-%5Cfrac%7B11%7D%7B100%7Dx%2B53
# elif motor == 3:
# if ((angle >=-66.5) and (angle <= 143.5)):
# f = lambda x: -(((200*x)-2200)/67) #https://www.symbolab.com/solver/function-inverse-calculator/inverse%20f%5Cleft(x%5Cright)%3D-%5Cfrac%7B67%7D%7B200%7Dx%2B11
# else:
# self.alert("Joint 3 out of range. Angle must be between 143.5 and -66.5")
# return
# elif motor == 4:
# f = lambda x:-(((50*x)-100)/9) #https://www.symbolab.com/solver/function-inverse-calculator/inverse%20f%5Cleft(x%5Cright)%3D%5Cfrac%7B9%7D%7B50%7Dx%20%2B2
#####################################
if motor == 1:
#f = lambda x:-3.1428474*x
f = lambda x:-3*x
elif motor == 2:
#f = lambda x:-8.824695*x
f = lambda x:-9.4*x
elif motor == 3:
#f = lambda x:-3.4044209*x
f = lambda x:-3*x
elif motor ==4:
f = lambda x:-17.61158871*x
else:
return 0
ret = f(angle)
# if not(ret < bounds[motor-1][0] and ret > bounds[motor-1][1]) and strict:
# ret = bounds[motor-1][0]*np.sign(ret)
return ret
[docs] def SendCommandJoint1_lite(self):
'''
Send only reference to 1st joint
This function allows to send a reference
to the 1st joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
if self.checked_remote:
msg = "[2,1,na,{}]".format(self.d["Motor Config"]["ref_M1"].get())
self.mqtt_client.publish(self.topic,msg,qos=0)
pass
elif self.checked_usb:
self.sendCommand16( 0x02, ((self.d["Motor Config"]["ref_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M1"].get()) & 0xFF), True) #Ref M1 0
print("Reference sent:",self.d["Motor Config"]["ref_M1"].get())
self.sendCommand16( 0x02, ((self.d["Motor Config"]["ref_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M1"].get()) & 0xFF), True) #Ref M1 0
self.sendCommand16( 0x02, ((self.d["Motor Config"]["ref_M1"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M1"].get()) & 0xFF), True) #Ref M1 0
# pass
[docs] def SendCommandJoint2_lite(self):
'''
Send only reference to 2nd joint
This function allows to send a reference
to the 2nd joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
if self.checked_remote:
msg = "[2,2,na,{}]".format(self.d["Motor Config"]["ref_M2"].get())
self.mqtt_client.publish(self.topic,msg,qos=0)
elif self.checked_usb:
self.sendCommand16( 0x22, ((self.d["Motor Config"]["ref_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M2"].get()) & 0xFF), True) #Ref M2 0
print("Reference sent:",self.d["Motor Config"]["ref_M2"].get())
self.sendCommand16( 0x22, ((self.d["Motor Config"]["ref_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M2"].get()) & 0xFF), True) #Ref M2 0
self.sendCommand16( 0x22, ((self.d["Motor Config"]["ref_M2"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M2"].get()) & 0xFF), True) #Ref M2 0
# pass
[docs] def SendCommandJoint3_lite(self):
'''
Send only reference to 3rd joint
This function allows to send a reference
to the 3rd joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
if self.checked_remote:
msg = "[2,3,na,{}]".format(self.d["Motor Config"]["ref_M3"].get())
self.mqtt_client.publish(self.topic,msg,qos=0)
elif self.checked_usb:
self.sendCommand16( 0x42, ((self.d["Motor Config"]["ref_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M3"].get()) & 0xFF), True) #Ref M3 0
print("Reference sent:",self.d["Motor Config"]["ref_M3"].get())
self.sendCommand16( 0x42, ((self.d["Motor Config"]["ref_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M3"].get()) & 0xFF), True) #Ref M3 0
self.sendCommand16( 0x42, ((self.d["Motor Config"]["ref_M3"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M3"].get()) & 0xFF), True) #Ref M3 0
# pass
[docs] def SendCommandJoint4_lite(self):
'''
Send only reference to 4th joint
This function allows to send a reference
to the 4th joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
if self.checked_remote:
msg = "[2,4,na,{}]".format(self.d["Motor Config"]["ref_M4"].get())
self.mqtt_client.publish(self.topic,msg,qos=0)
elif self.checked_usb:
self.sendCommand16( 0x62, ((self.d["Motor Config"]["ref_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M4"].get()) & 0xFF), True) #Ref M4 0
print("Reference sent:",self.d["Motor Config"]["ref_M4"].get())
self.sendCommand16( 0x62, ((self.d["Motor Config"]["ref_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M4"].get()) & 0xFF), True) #Ref M4 0
self.sendCommand16( 0x62, ((self.d["Motor Config"]["ref_M4"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M4"].get()) & 0xFF), True) #Ref M4 0
# pass
[docs] def SendCommandJoint5_lite(self):
'''
Send only reference to 5th joint
This function allows to send a reference
to the 5th joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
if self.checked_remote:
msg = "[2,5,na,{}]".format(self.d["Motor Config"]["ref_M5"].get())
self.mqtt_client.publish(self.topic,msg,qos=0)
elif self.checked_usb:
self.sendCommand16( 0x82, ((self.d["Motor Config"]["ref_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M5"].get()) & 0xFF), True) #Ref M5 0
print("Reference sent:",self.d["Motor Config"]["ref_M5"].get())
self.sendCommand16( 0x82, ((self.d["Motor Config"]["ref_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M5"].get()) & 0xFF), True) #Ref M5 0
self.sendCommand16( 0x82, ((self.d["Motor Config"]["ref_M5"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M5"].get()) & 0xFF), True) #Ref M5 0
pass
[docs] def SendCommandJoint6_lite(self):
'''
Send only reference to 6th joint
This function allows to send a reference
to the 6th joint in order to move it,
reference to angle are mapped in angle_to_ref function
'''
if self.checked_remote:
msg = "[2,6,na,{}]".format(self.d["Motor Config"]["ref_M6"].get())
self.mqtt_client.publish(self.topic,msg,qos=0)
elif self.checked_usb:
self.sendCommand16( 0xA2, ((self.d["Motor Config"]["ref_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M6"].get()) & 0xFF), True) #Ref M6 0
self.sendCommand16( 0xA2, ((self.d["Motor Config"]["ref_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M6"].get()) & 0xFF), True) #Ref M6 0
self.sendCommand16( 0xA2, ((self.d["Motor Config"]["ref_M6"].get() >> 8) & 0xFF), ((self.d["Motor Config"]["ref_M6"].get()) & 0xFF), True) #Ref M6 0
# pass
[docs] def send_dynapse2(self):
'''
Sends dynapse2 filter threshold and reset for joint1 now.
'''
self.sendCommand16( 0xE1, ((self.d["Dynapse2"]["Threshold1"].get() >> 8) & 0xFF), ((self.d["Dynapse2"]["Threshold1"].get()) & 0xFF), True) #
print("Reference sent:",self.d["Dynapse2"]["Threshold1"].get())
self.sendCommand16( 0xE1, ((self.d["Dynapse2"]["Threshold1"].get() >> 8) & 0xFF), ((self.d["Dynapse2"]["Threshold1"].get()) & 0xFF), True) #
self.sendCommand16( 0xE9, ((self.d["Dynapse2"]["Reset1"].get() >> 8) & 0xFF), ((self.d["Dynapse2"]["Reset1"].get()) & 0xFF), True) #
print("Reference sent:",self.d["Dynapse2"]["Reset1"].get())
self.sendCommand16( 0xE9, ((self.d["Dynapse2"]["Reset1"].get() >> 8) & 0xFF), ((self.d["Dynapse2"]["Reset1"].get()) & 0xFF), True) #
# pass
def devmem(self,addr,length,data=None):
cmd = "devmem " + hex(addr) + " " + str(length)
if data is not None:
cmd += " " + str(data)
print("Executing command: ",cmd)
#system(cmd)
proc = subprocess.Popen(cmd.split(' '), stdout=subprocess.PIPE)
tmp = proc.stdout.read()
return tmp
def execute_script(self,script):
cmd = script
proc = proc = subprocess.Popen(cmd.split(' '), stdout=subprocess.PIPE)
tmp = proc.stdout.read()
return tmp
def calculate_error(self,motor, gt, cmd, t='ref'):
if t not in ['ref','angle','counter']:
raise TypeError('Type not supported. Type must be one of ["ref","angle","counter"]')
else:
if t=='ref':
#Convert ground truth to ref
gt = self.count_to_ref(motor,gt)
pass
elif t =='angle':
#Convert both to angles
cmd = self.ref_to_angle(motor,cmd)
gt = self.count_to_angle(motor,gt)
pass
elif t=='counter':
#Convert commanded to counter
cmd = self.ref_to_count(motor,cmd)
pass
error = []
for ground_truth,command in zip(gt,cmd):
rmse = np.sqrt(np.mean((command - ground_truth)**2))
error.append(rmse)
return error
[docs] @staticmethod
def count_to_ref(motor,count):
"""
Convert counter of motor to reference
This function takes a motor and its collected position and
returns the corresponding reference to said position
for that specific motor.
Args:
motor (int): Number of the motor (1-4)
count (int): Position counter to be converted
Returns:
float: Reference that corresponds to the position given for the given motor
"""
f = lambda x:x
if motor == 1:
f = lambda x:0.02478*(x-32768)
elif motor == 2:
f = lambda x:0.0677*(x-32768)
elif motor == 3:
f = lambda x:(1/44.3)*(x-32768)
elif motor ==4:
f = lambda x:0.2182353*(x-32768)
else:
return 0
return f(count)
[docs] @staticmethod
def ref_to_count(motor,ref):
"""
Convert reference of motor to counter (estimated) absolute position
This function takes a motor and an arbitrary reference and
returns the corresponding estimated position for said reference
for that specific motor.
Args:
motor (int): Number of the motor (1-4)
ref (int): Reference to be converted
Returns:
float: Absolute estimated position that corresponds to the reference given for the given motor
"""
f = lambda x:x
if motor == 1:
f = lambda x:40.35269645959781*x + 32768
elif motor == 2:
f = lambda x:14.770677455806219*x + 32768
elif motor == 3:
f = lambda x:44.3*x + 32768
elif motor ==4:
f = lambda x:4.582209206643176*x + 32768
else:
return 0
return f(ref)
[docs] @staticmethod
def count_to_angle(motor,count):
"""
Convert counter of motor to estimated angle
This function takes a motor and its collected position and
returns the corresponding estimated angle to said position
for that specific motor.
Args:
motor (int): Number of the motor (1-4)
count (int): Position counter to be converted
Returns:
float: Estimated angle that corresponds to the position given for the given motor
"""
f = lambda x:x
if motor == 1:
f = lambda x:(1/125.5)*(x-32768)
elif motor == 2:
f = lambda x:(1/131)*(x-32768)
elif motor == 3:
f = lambda x:(1/132.5)*(x-32768)
elif motor ==4:
f = lambda x:0.012391573729863692*(x-32768)
else:
f = lambda x:x
return f(count)
[docs] def traj_to_json(self):
'''
[[q1],q2],q3],q4] to [r1,r2,r3,r4,r5,r6] with padding
input file is .npy in qx format
'''
filename = filedialog.askopenfile(mode="r")
real_name = filename.name.split("/")[-1]
arr = np.load(filename.name)
df = pandas.DataFrame(arr)
out = a_to_j.angles_to_json(df)
savename = filedialog.asksaveasfilename()
f = open(savename,"w")
js = json.dump(out.tolist(),f,indent=4)
f.close()
self.alert("Saved output to file {}".format(savename))
def angles_to_ref(self):
filename = filedialog.askopenfile(mode="r")
real_name = filename.name.split("/")[-1]
arr = np.load(filename.name)
refs = a_to_r.angles_to_refs(arr)
savename = filedialog.asksaveasfilename()
np.save(savename,refs)
self.alert("Saved output to file {}".format(savename))
pass
def pad_traj(self):
pass
def count_to_xyz_json(self):
filename = filedialog.askopenfile(mode="r")
real_name = filename.name.split("/")[-1]
conts = np.array(json.load(open(filename.name,'r')))
xyz = c_to_xyz.cont_to_xyz(conts,True)
savename = filedialog.asksaveasfilename()
np.save(savename,xyz)
self.alert("Saved output to file {}".format(savename))
pass
def count_to_xyz_npy(self):
filename = filedialog.askopenfile(mode="r")
real_name = filename.name.split("/")[-1]
conts = np.load(filename.name)
xyz = c_to_xyz.cont_to_xyz(conts,True)
savename = filedialog.asksaveasfilename()
np.save(savename,xyz)
self.alert("Saved output to file {}".format(savename))
pass
def count_to_angles_json(self):
filename = filedialog.askopenfile(mode="r")
real_name = filename.name.split("/")[-1]
conts = np.array(json.load(open(filename.name,'r')))
qs,cs,timestamp = c_to_a.cont_to_angle(conts)
savename = filedialog.asksaveasfilename()
np.save(savename,qs)
self.alert("Saved output to file {}".format(savename))
pass
def count_to_angles_npy(self):
filename = filedialog.askopenfile(mode="r")
real_name = filename.name.split("/")[-1]
conts = np.load(filename.name)
qs,cs,timestamp = c_to_a.cont_to_angle(conts)
savename = filedialog.asksaveasfilename()
np.save(savename,qs)
self.alert("Saved output to file {}".format(savename))
pass
def angles_to_xyz(self):
filename = filedialog.askopenfile(mode="r")
real_name = filename.name.split("/")[-1]
angles = np.load(filename.name)
angles[:,0]=-angles[:,0]
angles[:,1]=-angles[:,1]
xyz,qs = a_to_xyz.angles_to_xyz(angles*(np.pi/180))
#savename = simpledialog.askstring("Output file name","Please write the name you want for the output file")
savename = filedialog.asksaveasfilename()
np.save(savename,xyz)
self.alert("Saved output to file {}".format(savename))
def w_to_angles(self):
filename = filedialog.askopenfile(mode="r")
real_name = filename.name.split("/")[-1]
w = np.load(filename.name)
angles = w_to_a.w_to_angles(w)
#savename = simpledialog.askstring("Output file name","Please write the name you want for the output file")
savename = filedialog.asksaveasfilename()
np.save(savename,angles)
self.alert("Saved output to file {}".format(savename))
def plot_traj_3d(self):
filename = filedialog.askopenfile(mode="r")
xyz = np.load(filename.name,allow_pickle=True)
plot3d(xyz[:,0],xyz[:,1],xyz[:,2],label="Trajectory data",title="3D Trajectory",order=False)
def plot_counters(self):
filename = filedialog.askopenfile(mode="r")
if filename.name.lower().endswith('.json'):
conts = np.array(json.load(open(filename.name,'r')))
elif filename.name.lower().endswith(('.npy','.p','.pkl')):
conts = np.load(filename.name,allow_pickle=True)
else:
self.alert("You tried to open an invalid file")
plotcounters(conts[:,:-1],label="Counter data",title="")
def plot_angles(self):
filename = filedialog.askopenfile(mode="r")
angles = np.load(filename.name,allow_pickle=True)
plotangles(angles,label="Angle data",title="Angle Space")
pass
# if __name__ == "__main__":
# config = pyAER()
# config.render_gui()
# pass