mx/smargopolo
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

stand vor active correction routine

Browse Source
This commit is contained in:
Wayne Glettig
2021-09-29 16:30:57 +02:00
parent b1d4f1b643
commit e3da60b469
29 changed files with 16 additions and 77089 deletions
Download Patch File Download Diff File Expand all files Collapse all files

181
python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatability_aerotech_2.py
View File

@@ -1,181 +0,0 @@
#!/usr/bin/env python
# This Script creates a lookup table from the measured error CSV
# Wayne Glettig, 16.7.2021
import rospy
from sensor_msgs.msg import JointState
import matplotlib.pyplot as plt
import requests
import time
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import epics
import csv
import math
DMS_X=[];
DMS_Y=[];
DMS_Z=[];
DMS_Seq=[];
DMS_Secs=[];
DMS_Nsecs=[];
OMEGA_inst=0
OMEGA=[];
def callback_LJUE9_JointState(data):
global OMEGA_inst
OMEGA_inst = data.position[0]
def callback_readbackCAL_JointState(data):
global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
DMS_X.append(data.position[0])
DMS_Y.append(data.position[1])
DMS_Z.append(data.position[2])
DMS_Secs.append(data.header.stamp.secs)
DMS_Nsecs.append(data.header.stamp.nsecs)
DMS_Seq.append(data.header.seq)
OMEGA.append(OMEGA_inst)
def getCurrentPoint():
return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
def set_axes_equal(ax):
'''Make axes of 3D plot have equal scale so that spheres appear as spheres,
cubes as cubes, etc.. This is one possible solution to Matplotlib's
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
Input
ax: a matplotlib axis, e.g., as output from plt.gca().
'''
x_limits = ax.get_xlim3d()
y_limits = ax.get_ylim3d()
z_limits = ax.get_zlim3d()
x_range = abs(x_limits[1] - x_limits[0])
x_middle = np.mean(x_limits)
y_range = abs(y_limits[1] - y_limits[0])
y_middle = np.mean(y_limits)
z_range = abs(z_limits[1] - z_limits[0])
z_middle = np.mean(z_limits)
print (f"x_range: {x_range}")
print (f"y_range: {y_range}")
print (f"z_range: {z_range}")
print (f"x_middle: {x_middle}")
print (f"y_middle: {y_middle}")
print (f"z_middle: {z_middle}")
# The plot bounding box is a sphere in the sense of the infinity
# norm, hence I call half the max range the plot radius.
plot_radius = 0.5*max([x_range, y_range, z_range])
ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
def calculate_correction(VECT):
current = VECT[0]
centre = (max(VECT) + min(VECT))/2.
correction = -(current-centre)
print (f"MAX= {max(VECT)}, MIN= {min(VECT)}")
print (f"current {current}")
print (f"centre {centre}")
print (f"CORRECTION: {correction}")
#if __name__ == '__main__':
### START SCRIPT HERE #########################################################
#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
print ("This script rotates the Aerotech OMEGA axis from 0-360deg,")
print ("and records the calibration tool position during this motion.")
print ("Make sure:")
print ("* OMEGA is ready to turn freely.")
print ("* Calibration Tool is ready, in contact and feedback is active.")
key=input ("OK to continue? (y/n) ")
if (key != "y"):
print ('Stopping script.')
exit(code=None)
print ("Setting up ROS")
#connect to ROS topics for OMEGA and DMS values:
rospy.init_node('DMS_Recorder', anonymous=True)
subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
subsDMS =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
print ("Starting data collection...")
smargopolo_server = "http://smargopolo:3000"
for i in range(0,5,1):
print("Move SHY to +2")
epics.caput("X06SA-ES-DF1:GMZ-INCP",2)
time.sleep(2)
startPoint = getCurrentPoint()
print("Move SHX to +2")
epics.caput("X06SA-ES-DF1:GMY-INCP",2)
time.sleep(2)
secondPoint = getCurrentPoint()
print("Move SHY to -2")
epics.caput("X06SA-ES-DF1:GMZ-INCP",-2)
time.sleep(2)
print("Move SHX to -2")
epics.caput("X06SA-ES-DF1:GMY-INCP",-2)
time.sleep(2)
print("Move SHZ to +1")
epics.caput("X06SA-ES-DF1:GMX-INCP",1)
time.sleep(2)
#stop ROS to stop measuring.
rospy.signal_shutdown('finished measuring')
################################################################################
fig = plt.figure()
ax=fig.add_subplot(111, projection='3d')
ax.plot(DMS_Z,DMS_X,DMS_Y)
ax.set_xlabel("DMS_Z")
ax.set_ylabel("DMS_X")
ax.set_zlabel("DMS_Y")
ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0], 'rx')
ax.plot([startPoint[2]],[startPoint[0]],[startPoint[1]],'rx')
ax.plot([secondPoint[2]], [secondPoint[0]], [secondPoint[1]], 'ro')
set_axes_equal(ax)
fig.show()
fig2 = plt.figure()
ax2=fig2.add_subplot(111)
ax2.plot(OMEGA, DMS_X, label='DMS_X')
ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
ax2.set_xlabel("OMEGA")
ax2.legend()
fig2.show()
#Omega Offset in degree
print ('Omega Offset in degree')
YOffset= secondPoint[1]-startPoint[1]
ZOffset= secondPoint[2]-startPoint[2]
OmegaOffset = (math.tan(ZOffset/YOffset))*(180/math.pi)
print (f"Omega Offset: {OmegaOffset}")
##########################################i######################################
# Save Data to CSV
rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
with open('measure_repeatability_aerotech_2.csv', 'w', newline='') as file:
writer = csv.writer(file)
writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
for row in rows:
writer.writerow(row)
input("done.")

184
python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatability_smargon_2.py
View File

@@ -1,184 +0,0 @@
#!/usr/bin/env pythoni
# This Script creates a lookup table from the measured error CSV
# Wayne Glettig, 16.7.2021
import rospy
from sensor_msgs.msg import JointState
import matplotlib.pyplot as plt
import requests
import time
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import epics
import csv
import math
DMS_X=[];
DMS_Y=[];
DMS_Z=[];
DMS_Seq=[];
DMS_Secs=[];
DMS_Nsecs=[];
OMEGA_inst=0
OMEGA=[];
def callback_LJUE9_JointState(data):
global OMEGA_inst
OMEGA_inst = data.position[0]
def callback_readbackCAL_JointState(data):
global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
DMS_X.append(data.position[0])
DMS_Y.append(data.position[1])
DMS_Z.append(data.position[2])
DMS_Secs.append(data.header.stamp.secs)
DMS_Nsecs.append(data.header.stamp.nsecs)
DMS_Seq.append(data.header.seq)
OMEGA.append(OMEGA_inst)
def getCurrentPoint():
return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
def set_axes_equal(ax):
'''Make axes of 3D plot have equal scale so that spheres appear as spheres,
cubes as cubes, etc.. This is one possible solution to Matplotlib's
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
Input
ax: a matplotlib axis, e.g., as output from plt.gca().
'''
x_limits = ax.get_xlim3d()
y_limits = ax.get_ylim3d()
z_limits = ax.get_zlim3d()
x_range = abs(x_limits[1] - x_limits[0])
x_middle = np.mean(x_limits)
y_range = abs(y_limits[1] - y_limits[0])
y_middle = np.mean(y_limits)
z_range = abs(z_limits[1] - z_limits[0])
z_middle = np.mean(z_limits)
print (f"x_range: {x_range}")
print (f"y_range: {y_range}")
print (f"z_range: {z_range}")
print (f"x_middle: {x_middle}")
print (f"y_middle: {y_middle}")
print (f"z_middle: {z_middle}")
# The plot bounding box is a sphere in the sense of the infinity
# norm, hence I call half the max range the plot radius.
plot_radius = 0.5*max([x_range, y_range, z_range])
ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
def calculate_correction(VECT):
current = VECT[0]
centre = (max(VECT) + min(VECT))/2.
correction = -(current-centre)
print (f"MAX= {max(VECT)}, MIN= {min(VECT)}")
print (f"current {current}")
print (f"centre {centre}")
print (f"CORRECTION: {correction}")
#if __name__ == '__main__':
### START SCRIPT HERE #########################################################
#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
print ("This script rotates the Aerotech OMEGA axis from 0-360deg,")
print ("and records the calibration tool position during this motion.")
print ("Make sure:")
print ("* OMEGA is ready to turn freely.")
print ("* Calibration Tool is ready, in contact and feedback is active.")
key=input ("OK to continue? (y/n) ")
if (key != "y"):
print ('Stopping script.')
exit(code=None)
print ("Setting up ROS")
#connect to ROS topics for OMEGA and DMS values:
rospy.init_node('DMS_Recorder', anonymous=True)
subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
subsDMS =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
print ("Starting data collection...")
smargopolo_server = "http://smargopolo:3000"
for i in range(0,5,1):
print("Move SHY to +2")
response = requests.put(smargopolo_server+'/nudgeBCS?BZ=2')
time.sleep(2)
startPoint = getCurrentPoint()
print("Move SHX to +1")
response = requests.put(smargopolo_server+'/nudgeBCS?BY=2')
time.sleep(2)
secondPoint = getCurrentPoint()
print("Move SHY to +1")
response = requests.put(smargopolo_server+'/nudgeBCS?BZ=-2')
time.sleep(2)
print("Move SHX to +1")
response = requests.put(smargopolo_server+'/nudgeBCS?BY=-2')
time.sleep(2)
print("Move SHZ to +1")
response = requests.put(smargopolo_server+'/nudgeBCS?BX=1')
time.sleep(2)
#stop ROS to stop measuring.
rospy.signal_shutdown('finished measuring')
################################################################################
fig = plt.figure()
ax=fig.add_subplot(111, projection='3d')
ax.plot(DMS_Z,DMS_X,DMS_Y)
ax.set_xlabel("DMS_Z")
ax.set_ylabel("DMS_X")
ax.set_zlabel("DMS_Y")
ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0], 'rx')
ax.plot([startPoint[2]],[startPoint[0]],[startPoint[1]],'rx')
ax.plot([secondPoint[2]], [secondPoint[0]], [secondPoint[1]], 'ro')
set_axes_equal(ax)
fig.show()
fig2 = plt.figure()
ax2=fig2.add_subplot(111)
ax2.plot(OMEGA, DMS_X, label='DMS_X')
ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
ax2.set_xlabel("OMEGA")
ax2.legend()
fig2.show()
#Omega Offset in degree
print ('Omega Offset in degree')
YOffset= secondPoint[1]-startPoint[1]
ZOffset= secondPoint[2]-startPoint[2]
OmegaOffset = (math.tan(ZOffset/YOffset))*(180/math.pi)
print (f"Omega Offset: {OmegaOffset}")
##########################################i######################################
# Save Data to CSV
rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
with open('measure_repeatability_smargon_2.csv', 'w', newline='') as file:
writer = csv.writer(file)
writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
for row in rows:
writer.writerow(row)
input("done.")

8343
python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatabilty_Aerotech_2.csv
View File

File diff suppressed because it is too large Load Diff

BIN
python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatabilty_Aerotech_2.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 30 KiB

BIN
python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatabilty_Aerotech_2_2.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 32 KiB

BIN
python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatabilty_Aerotech_2_3.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 30 KiB

BIN
python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatabilty_Aerotech_2_4Corrected.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 48 KiB

8344
python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatabilty_Smargon_2_noOmegaCorrection.csv
View File

File diff suppressed because it is too large Load Diff

BIN
python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatabilty_Smargon_2_noOmegaCorrection.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 34 KiB

8349
python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatabilty_Smargon_2_withOmegaCorrection.csv
View File

File diff suppressed because it is too large Load Diff

BIN
python_algorithms/Dominik/1_check_DMS_Alignment/210804_measure_repeatabilty_Smargon_2_withOmegaCorrection.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 32 KiB

8376
python_algorithms/Dominik/210803_measure_repeatabilty_Smargon_003.csv
View File

File diff suppressed because it is too large Load Diff

8376
python_algorithms/Dominik/210803_measure_repeatabilty_Smargon_005.csv
View File

File diff suppressed because it is too large Load Diff

181
python_algorithms/Dominik/210804_measure_repeatability_aerotech_2.py
View File

@@ -1,181 +0,0 @@
#!/usr/bin/env python
# This Script creates a lookup table from the measured error CSV
# Wayne Glettig, 16.7.2021
import rospy
from sensor_msgs.msg import JointState
import matplotlib.pyplot as plt
import requests
import time
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import epics
import csv
import math
DMS_X=[];
DMS_Y=[];
DMS_Z=[];
DMS_Seq=[];
DMS_Secs=[];
DMS_Nsecs=[];
OMEGA_inst=0
OMEGA=[];
def callback_LJUE9_JointState(data):
global OMEGA_inst
OMEGA_inst = data.position[0]
def callback_readbackCAL_JointState(data):
global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
DMS_X.append(data.position[0])
DMS_Y.append(data.position[1])
DMS_Z.append(data.position[2])
DMS_Secs.append(data.header.stamp.secs)
DMS_Nsecs.append(data.header.stamp.nsecs)
DMS_Seq.append(data.header.seq)
OMEGA.append(OMEGA_inst)
def getCurrentPoint():
return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
def set_axes_equal(ax):
'''Make axes of 3D plot have equal scale so that spheres appear as spheres,
cubes as cubes, etc.. This is one possible solution to Matplotlib's
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
Input
ax: a matplotlib axis, e.g., as output from plt.gca().
'''
x_limits = ax.get_xlim3d()
y_limits = ax.get_ylim3d()
z_limits = ax.get_zlim3d()
x_range = abs(x_limits[1] - x_limits[0])
x_middle = np.mean(x_limits)
y_range = abs(y_limits[1] - y_limits[0])
y_middle = np.mean(y_limits)
z_range = abs(z_limits[1] - z_limits[0])
z_middle = np.mean(z_limits)
print (f"x_range: {x_range}")
print (f"y_range: {y_range}")
print (f"z_range: {z_range}")
print (f"x_middle: {x_middle}")
print (f"y_middle: {y_middle}")
print (f"z_middle: {z_middle}")
# The plot bounding box is a sphere in the sense of the infinity
# norm, hence I call half the max range the plot radius.
plot_radius = 0.5*max([x_range, y_range, z_range])
ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
def calculate_correction(VECT):
current = VECT[0]
centre = (max(VECT) + min(VECT))/2.
correction = -(current-centre)
print (f"MAX= {max(VECT)}, MIN= {min(VECT)}")
print (f"current {current}")
print (f"centre {centre}")
print (f"CORRECTION: {correction}")
#if __name__ == '__main__':
### START SCRIPT HERE #########################################################
#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
print ("This script rotates the Aerotech OMEGA axis from 0-360deg,")
print ("and records the calibration tool position during this motion.")
print ("Make sure:")
print ("* OMEGA is ready to turn freely.")
print ("* Calibration Tool is ready, in contact and feedback is active.")
key=input ("OK to continue? (y/n) ")
if (key != "y"):
print ('Stopping script.')
exit(code=None)
print ("Setting up ROS")
#connect to ROS topics for OMEGA and DMS values:
rospy.init_node('DMS_Recorder', anonymous=True)
subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
subsDMS =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
print ("Starting data collection...")
smargopolo_server = "http://smargopolo:3000"
for i in range(0,5,1):
print("Move SHY to +2")
epics.caput("X06SA-ES-DF1:GMZ-INCP",2)
time.sleep(2)
startPoint = getCurrentPoint()
print("Move SHX to +2")
epics.caput("X06SA-ES-DF1:GMY-INCP",2)
time.sleep(2)
secondPoint = getCurrentPoint()
print("Move SHY to -2")
epics.caput("X06SA-ES-DF1:GMZ-INCP",-2)
time.sleep(2)
print("Move SHX to -2")
epics.caput("X06SA-ES-DF1:GMY-INCP",-2)
time.sleep(2)
print("Move SHZ to +1")
epics.caput("X06SA-ES-DF1:GMX-INCP",1)
time.sleep(2)
#stop ROS to stop measuring.
rospy.signal_shutdown('finished measuring')
################################################################################
fig = plt.figure()
ax=fig.add_subplot(111, projection='3d')
ax.plot(DMS_Z,DMS_X,DMS_Y)
ax.set_xlabel("DMS_Z")
ax.set_ylabel("DMS_X")
ax.set_zlabel("DMS_Y")
ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0], 'rx')
ax.plot([startPoint[2]],[startPoint[0]],[startPoint[1]],'rx')
ax.plot([secondPoint[2]], [secondPoint[0]], [secondPoint[1]], 'ro')
set_axes_equal(ax)
fig.show()
fig2 = plt.figure()
ax2=fig2.add_subplot(111)
ax2.plot(OMEGA, DMS_X, label='DMS_X')
ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
ax2.set_xlabel("OMEGA")
ax2.legend()
fig2.show()
#Omega Offset in degree
print ('Omega Offset in degree')
YOffset= secondPoint[1]-startPoint[1]
ZOffset= secondPoint[2]-startPoint[2]
OmegaOffset = (math.tan(ZOffset/YOffset))*(180/math.pi)
print (f"Omega Offset: {OmegaOffset}")
##########################################i######################################
# Save Data to CSV
rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
with open('measure_repeatability_aerotech_2.csv', 'w', newline='') as file:
writer = csv.writer(file)
writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
for row in rows:
writer.writerow(row)
input("done.")

184
python_algorithms/Dominik/210804_measure_repeatability_smargon_2.py
View File

@@ -1,184 +0,0 @@
#!/usr/bin/env pythoni
# This Script creates a lookup table from the measured error CSV
# Wayne Glettig, 16.7.2021
import rospy
from sensor_msgs.msg import JointState
import matplotlib.pyplot as plt
import requests
import time
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import epics
import csv
import math
DMS_X=[];
DMS_Y=[];
DMS_Z=[];
DMS_Seq=[];
DMS_Secs=[];
DMS_Nsecs=[];
OMEGA_inst=0
OMEGA=[];
def callback_LJUE9_JointState(data):
global OMEGA_inst
OMEGA_inst = data.position[0]
def callback_readbackCAL_JointState(data):
global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
DMS_X.append(data.position[0])
DMS_Y.append(data.position[1])
DMS_Z.append(data.position[2])
DMS_Secs.append(data.header.stamp.secs)
DMS_Nsecs.append(data.header.stamp.nsecs)
DMS_Seq.append(data.header.seq)
OMEGA.append(OMEGA_inst)
def getCurrentPoint():
return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
def set_axes_equal(ax):
'''Make axes of 3D plot have equal scale so that spheres appear as spheres,
cubes as cubes, etc.. This is one possible solution to Matplotlib's
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
Input
ax: a matplotlib axis, e.g., as output from plt.gca().
'''
x_limits = ax.get_xlim3d()
y_limits = ax.get_ylim3d()
z_limits = ax.get_zlim3d()
x_range = abs(x_limits[1] - x_limits[0])
x_middle = np.mean(x_limits)
y_range = abs(y_limits[1] - y_limits[0])
y_middle = np.mean(y_limits)
z_range = abs(z_limits[1] - z_limits[0])
z_middle = np.mean(z_limits)
print (f"x_range: {x_range}")
print (f"y_range: {y_range}")
print (f"z_range: {z_range}")
print (f"x_middle: {x_middle}")
print (f"y_middle: {y_middle}")
print (f"z_middle: {z_middle}")
# The plot bounding box is a sphere in the sense of the infinity
# norm, hence I call half the max range the plot radius.
plot_radius = 0.5*max([x_range, y_range, z_range])
ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
def calculate_correction(VECT):
current = VECT[0]
centre = (max(VECT) + min(VECT))/2.
correction = -(current-centre)
print (f"MAX= {max(VECT)}, MIN= {min(VECT)}")
print (f"current {current}")
print (f"centre {centre}")
print (f"CORRECTION: {correction}")
#if __name__ == '__main__':
### START SCRIPT HERE #########################################################
#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
print ("This script rotates the Aerotech OMEGA axis from 0-360deg,")
print ("and records the calibration tool position during this motion.")
print ("Make sure:")
print ("* OMEGA is ready to turn freely.")
print ("* Calibration Tool is ready, in contact and feedback is active.")
key=input ("OK to continue? (y/n) ")
if (key != "y"):
print ('Stopping script.')
exit(code=None)
print ("Setting up ROS")
#connect to ROS topics for OMEGA and DMS values:
rospy.init_node('DMS_Recorder', anonymous=True)
subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
subsDMS =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
print ("Starting data collection...")
smargopolo_server = "http://smargopolo:3000"
for i in range(0,5,1):
print("Move SHY to +2")
response = requests.put(smargopolo_server+'/nudgeBCS?BZ=2')
time.sleep(2)
startPoint = getCurrentPoint()
print("Move SHX to +1")
response = requests.put(smargopolo_server+'/nudgeBCS?BY=2')
time.sleep(2)
secondPoint = getCurrentPoint()
print("Move SHY to +1")
response = requests.put(smargopolo_server+'/nudgeBCS?BZ=-2')
time.sleep(2)
print("Move SHX to +1")
response = requests.put(smargopolo_server+'/nudgeBCS?BY=-2')
time.sleep(2)
print("Move SHZ to +1")
response = requests.put(smargopolo_server+'/nudgeBCS?BX=1')
time.sleep(2)
#stop ROS to stop measuring.
rospy.signal_shutdown('finished measuring')
################################################################################
fig = plt.figure()
ax=fig.add_subplot(111, projection='3d')
ax.plot(DMS_Z,DMS_X,DMS_Y)
ax.set_xlabel("DMS_Z")
ax.set_ylabel("DMS_X")
ax.set_zlabel("DMS_Y")
ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0], 'rx')
ax.plot([startPoint[2]],[startPoint[0]],[startPoint[1]],'rx')
ax.plot([secondPoint[2]], [secondPoint[0]], [secondPoint[1]], 'ro')
set_axes_equal(ax)
fig.show()
fig2 = plt.figure()
ax2=fig2.add_subplot(111)
ax2.plot(OMEGA, DMS_X, label='DMS_X')
ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
ax2.set_xlabel("OMEGA")
ax2.legend()
fig2.show()
#Omega Offset in degree
print ('Omega Offset in degree')
YOffset= secondPoint[1]-startPoint[1]
ZOffset= secondPoint[2]-startPoint[2]
OmegaOffset = (math.tan(ZOffset/YOffset))*(180/math.pi)
print (f"Omega Offset: {OmegaOffset}")
##########################################i######################################
# Save Data to CSV
rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
with open('measure_repeatability_smargon_2.csv', 'w', newline='') as file:
writer = csv.writer(file)
writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
for row in rows:
writer.writerow(row)
input("done.")

166
python_algorithms/Dominik/2_1_get_Calibrationpin_Geometry_SHX&SHY/measure_phi_rot.py
View File

@@ -1,166 +0,0 @@
#!/usr/bin/env python
# This Script creates a lookup table from the measured error CSV
# Wayne Glettig, 16.7.2021
import rospy
from sensor_msgs.msg import JointState
import matplotlib.pyplot as plt
import requests
import time
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import csv
DMS_X=[];
DMS_Y=[];
DMS_Z=[];
DMS_Seq=[];
DMS_Secs=[];
DMS_Nsecs=[];
OMEGA =0;
def callback_LJUE9_JointState(data):
global OMEGA
OMEGA = data.position[0]
def callback_readbackCAL_JointState(data):
global DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
DMS_X.append(data.position[0])
DMS_Y.append(data.position[1])
DMS_Z.append(data.position[2])
DMS_Secs.append(data.header.stamp.secs)
DMS_Nsecs.append(data.header.stamp.nsecs)
DMS_Seq.append(data.header.seq)
def set_axes_equal(ax):
'''Make axes of 3D plot have equal scale so that spheres appear as spheres,
cubes as cubes, etc.. This is one possible solution to Matplotlib's
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
Input
ax: a matplotlib axis, e.g., as output from plt.gca().
'''
x_limits = ax.get_xlim3d()
y_limits = ax.get_ylim3d()
z_limits = ax.get_zlim3d()
x_range = abs(x_limits[1] - x_limits[0])
x_middle = np.mean(x_limits)
y_range = abs(y_limits[1] - y_limits[0])
y_middle = np.mean(y_limits)
z_range = abs(z_limits[1] - z_limits[0])
z_middle = np.mean(z_limits)
print (f"x_range: {x_range}")
print (f"y_range: {y_range}")
print (f"z_range: {z_range}")
print (f"x_middle: {x_middle}")
print (f"y_middle: {y_middle}")
print (f"z_middle: {z_middle}")
# The plot bounding box is a sphere in the sense of the infinity
# norm, hence I call half the max range the plot radius.
plot_radius = 0.5*max([x_range, y_range, z_range])
ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
def calculate_correction_SHX(VECT):
current = VECT[0]
centre = (max(VECT) + min(VECT))/2.
correction = -(current-centre)
#print (f"MAX=: {max(VECT)}, MIN= {min(VECT)}")
#print (f"current: {current}")
#print (f"centre: {centre}")
print (f"CORRECTION value for SHX: {correction}")
def calculate_correction_SHY(VECT):
current = VECT[0]
centre = (max(VECT) + min(VECT))/2.
correction = -(current-centre)
#print (f"MAX=: {max(VECT)}, MIN= {min(VECT)}")
#print (f"current: {current}")
#print (f"centre: {centre}")
print (f"CORRECTION value for SHY: -{correction}")
#if __name__ == '__main__':
smargopolo_server = "http://smargopolo:3000"
response = requests.put(smargopolo_server+"/targetSCS?PHI=0")
print ("Setting up ROS")
#connect to ROS topics for OMEGA and DMS values:
rospy.init_node('DMS_Recorder', anonymous=True)
subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
subsDMS =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
time.sleep(1)
print ("Moving phi to -180deg")
response = requests.put(smargopolo_server+"/targetSCS?PHI=-90")
time.sleep(5)
response = requests.put(smargopolo_server+"/targetSCS?PHI=-180")
time.sleep(5)
print ("moving to phi=180deg")
response = requests.put(smargopolo_server+"/targetSCS?PHI=-90")
time.sleep(5)
response = requests.put(smargopolo_server+"/targetSCS?PHI=0")
time.sleep(5)
response = requests.put(smargopolo_server+"/targetSCS?PHI=90")
time.sleep(5)
response = requests.put(smargopolo_server+"/targetSCS?PHI=180")
time.sleep(5)
print ("moving to phi=0deg")
response = requests.put(smargopolo_server+"/targetSCS?PHI=0")
time.sleep(8)
#stop ROS to stop measuring.
rospy.signal_shutdown('finished measuring')
print ("Stopped collecting data.")
################################################################################
fig = plt.figure()
ax=fig.add_subplot(111, projection='3d')
ax.plot(DMS_Z,DMS_X,DMS_Y)
ax.set_xlabel("DMS_Z")
ax.set_ylabel("DMS_X")
ax.set_zlabel("DMS_Y")
ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0], 'rx')
set_axes_equal(ax)
fig.show()
fig2 = plt.figure()
ax2=fig2.add_subplot(111)
ax2.plot(DMS_X, label='DMS_X')
ax2.plot(DMS_Y, label='DMS_Y')
ax2.plot(DMS_Z, label='DMS_Z')
ax2.legend()
fig2.show()
################################################################################
calculate_correction_SHX(DMS_Y)
calculate_correction_SHY(DMS_Z)
################################################################################
# Save Data to CSV
rows = zip(DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
with open('measure_Phi_rot.csv', 'w', newline='') as file:
writer = csv.writer(file)
writer.writerow(["DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
for row in rows:
writer.writerow(row)

191
python_algorithms/Dominik/2_2_get_CalibrationPin_Geometry_SHZ/measure_chi_rot.py
View File

@@ -1,191 +0,0 @@
#!/usr/bin/env python
# This Script creates a lookup table from the measured error CSV
# Wayne Glettig, 16.7.2021
import rospy
from sensor_msgs.msg import JointState
import matplotlib.pyplot as plt
import requests
import time
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import csv
DMS_X=[];
DMS_Y=[];
DMS_Z=[];
DMS_Seq=[];
DMS_Secs=[];
DMS_Nsecs=[];
SCS_CHI=[];
#SCS_Seq=[];
#SCS_Secs=[];
#SCS_Nsecs=[];
OMEGA =0
CHI =0
def callback_LJUE9_JointState(data):
global OMEGA
OMEGA = data.position[0]
def callback_readbackCAL_JointState(data):
global DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs,CHI
DMS_X.append(data.position[0])
DMS_Y.append(data.position[1])
DMS_Z.append(data.position[2])
DMS_Secs.append(data.header.stamp.secs)
DMS_Nsecs.append(data.header.stamp.nsecs)
DMS_Seq.append(data.header.seq)
SCS_CHI.append(CHI)
def callback_readbackSCS_JointState(data):
global CHI
CHI = data.position[4]
def getCurrentPoint():
return [DMS_X[-1],DMS_Y[-1],DMS_Z[-1]]
def set_axes_equal(ax):
'''Make axes of 3D plot have equal scale so that spheres appear as spheres,
cubes as cubes, etc.. This is one possible solution to Matplotlib's
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
Input
ax: a matplotlib axis, e.g., as output from plt.gca().
'''
x_limits = ax.get_xlim3d()
y_limits = ax.get_ylim3d()
z_limits = ax.get_zlim3d()
x_range = abs(x_limits[1] - x_limits[0])
x_middle = np.mean(x_limits)
y_range = abs(y_limits[1] - y_limits[0])
y_middle = np.mean(y_limits)
z_range = abs(z_limits[1] - z_limits[0])
z_middle = np.mean(z_limits)
print (f"x_range: {x_range}")
print (f"y_range: {y_range}")
print (f"z_range: {z_range}")
print (f"x_middle: {x_middle}")
print (f"y_middle: {y_middle}")
print (f"z_middle: {z_middle}")
# The plot bounding box is a sphere in the sense of the infinity
# norm, hence I call half the max range the plot radius.
plot_radius = 0.5*max([x_range, y_range, z_range])
ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
def calculate_correction(VECT):
current = VECT[0]
centre = (max(VECT) + min(VECT))/2.
correction = -(current-centre)
#print (f"MAX=: {max(VECT)}, MIN= {min(VECT)}")
#print (f"current: {current}")
#print (f"centre: {centre}")
print (f"CORRECTION value for __: {correction}")
#######################################################################################
#if __name__ == '__main__':
smargopolo_server = "http://smargopolo:3000"
response = requests.put(smargopolo_server+"/targetSCS?CHI=0")
print ("Setting up ROS")
#connect to ROS topics for OMEGA and DMS values:
rospy.init_node('DMS_Recorder', anonymous=True)
subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
subsDMS =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
subsSCS =rospy.Subscriber("/readbackSCS", JointState, callback_readbackSCS_JointState)
time.sleep(1)
startPoint = getCurrentPoint()
print ("Moving chi to 90deg")
response = requests.put(smargopolo_server+"/targetSCS?CHI=30")
time.sleep(5)
response = requests.put(smargopolo_server+"/targetSCS?CHI=60")
time.sleep(5)
response = requests.put(smargopolo_server+"/targetSCS?CHI=90")
time.sleep(5)
endPoint = getCurrentPoint()
print ("Moving chi to 0deg")
response = requests.put(smargopolo_server+"/targetSCS?CHI=60")
time.sleep(5)
response = requests.put(smargopolo_server+"/targetSCS?CHI=30")
time.sleep(5)
response = requests.put(smargopolo_server+"/targetSCS?CHI=0")
time.sleep(7)
#response = requests.put(smargopolo_server+"/targetSCS?CHI=0")
#time.sleep(8)
#stop ROS to stop measuring.
rospy.signal_shutdown('finished measuring')
print ("Stopped collecting data.")
################################################################################
fig = plt.figure()
ax=fig.add_subplot(111, projection='3d')
ax.plot(DMS_Z,DMS_X,DMS_Y)
ax.set_xlabel("DMS_Z")
ax.set_ylabel("DMS_X")
ax.set_zlabel("DMS_Y")
ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0], 'rx')
ax.plot([startPoint[2]],[startPoint[0]],[startPoint[1]],'rx')
ax.plot([endPoint[2]], [endPoint[0]], [endPoint[1]], 'ro')
set_axes_equal(ax)
fig.show()
fig2 = plt.figure()
ax2=fig2.add_subplot(111)
ax2.plot(DMS_X, label='DMS_X')
ax2.plot(DMS_Y, label='DMS_Y')
ax2.plot(DMS_Z, label='DMS_Z')
ax2.legend()
fig2.show()
################################################################################
#calculate_correction(DMS_X)
#calculate_correction(DMS_Y)
#calculate_correction(DMS_Z)
################################################################################
#Find Centre point of CHI rotation in DMS_X DMS_Y plane (2D projection)
P0 = [startPoint[0], startPoint[1]]
P1 = [endPoint[0], endPoint[1]]
v01half = [ (P1[0]-P0[0]) / 2. , (P1[1]-P0[1]) / 2. ]
v01half90 = [ -v01half[1] , v01half[0] ]
v0c = [v01half[0]+v01half90[0], v01half[1]+v01half90[1]]
PC = [P0[0]+v0c[0], P0[1]+v0c[1]]
ax.plot([startPoint[2]], [PC[0]], [PC[1]], 'g+')
fig.show()
print (f"Centre Point of Quarter Circle: DMS_X: {PC[0]} DMS_Y: {PC[1]}")
print (f"Correction from current postion: in DMS_X: {v0c[0]} in DMS_Y: {v0c[1]}")
print (f"CORRECTION value for SHZ: {v0c[0]}")
################################################################################
# Save Data to CSV
rows = zip(SCS_CHI, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
with open('measure_chi_rot_OUTPUT.csv', 'w', newline='') as file:
writer = csv.writer(file)
writer.writerow(["SCS_CHI", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
for row in rows:
writer.writerow(row)
################################################################################

12834
python_algorithms/Dominik/3_get_Smargon_centeredToOmega_OX&OY/210804_measure_Omega_Rot_noCorrection.csv
View File

File diff suppressed because it is too large Load Diff

BIN
python_algorithms/Dominik/3_get_Smargon_centeredToOmega_OX&OY/210804_measure_Omega_Rot_noCorrection.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 59 KiB

BIN
python_algorithms/Dominik/3_get_Smargon_centeredToOmega_OX&OY/210804_measure_Omega_Rot_noCorrection_2.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 98 KiB

BIN
python_algorithms/Dominik/3_get_Smargon_centeredToOmega_OX&OY/210804_measure_Omega_Rot_noCorrection_3.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 50 KiB

12834
python_algorithms/Dominik/3_get_Smargon_centeredToOmega_OX&OY/210804_measure_Omega_Rot_withCorrection.csv
View File

File diff suppressed because it is too large Load Diff

BIN
python_algorithms/Dominik/3_get_Smargon_centeredToOmega_OX&OY/210804_measure_Omega_Rot_withCorrection_1.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 224 KiB

BIN
python_algorithms/Dominik/3_get_Smargon_centeredToOmega_OX&OY/210804_measure_Omega_Rot_withCorrection_2.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 122 KiB

BIN
python_algorithms/Dominik/3_get_Smargon_centeredToOmega_OX&OY/210804_measure_Omega_Rot_withCorrection_3.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 114 KiB

BIN
python_algorithms/Dominik/3_get_Smargon_centeredToOmega_OX&OY/210804_measure_Omega_Rot_withCorrection_4.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 26 KiB

180
python_algorithms/Dominik/3_get_Smargon_centeredToOmega_OX&OY/measure_omega_rot.py
View File

@@ -1,180 +0,0 @@
#!/usr/bin/env python
# This Script creates a lookup table from the measured error CSV
# Wayne Glettig, 16.7.2021
import rospy
from sensor_msgs.msg import JointState
import matplotlib.pyplot as plt
import requests
import time
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import epics
import csv
DMS_X=[];
DMS_Y=[];
DMS_Z=[];
DMS_Seq=[];
DMS_Secs=[];
DMS_Nsecs=[];
OMEGA_inst=0
OMEGA=[];
def callback_LJUE9_JointState(data):
global OMEGA_inst
OMEGA_inst = data.position[0]
def callback_readbackCAL_JointState(data):
global OMEGA,DMS_X,DMS_Y,DMS_Z,DMS_Seq,DMS_Secs,DMS_Nsecs
DMS_X.append(data.position[0])
DMS_Y.append(data.position[1])
DMS_Z.append(data.position[2])
DMS_Secs.append(data.header.stamp.secs)
DMS_Nsecs.append(data.header.stamp.nsecs)
DMS_Seq.append(data.header.seq)
OMEGA.append(OMEGA_inst)
def set_axes_equal(ax):
'''Make axes of 3D plot have equal scale so that spheres appear as spheres,
cubes as cubes, etc.. This is one possible solution to Matplotlib's
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
Input
ax: a matplotlib axis, e.g., as output from plt.gca().
'''
x_limits = ax.get_xlim3d()
y_limits = ax.get_ylim3d()
z_limits = ax.get_zlim3d()
x_range = abs(x_limits[1] - x_limits[0])
x_middle = np.mean(x_limits)
y_range = abs(y_limits[1] - y_limits[0])
y_middle = np.mean(y_limits)
z_range = abs(z_limits[1] - z_limits[0])
z_middle = np.mean(z_limits)
print (f"x_range: {x_range}")
print (f"y_range: {y_range}")
print (f"z_range: {z_range}")
print (f"x_middle: {x_middle}")
print (f"y_middle: {y_middle}")
print (f"z_middle: {z_middle}")
# The plot bounding box is a sphere in the sense of the infinity
# norm, hence I call half the max range the plot radius.
plot_radius = 0.5*max([x_range, y_range, z_range])
ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
def calculate_correction_OX(VECT):
current = VECT[0]
centre = (max(VECT) + min(VECT))/2.
correction = -(current-centre)
#print (f"MAX=: {max(VECT)}, MIN= {min(VECT)}")
#print (f"current: {current}")
#print (f"centre: {centre}")
print (f"CORRECTION value for OX: {correction}")
def calculate_correction_OY(VECT):
current = VECT[0]
centre = (max(VECT) + min(VECT))/2.
correction = -(current-centre)
#print (f"MAX=: {max(VECT)}, MIN= {min(VECT)}")
#print (f"current: {current}")
#print (f"centre: {centre}")
print (f"CORRECTION value for OY: {correction}")
#if __name__ == '__main__':
### START SCRIPT HERE #########################################################
#AEROTECH_EPICS_RECORD = "X06MX-ES-DF1"
AEROTECH_EPICS_RECORD = "X06SA-ES-DF1"
print ("This script rotates the Aerotech OMEGA axis from 0-360deg,")
print ("and records the calibration tool position during this motion.")
print ("Make sure:")
print ("* OMEGA is ready to turn freely.")
print ("* Calibration Tool is ready, in contact and feedback is active.")
key=input ("OK to continue? (y/n) ")
if (key != "y"):
print ('Stopping script.')
exit(code=None)
#make sure OMEGA is at 0deg
print ("Moving OMEGA to 0deg")
movebackSpeed = 40 # deg/s
currentOMEGA = epics.caget(AEROTECH_EPICS_RECORD + ":OMEGA-GETP")
epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETV", movebackSpeed)
epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETP", 0)
time.sleep(currentOMEGA/movebackSpeed + 2)
print ("Setting up ROS")
#connect to ROS topics for OMEGA and DMS values:
rospy.init_node('DMS_Recorder', anonymous=True)
subsOMEGA=rospy.Subscriber("/LJUE9_JointState", JointState, callback_LJUE9_JointState)
subsDMS =rospy.Subscriber("/readbackCAL", JointState, callback_readbackCAL_JointState)
print ("Starting data collection...")
print ("Moving OMEGA to 360deg ")
dataCollectionSpeed = 5 #deg/s
print (f"Data collection will take approx {360/dataCollectionSpeed+5} [s]")
epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETV",dataCollectionSpeed)
epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETP",360)
time.sleep(360/dataCollectionSpeed + 5)
#stop ROS to stop measuring.
rospy.signal_shutdown('finished measuring')
epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETV", movebackSpeed)
epics.caput(AEROTECH_EPICS_RECORD + ":OMEGA-SETP",0)
print ("Stopped collecting data.")
################################################################################
fig = plt.figure()
ax=fig.add_subplot(111, projection='3d')
ax.plot(DMS_Z,DMS_X,DMS_Y)
ax.set_xlabel("DMS_Z")
ax.set_ylabel("DMS_X")
ax.set_zlabel("DMS_Y")
ax.plot(DMS_Z[0:1],DMS_X[0],DMS_Y[0], 'rx')
set_axes_equal(ax)
fig.show()
fig2 = plt.figure()
ax2=fig2.add_subplot(111)
ax2.plot(OMEGA, DMS_X, label='DMS_X')
ax2.plot(OMEGA, DMS_Y, label='DMS_Y')
ax2.plot(OMEGA, DMS_Z, label='DMS_Z')
ax2.set_xlabel("OMEGA")
ax2.legend()
fig2.show()
################################################################################
calculate_correction_OX(DMS_Y)
calculate_correction_OY(DMS_Z)
################################################################################
# Save Data to CSV
rows = zip(OMEGA, DMS_X, DMS_Y, DMS_Z, DMS_Seq, DMS_Secs, DMS_Nsecs)
with open('measure_omega_rot_OUTPUT.csv', 'w', newline='') as file:
writer = csv.writer(file)
writer.writerow(["OMEGA", "DMS_X", "DMS_Y", "DMS_Z", "DMS_Seq", "DMS_Secs", "DMS_Nsecs"])
for row in rows:
writer.writerow(row)
input("done.")

8350
python_algorithms/Dominik/measure_repeatability.csv
View File

File diff suppressed because it is too large Load Diff