def configure_gui(STATE):
def getFile():
dataFile = tkinter.filedialog.askopenfile(
parent=STATE.mainGUI,
mode="rb",
title="Choose the data file (.JSON)",
initialdir=STATE.project_path.get(),
)
fileEntry.configure(state="normal")
fileEntry.delete(0, END)
fileEntry.insert(0, dataFile.name)
fileEntry.configure(state="readonly")
try:
data = json.load(dataFile)
try:
# Transform the data into a numpy array to make it easier to use
# -> transpose it so we can deal with it in columns
for k in JSON_DATA_KEYS:
data[k] = np.array(data[k]).transpose()
STATE.stored_data = data
analyzeButton.configure(state="normal")
except Exception as e:
messagebox.showerror(
"Error!",
"The structure of the data JSON was not recognized.\n" +
"Details\n" + repr(e),
)
return
except Exception as e:
messagebox.showerror(
"Error!",
"The JSON file could not be loaded.\n" + "Details:\n" +
repr(e),
parent=STATE.mainGUI,
)
return
def runAnalysis():
(
STATE.quasi_forward,
STATE.quasi_backward,
STATE.step_forward,
STATE.step_backward,
) = prepare_data(STATE.stored_data,
window=STATE.window_size.get(),
STATE=STATE)
if (STATE.quasi_forward is None or STATE.quasi_backward is None
or STATE.step_forward is None or STATE.step_backward is None):
return
if STATE.direction.get() == "Forward":
kg, kfr, kv, ka, rsquare = calcFit(STATE.quasi_forward,
STATE.step_forward)
elif STATE.direction.get() == "Backward":
kg, kfr, kv, ka, rsquare = calcFit(STATE.quasi_backward,
STATE.step_backward)
else:
kg, kfr, kv, ka, rsquare = calcFit(
np.concatenate((STATE.quasi_forward, STATE.quasi_backward),
axis=1),
np.concatenate((STATE.step_forward, STATE.step_backward),
axis=1),
)
STATE.kg.set(float("%.3g" % kg))
STATE.kfr.set(float("%.3g" % kfr))
STATE.kv.set(float("%.3g" % kv))
STATE.ka.set(float("%.3g" % ka))
STATE.r_square.set(float("%.3g" % rsquare))
calcGains()
timePlotsButton.configure(state="normal")
voltPlotsButton.configure(state="normal")
fancyPlotButton.configure(state="normal")
calcGainsButton.configure(state="normal")
def plotTimeDomain():
if STATE.direction.get() == "Forward":
_plotTimeDomain("Forward", STATE.quasi_forward, STATE.step_forward)
elif STATE.direction.get() == "Backward":
_plotTimeDomain("Backward", STATE.quasi_backward,
STATE.step_backward)
else:
_plotTimeDomain(
"Combined",
np.concatenate((STATE.quasi_forward, STATE.quasi_backward),
axis=1),
np.concatenate((STATE.step_forward, STATE.step_backward),
axis=1),
)
def plotVoltageDomain():
if STATE.direction.get() == "Forward":
_plotVoltageDomain("Forward", STATE.quasi_forward,
STATE.step_forward, STATE)
elif STATE.direction.get() == "Backward":
_plotVoltageDomain("Backward", STATE.quasi_backward,
STATE.step_backward, STATE)
else:
_plotVoltageDomain(
"Combined",
np.concatenate((STATE.quasi_forward, STATE.quasi_backward),
axis=1),
np.concatenate((STATE.step_forward, STATE.step_backward),
axis=1),
STATE,
)
def plot3D():
if STATE.direction.get() == "Forward":
_plot3D("Forward", STATE.quasi_forward, STATE.step_forward, STATE)
elif STATE.direction.get() == "Backward":
_plot3D("Backward", STATE.quasi_backward, STATE.step_backward,
STATE)
else:
_plot3D(
"Combined",
np.concatenate((STATE.quasi_forward, STATE.quasi_backward),
axis=1),
np.concatenate((STATE.step_forward, STATE.step_backward),
axis=1),
STATE,
)
def calcGains():
period = (STATE.period.get()
if not STATE.has_slave.get() else STATE.slave_period.get())
kp, kd = _calcGains(
STATE.kv.get(),
STATE.ka.get(),
STATE.qp.get(),
STATE.qv.get(),
STATE.max_effort.get(),
period,
STATE.measurement_delay.get(),
)
# Scale gains to output
kp = kp / 12 * STATE.max_controller_output.get()
kd = kd / 12 * STATE.max_controller_output.get()
# Rescale kD if not time-normalized
if not STATE.controller_time_normalized.get():
kd = kd / STATE.period.get()
# Get correct conversion factor for rotations
if STATE.units.get() == "Degrees":
rotation = 360
elif STATE.units.get() == "Radians":
rotation = 2 * math.pi
elif STATE.units.get() == "Rotations":
rotation = 1
else:
rotation = STATE.pulley_diam.get() * math.pi
# Convert to motor-controller native units
if STATE.controller_type.get() == "Talon":
kp = kp * rotation / (STATE.encoder_epr.get() *
STATE.gearing.get())
kd = kd * rotation / (STATE.encoder_epr.get() *
STATE.gearing.get())
STATE.kp.set(float("%.3g" % kp))
STATE.kd.set(float("%.3g" % kd))
def presetGains(*args):
# Note that all the delays are zero because the elevator characterizer only runs in position mode and most motor controllers do not have non-CAN (i.e. filtering) delay in position mode
presets = {
"Default":
lambda: (
STATE.max_controller_output.set(12),
STATE.period.set(0.02),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Onboard"),
STATE.measurement_delay.set(0),
),
"WPILib (2020-)":
lambda: (
STATE.max_controller_output.set(12),
STATE.period.set(0.02),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Onboard"),
# Note that the user will need to remember to set this if the onboard controller is getting delayed measurements
STATE.measurement_delay.set(0),
),
"WPILib (Pre-2020)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.05),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Onboard"),
# Note that the user will need to remember to set this if the onboard controller is getting delayed measurements
STATE.measurement_delay.set(0),
),
"Talon FX":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Talon"),
STATE.measurement_delay.set(0),
),
"Talon SRX (2020-)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Talon"),
STATE.measurement_delay.set(0),
),
"Talon SRX (Pre-2020)":
lambda: (
STATE.max_controller_output.set(1023),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Talon"),
STATE.measurement_delay.set(0),
),
"Spark MAX (brushless)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Spark"),
STATE.measurement_delay.set(0),
),
"Spark MAX (brushed)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Spark"),
STATE.measurement_delay.set(0),
),
}
presets.get(STATE.gain_units_preset.get(), "Default")()
def enablePulleyDiam(*args):
if (STATE.units.get() == "Feet" or STATE.units.get() == "Inches"
or STATE.units.get() == "Meters"):
diamEntry.configure(state="normal")
else:
diamEntry.configure(state="disabled")
def enableOffboard(*args):
if STATE.controller_type.get() == "Onboard":
gearingEntry.configure(state="disabled")
eprEntry.configure(state="disabled")
hasSlave.configure(state="disabled")
slavePeriodEntry.configure(state="disabled")
elif STATE.controller_type.get() == "Talon":
gearingEntry.configure(state="normal")
eprEntry.configure(state="normal")
hasSlave.configure(state="normal")
if STATE.has_slave.get():
slavePeriodEntry.configure(state="normal")
else:
slavePeriodEntry.configure(state="disabled")
else:
gearingEntry.configure(state="disabled")
eprEntry.configure(state="disabled")
hasSlave.configure(state="normal")
if STATE.has_slave.get():
slavePeriodEntry.configure(state="normal")
else:
slavePeriodEntry.configure(state="disabled")
# TOP OF WINDOW (FILE SELECTION)
topFrame = Frame(STATE.mainGUI)
topFrame.grid(row=0, column=0, columnspan=4)
Button(topFrame, text="Select Data File", command=getFile).grid(row=0,
column=0)
fileEntry = Entry(topFrame, width=80)
fileEntry.grid(row=0, column=1, columnspan=3)
fileEntry.configure(state="readonly")
Label(topFrame, text="Units:", width=10).grid(row=0, column=4)
unitChoices = {
"Feet", "Inches", "Meters", "Degrees", "Radians", "Rotations"
}
unitsMenu = OptionMenu(topFrame, STATE.units, *sorted(unitChoices))
unitsMenu.configure(width=10)
unitsMenu.grid(row=0, column=5, sticky="ew")
STATE.units.trace_add("write", enablePulleyDiam)
Label(topFrame, text="Pulley Diameter (units):",
anchor="e").grid(row=1, column=3, columnspan=2, sticky="ew")
diamEntry = FloatEntry(topFrame, textvariable=STATE.pulley_diam)
diamEntry.configure(state="disabled")
diamEntry.grid(row=1, column=5)
Label(topFrame, text="Direction:", width=10).grid(row=0, column=6)
directions = {"Combined", "Forward", "Backward"}
dirMenu = OptionMenu(topFrame, STATE.direction, *sorted(directions))
dirMenu.configure(width=10)
dirMenu.grid(row=0, column=7)
for child in topFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
# FEEDFORWARD ANALYSIS FRAME
ffFrame = Frame(STATE.mainGUI, bd=2, relief="groove")
ffFrame.grid(row=1, column=0, columnspan=3, sticky="ns")
Label(ffFrame, text="Feedforward Analysis").grid(row=0,
column=0,
columnspan=5)
analyzeButton = Button(ffFrame,
text="Analyze Data",
command=runAnalysis,
state="disabled")
analyzeButton.grid(row=1, column=0, sticky="ew")
timePlotsButton = Button(
ffFrame,
text="Time-Domain Diagnostics",
command=plotTimeDomain,
state="disabled",
)
timePlotsButton.grid(row=2, column=0, sticky="ew")
voltPlotsButton = Button(
ffFrame,
text="Voltage-Domain Diagnostics",
command=plotVoltageDomain,
state="disabled",
)
voltPlotsButton.grid(row=3, column=0, sticky="ew")
fancyPlotButton = Button(ffFrame,
text="3D Diagnostics",
command=plot3D,
state="disabled")
fancyPlotButton.grid(row=4, column=0, sticky="ew")
Label(ffFrame, text="Accel Window Size:", anchor="e").grid(row=1,
column=1,
sticky="ew")
windowEntry = IntEntry(ffFrame, textvariable=STATE.window_size, width=5)
windowEntry.grid(row=1, column=2)
Label(ffFrame, text="Motion Threshold (units/s):",
anchor="e").grid(row=2, column=1, sticky="ew")
thresholdEntry = FloatEntry(ffFrame,
textvariable=STATE.motion_threshold,
width=5)
thresholdEntry.grid(row=2, column=2)
Label(ffFrame, text="kG:", anchor="e").grid(row=1, column=3, sticky="ew")
kGEntry = FloatEntry(ffFrame, textvariable=STATE.kg, width=10)
kGEntry.grid(row=1, column=4)
kGEntry.configure(state="readonly")
Label(ffFrame, text="kFr:", anchor="e").grid(row=2, column=3, sticky="ew")
kFrEntry = FloatEntry(ffFrame, textvariable=STATE.kfr, width=10)
kFrEntry.grid(row=2, column=4)
kFrEntry.configure(state="readonly")
Label(ffFrame, text="kV:", anchor="e").grid(row=3, column=3, sticky="ew")
kVEntry = FloatEntry(ffFrame, textvariable=STATE.kv, width=10)
kVEntry.grid(row=3, column=4)
kVEntry.configure(state="readonly")
Label(ffFrame, text="kA:", anchor="e").grid(row=4, column=3, sticky="ew")
kAEntry = FloatEntry(ffFrame, textvariable=STATE.ka, width=10)
kAEntry.grid(row=4, column=4)
kAEntry.configure(state="readonly")
Label(ffFrame, text="r-squared:", anchor="e").grid(row=5,
column=3,
sticky="ew")
rSquareEntry = FloatEntry(ffFrame, textvariable=STATE.r_square, width=10)
rSquareEntry.grid(row=5, column=4)
rSquareEntry.configure(state="readonly")
for child in ffFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
# FEEDBACK ANALYSIS FRAME
fbFrame = Frame(STATE.mainGUI, bd=2, relief="groove")
fbFrame.grid(row=1, column=3, columnspan=5)
Label(fbFrame, text="Feedback Analysis").grid(row=0,
column=0,
columnspan=5)
Label(fbFrame, text="Gain Settings Preset:", anchor="e").grid(row=1,
column=0,
sticky="ew")
presetChoices = {
"Default",
"WPILib (2020-)",
"WPILib (Pre-2020)",
"Talon FX",
"Talon SRX (2020-)",
"Talon SRX (Pre-2020)",
"Spark MAX (brushless)",
"Spark MAX (brushed)",
}
presetMenu = OptionMenu(fbFrame, STATE.gain_units_preset,
*sorted(presetChoices))
presetMenu.grid(row=1, column=1)
presetMenu.config(width=12)
STATE.gain_units_preset.trace_add("write", presetGains)
Label(fbFrame, text="Controller Period (s):", anchor="e").grid(row=2,
column=0,
sticky="ew")
periodEntry = FloatEntry(fbFrame, textvariable=STATE.period, width=10)
periodEntry.grid(row=2, column=1)
Label(fbFrame, text="Max Controller Output:", anchor="e").grid(row=3,
column=0,
sticky="ew")
controllerMaxEntry = FloatEntry(fbFrame,
textvariable=STATE.max_controller_output,
width=10)
controllerMaxEntry.grid(row=3, column=1)
Label(fbFrame, text="Time-Normalized Controller:",
anchor="e").grid(row=4, column=0, sticky="ew")
normalizedButton = Checkbutton(fbFrame,
variable=STATE.controller_time_normalized)
normalizedButton.grid(row=4, column=1)
Label(fbFrame, text="Controller Type:", anchor="e").grid(row=5,
column=0,
sticky="ew")
controllerTypes = {"Onboard", "Talon", "Spark"}
controllerTypeMenu = OptionMenu(fbFrame, STATE.controller_type,
*sorted(controllerTypes))
controllerTypeMenu.grid(row=5, column=1)
STATE.controller_type.trace_add("write", enableOffboard)
Label(fbFrame, text="Measurement delay (ms):",
anchor="e").grid(row=6, column=0, sticky="ew")
velocityDelay = FloatEntry(fbFrame,
textvariable=STATE.measurement_delay,
width=10)
velocityDelay.grid(row=6, column=1)
Label(fbFrame, text="Post-Encoder Gearing:", anchor="e").grid(row=7,
column=0,
sticky="ew")
gearingEntry = FloatEntry(fbFrame, textvariable=STATE.gearing, width=10)
gearingEntry.configure(state="disabled")
gearingEntry.grid(row=7, column=1)
Label(fbFrame, text="Encoder EPR:", anchor="e").grid(row=8,
column=0,
sticky="ew")
eprEntry = IntEntry(fbFrame, textvariable=STATE.encoder_epr, width=10)
eprEntry.configure(state="disabled")
eprEntry.grid(row=8, column=1)
Label(fbFrame, text="Has Slave:", anchor="e").grid(row=9,
column=0,
sticky="ew")
hasSlave = Checkbutton(fbFrame, variable=STATE.has_slave)
hasSlave.grid(row=9, column=1)
hasSlave.configure(state="disabled")
STATE.has_slave.trace_add("write", enableOffboard)
Label(fbFrame, text="Slave Update Period (s):",
anchor="e").grid(row=10, column=0, sticky="ew")
slavePeriodEntry = FloatEntry(fbFrame,
textvariable=STATE.slave_period,
width=10)
slavePeriodEntry.grid(row=10, column=1)
slavePeriodEntry.configure(state="disabled")
Label(fbFrame, text="Max Acceptable Position Error (units):",
anchor="e").grid(row=1, column=2, columnspan=2, sticky="ew")
qPEntry = FloatEntry(fbFrame, textvariable=STATE.qp, width=10)
qPEntry.grid(row=1, column=4)
Label(fbFrame, text="Max Acceptable Velocity Error (units/s):",
anchor="e").grid(row=2, column=2, columnspan=2, sticky="ew")
qVEntry = FloatEntry(fbFrame, textvariable=STATE.qv, width=10)
qVEntry.grid(row=2, column=4)
Label(fbFrame, text="Max Acceptable Control Effort (V):",
anchor="e").grid(row=3, column=2, columnspan=2, sticky="ew")
effortEntry = FloatEntry(fbFrame, textvariable=STATE.max_effort, width=10)
effortEntry.grid(row=3, column=4)
Label(fbFrame, text="kV:", anchor="e").grid(row=5, column=2, sticky="ew")
kVFBEntry = FloatEntry(fbFrame, textvariable=STATE.kv, width=10)
kVFBEntry.grid(row=5, column=3)
Label(fbFrame, text="kA:", anchor="e").grid(row=6, column=2, sticky="ew")
kAFBEntry = FloatEntry(fbFrame, textvariable=STATE.ka, width=10)
kAFBEntry.grid(row=6, column=3)
calcGainsButton = Button(
fbFrame,
text="Calculate Optimal Controller Gains",
command=calcGains,
state="disabled",
)
calcGainsButton.grid(row=7, column=2, columnspan=3)
Label(fbFrame, text="kP:", anchor="e").grid(row=8, column=2, sticky="ew")
kPEntry = FloatEntry(fbFrame,
textvariable=STATE.kp,
width=10,
state="readonly").grid(row=8, column=3)
Label(fbFrame, text="kD:", anchor="e").grid(row=9, column=2, sticky="ew")
kDEntry = FloatEntry(fbFrame,
textvariable=STATE.kd,
width=10,
state="readonly").grid(row=9, column=3)
for child in fbFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
enableOffboard()
enablePulleyDiam()
def configure_gui(STATE):
def getFile():
dataFile = tkinter.filedialog.askopenfile(
parent=STATE.mainGUI,
mode="rb",
title="Choose the data file (.JSON)",
initialdir=STATE.project_path.get(),
)
fileEntry.configure(state="normal")
fileEntry.delete(0, END)
fileEntry.insert(0, dataFile.name)
fileEntry.configure(state="readonly")
try:
data = json.load(dataFile)
try:
# Transform the data into a numpy array to make it easier to use
# -> transpose it so we can deal with it in columns
for k in JSON_DATA_KEYS:
data[k] = np.array(data[k]).transpose()
STATE.stored_data = data
analyzeButton.configure(state="normal")
except Exception as e:
messagebox.showerror(
"Error!",
"The structure of the data JSON was not recognized.\n" +
"Details\n" + repr(e),
)
return
except Exception as e:
messagebox.showerror(
"Error!",
"The JSON file could not be loaded.\n" + "Details:\n" +
repr(e),
parent=STATE.mainGUI,
)
return
def runAnalysis():
(
STATE.quasi_forward_l,
STATE.quasi_backward_l,
STATE.step_forward_l,
STATE.step_backward_l,
STATE.quasi_forward_r,
STATE.quasi_backward_r,
STATE.step_forward_r,
STATE.step_backward_r,
) = prepare_data(STATE.stored_data,
window=STATE.window_size.get(),
STATE=STATE)
if (STATE.quasi_forward_l is None or STATE.quasi_backward_l is None
or STATE.step_forward_l is None
or STATE.step_backward_l is None
or STATE.quasi_forward_r is None
or STATE.quasi_backward_r is None
or STATE.step_forward_r is None
or STATE.step_backward_r is None):
return
if STATE.subset.get() == "Forward Left":
ks, kv, ka, rsquare = calcFit(STATE.quasi_forward_l,
STATE.step_forward_l)
elif STATE.subset.get() == "Forward Right":
ks, kv, ka, rsquare = calcFit(STATE.quasi_forward_r,
STATE.step_forward_r)
elif STATE.subset.get() == "Backward Left":
ks, kv, ka, rsquare = calcFit(STATE.quasi_backward_l,
STATE.step_backward_l)
elif STATE.subset.get() == "Backward Right":
ks, kv, ka, rsquare = calcFit(STATE.quasi_backward_r,
STATE.step_backward_r)
elif STATE.subset.get() == "Forward Combined":
ks, kv, ka, rsquare = calcFit(
np.concatenate((STATE.quasi_forward_l, STATE.quasi_forward_r),
axis=1),
np.concatenate((STATE.step_forward_l, STATE.step_forward_r),
axis=1),
)
elif STATE.subset.get() == "Backward Combined":
ks, kv, ka, rsquare = calcFit(
np.concatenate(
(STATE.quasi_backward_l, STATE.quasi_backward_r), axis=1),
np.concatenate((STATE.step_backward_l, STATE.step_backward_r),
axis=1),
)
elif STATE.subset.get() == "Left Combined":
ks, kv, ka, rsquare = calcFit(
np.concatenate((STATE.quasi_forward_l, STATE.quasi_backward_l),
axis=1),
np.concatenate((STATE.step_forward_l, STATE.step_backward_l),
axis=1),
)
elif STATE.subset.get() == "Right Combined":
ks, kv, ka, rsquare = calcFit(
np.concatenate((STATE.quasi_forward_r, STATE.quasi_backward_r),
axis=1),
np.concatenate((STATE.step_forward_r, STATE.step_backward_r),
axis=1),
)
else:
ks, kv, ka, rsquare = calcFit(
np.concatenate(
(
STATE.quasi_forward_l,
STATE.quasi_forward_r,
STATE.quasi_backward_l,
STATE.quasi_backward_r,
),
axis=1,
),
np.concatenate(
(
STATE.step_forward_l,
STATE.step_forward_r,
STATE.step_backward_l,
STATE.step_backward_r,
),
axis=1,
),
)
STATE.ks.set(float("%.3g" % ks))
STATE.kv.set(float("%.3g" % kv))
STATE.ka.set(float("%.3g" % ka))
STATE.r_square.set(float("%.3g" % rsquare))
if "track-width" in STATE.stored_data:
STATE.track_width.set(
calcTrackWidth(STATE.stored_data["track-width"]))
else:
STATE.track_width.set("N/A")
calcGains()
timePlotsButton.configure(state="normal")
voltPlotsButton.configure(state="normal")
fancyPlotButton.configure(state="normal")
calcGainsButton.configure(state="normal")
def plotTimeDomain():
if STATE.subset.get() == "Forward Left":
_plotTimeDomain("Forward Left", STATE.quasi_forward_l,
STATE.step_forward_l)
elif STATE.subset.get() == "Forward Right":
_plotTimeDomain("Forward Right", STATE.quasi_forward_r,
STATE.step_forward_r)
elif STATE.subset.get() == "Backward Left":
_plotTimeDomain("Backward Left", STATE.quasi_backward_l,
STATE.step_backward_l)
elif STATE.subset.get() == "Backward Right":
_plotTimeDomain("Backward Right", STATE.quasi_backward_r,
STATE.step_backward_r)
elif STATE.subset.get() == "Forward Combined":
_plotTimeDomain(
"Forward Combined",
np.concatenate((STATE.quasi_forward_l, STATE.quasi_forward_r),
axis=1),
np.concatenate((STATE.step_forward_l, STATE.step_forward_r),
axis=1),
)
elif STATE.subset.get() == "Backward Combined":
_plotTimeDomain(
"Backward Combined",
np.concatenate(
(STATE.quasi_backward_l, STATE.quasi_backward_r), axis=1),
np.concatenate((STATE.step_backward_l, STATE.step_backward_r),
axis=1),
)
elif STATE.subset.get() == "Left Combined":
_plotTimeDomain(
"Left Combined",
np.concatenate((STATE.quasi_forward_l, STATE.quasi_backward_l),
axis=1),
np.concatenate((STATE.step_forward_l, STATE.step_backward_l),
axis=1),
)
elif STATE.subset.get() == "Right Combined":
_plotTimeDomain(
"Right Combined",
np.concatenate((STATE.quasi_forward_r, STATE.quasi_backward_r),
axis=1),
np.concatenate((STATE.step_forward_r, STATE.step_backward_r),
axis=1),
)
else:
_plotTimeDomain(
"All Combined",
np.concatenate(
(
STATE.quasi_forward_l,
STATE.quasi_forward_r,
STATE.quasi_backward_l,
STATE.quasi_backward_r,
),
axis=1,
),
np.concatenate(
(
STATE.step_forward_l,
STATE.step_forward_r,
STATE.step_backward_l,
STATE.step_backward_r,
),
axis=1,
),
)
def plotVoltageDomain():
if STATE.subset.get() == "Forward Left":
_plotVoltageDomain("Forward Left", STATE.quasi_forward_l,
STATE.step_forward_l, STATE),
elif STATE.subset.get() == "Forward Right":
_plotVoltageDomain("Forward Right", STATE.quasi_forward_r,
STATE.step_forward_r, STATE)
elif STATE.subset.get() == "Backward Left":
_plotVoltageDomain("Backward Left", STATE.quasi_backward_l,
STATE.step_backward_l, STATE)
elif STATE.subset.get() == "Backward Right":
_plotVoltageDomain("Backward Right", STATE.quasi_backward_r,
STATE.step_backward_r, STATE)
elif STATE.subset.get() == "Forward Combined":
_plotVoltageDomain(
"Forward Combined",
np.concatenate((STATE.quasi_forward_l, STATE.quasi_forward_r),
axis=1),
np.concatenate((STATE.step_forward_l, STATE.step_forward_r),
axis=1),
STATE,
)
elif STATE.subset.get() == "Backward Combined":
_plotVoltageDomain(
"Backward Combined",
np.concatenate(
(STATE.quasi_backward_l, STATE.quasi_backward_r), axis=1),
np.concatenate((STATE.step_backward_l, STATE.step_backward_r),
axis=1),
STATE,
)
elif STATE.subset.get() == "Left Combined":
_plotVoltageDomain(
"Left Combined",
np.concatenate((STATE.quasi_forward_l, STATE.quasi_backward_l),
axis=1),
np.concatenate((STATE.step_forward_l, STATE.step_backward_l),
axis=1),
STATE,
)
elif STATE.subset.get() == "Right Combined":
_plotVoltageDomain(
"Right Combined",
np.concatenate((STATE.quasi_forward_r, STATE.quasi_backward_r),
axis=1),
np.concatenate((STATE.step_forward_r, STATE.step_backward_r),
axis=1),
STATE,
)
else:
_plotVoltageDomain(
"All Combined",
np.concatenate(
(
STATE.quasi_forward_l,
STATE.quasi_forward_r,
STATE.quasi_backward_l,
STATE.quasi_backward_r,
),
axis=1,
),
np.concatenate(
(
STATE.step_forward_l,
STATE.step_forward_r,
STATE.step_backward_l,
STATE.step_backward_r,
),
axis=1,
),
STATE,
)
def plot3D():
if STATE.subset.get() == "Forward Left":
_plot3D("Forward Left", STATE.quasi_forward_l,
STATE.step_forward_l, STATE)
elif STATE.subset.get() == "Forward Right":
_plot3D("Forward Right", STATE.quasi_forward_r,
STATE.step_forward_r, STATE)
elif STATE.subset.get() == "Backward Left":
_plot3D("Backward Left", STATE.quasi_backward_l,
STATE.step_backward_l, STATE)
elif STATE.subset.get() == "Backward Right":
_plot3D("Backward Right", STATE.quasi_backward_r,
STATE.step_backward_r, STATE)
elif STATE.subset.get() == "Forward Combined":
_plot3D(
"Forward Combined",
np.concatenate((STATE.quasi_forward_l, STATE.quasi_forward_r),
axis=1),
np.concatenate((STATE.step_forward_l, STATE.step_forward_r),
axis=1),
STATE,
)
elif STATE.subset.get() == "Backward Combined":
_plot3D(
"Backward Combined",
np.concatenate(
(STATE.quasi_backward_l, STATE.quasi_backward_r), axis=1),
np.concatenate((STATE.step_backward_l, STATE.step_backward_r),
axis=1),
STATE,
)
elif STATE.subset.get() == "Left Combined":
_plot3D(
"Left Combined",
np.concatenate((STATE.quasi_forward_l, STATE.quasi_backward_l),
axis=1),
np.concatenate((STATE.step_forward_l, STATE.step_backward_l),
axis=1),
STATE,
)
elif STATE.subset.get() == "Right Combined":
_plot3D(
"Right Combined",
np.concatenate((STATE.quasi_forward_r, STATE.quasi_backward_r),
axis=1),
np.concatenate((STATE.step_forward_r, STATE.step_backward_r),
axis=1),
STATE,
)
else:
_plot3D(
"All Combined",
np.concatenate(
(
STATE.quasi_forward_l,
STATE.quasi_forward_r,
STATE.quasi_backward_l,
STATE.quasi_backward_r,
),
axis=1,
),
np.concatenate(
(
STATE.step_forward_l,
STATE.step_forward_r,
STATE.step_backward_l,
STATE.step_backward_r,
),
axis=1,
),
STATE,
)
def calcGains():
period = (STATE.period.get()
if not STATE.has_slave.get() else STATE.slave_period.get())
if STATE.loop_type.get() == "Position":
kp, kd = _calcGainsPos(
STATE.kv.get(),
STATE.ka.get(),
STATE.qp.get(),
STATE.qv.get(),
STATE.max_effort.get(),
period,
)
else:
kp, kd = _calcGainsVel(
STATE.kv.get(),
STATE.ka.get(),
STATE.qv.get(),
STATE.max_effort.get(),
period,
)
# Scale gains to output
kp = kp / 12 * STATE.max_controller_output.get()
kd = kd / 12 * STATE.max_controller_output.get()
# Rescale kD if not time-normalized
if not STATE.controller_time_normalized.get():
kd = kd / STATE.period.get()
# Get correct conversion factor for rotations
if STATE.units.get() == "Radians":
rotation = 2 * math.pi
elif STATE.units.get() == "Rotations":
rotation = 1
else:
rotation = STATE.wheel_diam.get() * math.pi
# Convert to controller-native units
if STATE.controller_type.get() == "Talon":
kp = kp * rotation / (STATE.encoder_epr.get() *
STATE.gearing.get())
kd = kd * rotation / (STATE.encoder_epr.get() *
STATE.gearing.get())
if STATE.loop_type.get() == "Velocity":
kp = kp * 10
STATE.kp.set(float("%.3g" % kp))
STATE.kd.set(float("%.3g" % kd))
def calcTrackWidth(table):
# Note that this assumes the gyro angle is not modded (i.e. on [0, +infinity)),
# and that a positive angle travels in the counter-clockwise direction
d_left = table[-1][R_ENCODER_P_COL] - table[0][R_ENCODER_P_COL]
d_right = table[-1][L_ENCODER_P_COL] - table[0][L_ENCODER_P_COL]
d_angle = table[-1][GYRO_ANGLE_COL] - table[0][GYRO_ANGLE_COL]
if d_angle == 0:
messagebox.showerror(
"Error!",
"Change in gyro angle was 0... Is your gyro set up correctly?")
return 0.0
# The below comes from solving ω=(vr−vl)/2r for 2r
# Absolute values used to ensure the calculated value is always positive
# and to add robustness to sensor inversion
diameter = (abs(d_left) + abs(d_right)) / abs(d_angle)
return diameter
def presetGains(*args):
presets = {
"Default":
lambda: (
STATE.max_controller_output.set(12),
STATE.period.set(0.02),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Onboard"),
),
"WPILib (2020-)":
lambda: (
STATE.max_controller_output.set(12),
STATE.period.set(0.02),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Onboard"),
),
"WPILib (Pre-2020)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.05),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Onboard"),
),
"Talon (2020-)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Talon"),
),
"Talon (Pre-2020)":
lambda: (
STATE.max_controller_output.set(1023),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Talon"),
),
"Spark MAX":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Spark"),
),
}
presets.get(STATE.gain_units_preset.get(), "Default")()
def enableOffboard(*args):
if STATE.controller_type.get() == "Onboard":
gearingEntry.configure(state="disabled")
eprEntry.configure(state="disabled")
hasSlave.configure(state="disabled")
slavePeriodEntry.configure(state="disabled")
elif STATE.controller_type.get() == "Talon":
gearingEntry.configure(state="normal")
eprEntry.configure(state="normal")
hasSlave.configure(state="normal")
if STATE.has_slave.get():
slavePeriodEntry.configure(state="normal")
else:
slavePeriodEntry.configure(state="disabled")
else:
gearingEntry.configure(state="disabled")
eprEntry.configure(state="disabled")
hasSlave.configure(state="normal")
if STATE.has_slave.get():
slavePeriodEntry.configure(state="normal")
else:
slavePeriodEntry.configure(state="disabled")
def enableWheelDiam(*args):
if (STATE.units.get() == "Feet" or STATE.units.get() == "Inches"
or STATE.units.get() == "Meters"):
diamEntry.configure(state="normal")
else:
diamEntry.configure(state="disabled")
def enableErrorBounds(*args):
if STATE.loop_type.get() == "Position":
qPEntry.configure(state="normal")
else:
qPEntry.configure(state="disabled")
# TOP OF WINDOW (FILE SELECTION)
topFrame = Frame(STATE.mainGUI)
topFrame.grid(row=0, column=0, columnspan=4)
Button(topFrame, text="Select Data File", command=getFile).grid(row=0,
column=0,
padx=4)
fileEntry = Entry(topFrame, width=80)
fileEntry.grid(row=0, column=1, columnspan=3)
fileEntry.configure(state="readonly")
Label(topFrame, text="Units:", width=10).grid(row=0, column=4)
unitChoices = {"Feet", "Inches", "Meters", "Radians", "Rotations"}
unitsMenu = OptionMenu(topFrame, STATE.units, *sorted(unitChoices))
unitsMenu.configure(width=10)
unitsMenu.grid(row=0, column=5, sticky="ew")
STATE.units.trace_add("write", enableWheelDiam)
Label(topFrame, text="Wheel Diameter (units):",
anchor="e").grid(row=1, column=3, columnspan=2, sticky="ew")
diamEntry = FloatEntry(topFrame, textvariable=STATE.wheel_diam)
diamEntry.grid(row=1, column=5)
Label(topFrame, text="Subset:", width=15).grid(row=0, column=6)
subsets = {
"All Combined",
"Forward Left",
"Forward Right",
"Forward Combined",
"Backward Left",
"Backward Right",
"Backward Combined",
"Left Combined",
"Right Combined",
}
dirMenu = OptionMenu(topFrame, STATE.subset, *sorted(subsets))
dirMenu.configure(width=20)
dirMenu.grid(row=0, column=7)
for child in topFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
# FEEDFORWARD ANALYSIS FRAME
ffFrame = Frame(STATE.mainGUI, bd=2, relief="groove")
ffFrame.grid(row=1, column=0, columnspan=3, sticky="ns")
Label(ffFrame, text="Feedforward Analysis").grid(row=0,
column=0,
columnspan=5)
analyzeButton = Button(ffFrame,
text="Analyze Data",
command=runAnalysis,
state="disabled")
analyzeButton.grid(row=1, column=0, sticky="ew")
timePlotsButton = Button(
ffFrame,
text="Time-Domain Diagnostics",
command=plotTimeDomain,
state="disabled",
)
timePlotsButton.grid(row=2, column=0, sticky="ew")
voltPlotsButton = Button(
ffFrame,
text="Voltage-Domain Diagnostics",
command=plotVoltageDomain,
state="disabled",
)
voltPlotsButton.grid(row=3, column=0, sticky="ew")
fancyPlotButton = Button(ffFrame,
text="3D Diagnostics",
command=plot3D,
state="disabled")
fancyPlotButton.grid(row=4, column=0, sticky="ew")
Label(ffFrame, text="Accel Window Size:", anchor="e").grid(row=1,
column=1,
sticky="ew")
windowEntry = IntEntry(ffFrame, textvariable=STATE.window_size, width=5)
windowEntry.grid(row=1, column=2)
Label(ffFrame, text="Motion Threshold (units/s):",
anchor="e").grid(row=2, column=1, sticky="ew")
thresholdEntry = FloatEntry(ffFrame,
textvariable=STATE.motion_threshold,
width=5)
thresholdEntry.grid(row=2, column=2)
Label(ffFrame, text="kS:", anchor="e").grid(row=1, column=3, sticky="ew")
kSEntry = FloatEntry(ffFrame, textvariable=STATE.ks, width=10)
kSEntry.grid(row=1, column=4)
kSEntry.configure(state="readonly")
Label(ffFrame, text="kV:", anchor="e").grid(row=2, column=3, sticky="ew")
kVEntry = FloatEntry(ffFrame, textvariable=STATE.kv, width=10)
kVEntry.grid(row=2, column=4)
kVEntry.configure(state="readonly")
Label(ffFrame, text="kA:", anchor="e").grid(row=3, column=3, sticky="ew")
kAEntry = FloatEntry(ffFrame, textvariable=STATE.ka, width=10)
kAEntry.grid(row=3, column=4)
kAEntry.configure(state="readonly")
Label(ffFrame, text="r-squared:", anchor="e").grid(row=4,
column=3,
sticky="ew")
rSquareEntry = FloatEntry(ffFrame, textvariable=STATE.r_square, width=10)
rSquareEntry.grid(row=4, column=4)
rSquareEntry.configure(state="readonly")
Label(ffFrame, text="Track Width:", anchor="e").grid(row=5,
column=3,
sticky="ew")
trackWidthEntry = FloatEntry(ffFrame,
textvariable=STATE.track_width,
width=10)
trackWidthEntry.grid(row=5, column=4)
trackWidthEntry.configure(state="readonly")
for child in ffFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
# FEEDBACK ANALYSIS FRAME
fbFrame = Frame(STATE.mainGUI, bd=2, relief="groove")
fbFrame.grid(row=1, column=3, columnspan=5)
Label(fbFrame, text="Feedback Analysis").grid(row=0,
column=0,
columnspan=5)
Label(fbFrame, text="Gain Settings Preset:", anchor="e").grid(row=1,
column=0,
sticky="ew")
presetChoices = {
"Default",
"WPILib (2020-)",
"WPILib (Pre-2020)",
"Talon (2020-)",
"Talon (Pre-2020)",
"Spark MAX",
}
presetMenu = OptionMenu(fbFrame, STATE.gain_units_preset,
*sorted(presetChoices))
presetMenu.grid(row=1, column=1)
presetMenu.config(width=12)
STATE.gain_units_preset.trace_add("write", presetGains)
Label(fbFrame, text="Controller Period (s):", anchor="e").grid(row=2,
column=0,
sticky="ew")
periodEntry = FloatEntry(fbFrame, textvariable=STATE.period, width=10)
periodEntry.grid(row=2, column=1)
Label(fbFrame, text="Max Controller Output:", anchor="e").grid(row=3,
column=0,
sticky="ew")
controllerMaxEntry = FloatEntry(fbFrame,
textvariable=STATE.max_controller_output,
width=10)
controllerMaxEntry.grid(row=3, column=1)
Label(fbFrame, text="Time-Normalized Controller:",
anchor="e").grid(row=4, column=0, sticky="ew")
normalizedButton = Checkbutton(fbFrame,
variable=STATE.controller_time_normalized)
normalizedButton.grid(row=4, column=1)
Label(fbFrame, text="Controller Type:", anchor="e").grid(row=5,
column=0,
sticky="ew")
controllerTypes = {"Onboard", "Talon", "Spark"}
controllerTypeMenu = OptionMenu(fbFrame, STATE.controller_type,
*sorted(controllerTypes))
controllerTypeMenu.grid(row=5, column=1)
STATE.controller_type.trace_add("write", enableOffboard)
Label(fbFrame, text="Post-Encoder Gearing:", anchor="e").grid(row=6,
column=0,
sticky="ew")
gearingEntry = FloatEntry(fbFrame, textvariable=STATE.gearing, width=10)
gearingEntry.configure(state="disabled")
gearingEntry.grid(row=6, column=1)
Label(fbFrame, text="Encoder EPR:", anchor="e").grid(row=7,
column=0,
sticky="ew")
eprEntry = IntEntry(fbFrame, textvariable=STATE.encoder_epr, width=10)
eprEntry.configure(state="disabled")
eprEntry.grid(row=7, column=1)
Label(fbFrame, text="Has Slave:", anchor="e").grid(row=8,
column=0,
sticky="ew")
hasSlave = Checkbutton(fbFrame, variable=STATE.has_slave)
hasSlave.grid(row=8, column=1)
hasSlave.configure(state="disabled")
STATE.has_slave.trace_add("write", enableOffboard)
Label(fbFrame, text="Slave Update Period (s):",
anchor="e").grid(row=9, column=0, sticky="ew")
slavePeriodEntry = FloatEntry(fbFrame,
textvariable=STATE.slave_period,
width=10)
slavePeriodEntry.grid(row=9, column=1)
slavePeriodEntry.configure(state="disabled")
Label(fbFrame, text="Max Acceptable Position Error (units):",
anchor="e").grid(row=1, column=2, columnspan=2, sticky="ew")
qPEntry = FloatEntry(fbFrame, textvariable=STATE.qp, width=10)
qPEntry.grid(row=1, column=4)
qPEntry.configure(state="disabled")
Label(fbFrame, text="Max Acceptable Velocity Error (units/s):",
anchor="e").grid(row=2, column=2, columnspan=2, sticky="ew")
qVEntry = FloatEntry(fbFrame, textvariable=STATE.qv, width=10)
qVEntry.grid(row=2, column=4)
Label(fbFrame, text="Max Acceptable Control Effort (V):",
anchor="e").grid(row=3, column=2, columnspan=2, sticky="ew")
effortEntry = FloatEntry(fbFrame, textvariable=STATE.max_effort, width=10)
effortEntry.grid(row=3, column=4)
Label(fbFrame, text="Loop Type:", anchor="e").grid(row=4,
column=2,
columnspan=2,
sticky="ew")
loopTypes = {"Position", "Velocity"}
loopTypeMenu = OptionMenu(fbFrame, STATE.loop_type, *sorted(loopTypes))
loopTypeMenu.configure(width=8)
loopTypeMenu.grid(row=4, column=4)
STATE.loop_type.trace_add("write", enableErrorBounds)
Label(fbFrame, text="kV:", anchor="e").grid(row=5, column=2, sticky="ew")
kVFBEntry = FloatEntry(fbFrame, textvariable=STATE.kv, width=10)
kVFBEntry.grid(row=5, column=3)
Label(fbFrame, text="kA:", anchor="e").grid(row=6, column=2, sticky="ew")
kAFBEntry = FloatEntry(fbFrame, textvariable=STATE.ka, width=10)
kAFBEntry.grid(row=6, column=3)
calcGainsButton = Button(
fbFrame,
text="Calculate Optimal Controller Gains",
command=calcGains,
state="disabled",
)
calcGainsButton.grid(row=7, column=2, columnspan=3)
Label(fbFrame, text="kP:", anchor="e").grid(row=8, column=2, sticky="ew")
kPEntry = FloatEntry(fbFrame,
textvariable=STATE.kp,
width=10,
state="readonly").grid(row=8, column=3)
Label(fbFrame, text="kD:", anchor="e").grid(row=9, column=2, sticky="ew")
kDEntry = FloatEntry(fbFrame,
textvariable=STATE.kd,
width=10,
state="readonly").grid(row=9, column=3)
for child in fbFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
def configure_gui(STATE):
def getFile():
dataFile = tkinter.filedialog.askopenfile(
parent=STATE.mainGUI,
mode="rb",
title="Choose the data file (.JSON)",
initialdir=STATE.project_path.get(),
)
fileEntry.configure(state="normal")
fileEntry.delete(0, END)
fileEntry.insert(0, dataFile.name)
fileEntry.configure(state="readonly")
try:
data = json.load(dataFile)
try:
# Transform the data into a numpy array to make it easier to use
# -> transpose it so we can deal with it in columns
for k in JSON_DATA_KEYS:
data[k] = np.array(data[k]).transpose()
if len(data[JSON_DATA_KEYS[-1]]) > len(columns):
messagebox.showerror(
"Error!",
"You cannot import characterization data from a different mechanism.",
)
return
STATE.stored_data = data
analyzeButton.configure(state="normal")
except Exception as e:
messagebox.showerror(
"Error!",
"The structure of the data JSON was not recognized.\n" +
"Details\n" + repr(e),
)
return
except Exception as e:
messagebox.showerror(
"Error!",
"The JSON file could not be loaded.\n" + "Details:\n" +
repr(e),
parent=STATE.mainGUI,
)
return
def runAnalysis():
(
STATE.quasi_forward,
STATE.quasi_backward,
STATE.step_forward,
STATE.step_backward,
) = prepare_data(STATE.stored_data,
window=STATE.window_size.get(),
STATE=STATE)
if (STATE.quasi_forward is None or STATE.quasi_backward is None
or STATE.step_forward is None or STATE.step_backward is None):
return
if STATE.subset.get() == "Forward":
ks, kv, ka, rsquare = calcFit(STATE.quasi_forward,
STATE.step_forward)
elif STATE.subset.get() == "Backward":
ks, kv, ka, rsquare = calcFit(STATE.quasi_backward,
STATE.step_backward)
STATE.ks.set(float("%.3g" % ks))
STATE.kv.set(float("%.3g" % kv))
STATE.ka.set(float("%.3g" % ka))
STATE.r_square.set(float("%.3g" % rsquare))
calcGains()
timePlotsButton.configure(state="normal")
voltPlotsButton.configure(state="normal")
fancyPlotButton.configure(state="normal")
calcGainsButton.configure(state="normal")
def plotTimeDomain():
if STATE.subset.get() == "Forward":
_plotTimeDomain("Forward", STATE.quasi_forward, STATE.step_forward)
elif STATE.subset.get() == "Backward":
_plotTimeDomain("Backward", STATE.quasi_backward,
STATE.step_backward)
def plotVoltageDomain():
if STATE.subset.get() == "Forward":
_plotVoltageDomain("Forward", STATE.quasi_forward,
STATE.step_forward, STATE)
elif STATE.subset.get() == "Backward":
_plotVoltageDomain("Backward", STATE.quasi_backward,
STATE.step_backward, STATE)
def plot3D():
if STATE.subset.get() == "Forward":
_plot3D("Forward", STATE.quasi_forward, STATE.step_forward, STATE)
elif STATE.subset.get() == "Backward":
_plot3D("Backward", STATE.quasi_backward, STATE.step_backward,
STATE)
def calcGains():
period = (STATE.period.get()
if not STATE.has_slave.get() else STATE.slave_period.get())
if STATE.loop_type.get() == "Position":
kp, kd = _calcGainsPos(
STATE.kv.get(),
STATE.ka.get(),
STATE.qp.get(),
STATE.qv.get(),
STATE.max_effort.get(),
period,
)
else:
kp, kd = _calcGainsVel(
STATE.kv.get(),
STATE.ka.get(),
STATE.qv.get(),
STATE.max_effort.get(),
period,
)
# Scale gains to output
kp = kp / 12 * STATE.max_controller_output.get()
kd = kd / 12 * STATE.max_controller_output.get()
# Rescale kD if not time-normalized
if not STATE.controller_time_normalized.get():
kd = kd / STATE.period.get()
# Get the correct conversion factor for rotations
if STATE.units.get() == "Radians":
rotation = 2 * math.pi
elif STATE.units.get() == "Rotations":
rotation = 1
elif STATE.units.get() == "Degrees":
rotation = 360
# Convert to controller-native units
if STATE.controller_type.get() == "Talon":
kp = kp * rotation / (STATE.encoder_epr.get() *
STATE.gearing.get())
kd = kd * rotation / (STATE.encoder_epr.get() *
STATE.gearing.get())
if STATE.loop_type.get() == "Velocity":
kp = kp * 10
STATE.kp.set(float("%.3g" % kp))
STATE.kd.set(float("%.3g" % kd))
def presetGains(*args):
presets = {
"Default":
lambda: (
STATE.max_controller_output.set(12),
STATE.period.set(0.02),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Onboard"),
),
"WPILib (2020-)":
lambda: (
STATE.max_controller_output.set(12),
STATE.period.set(0.02),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Onboard"),
),
"WPILib (Pre-2020)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.05),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Onboard"),
),
"Talon (2020-)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Talon"),
),
"Talon (Pre-2020)":
lambda: (
STATE.max_controller_output.set(1023),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Talon"),
),
"Spark MAX":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Spark"),
),
}
presets.get(STATE.gain_units_preset.get(), "Default")()
def enableOffboard(*args):
if STATE.controller_type.get() == "Onboard":
gearingEntry.configure(state="disabled")
eprEntry.configure(state="disabled")
hasSlave.configure(state="disabled")
slavePeriodEntry.configure(state="disabled")
elif STATE.controller_type.get() == "Talon":
gearingEntry.configure(state="normal")
eprEntry.configure(state="normal")
hasSlave.configure(state="normal")
if STATE.has_slave.get():
slavePeriodEntry.configure(state="normal")
else:
slavePeriodEntry.configure(state="disabled")
else:
gearingEntry.configure(state="disabled")
eprEntry.configure(state="disabled")
hasSlave.configure(state="normal")
if STATE.has_slave.get():
slavePeriodEntry.configure(state="normal")
else:
slavePeriodEntry.configure(state="disabled")
def enableErrorBounds(*args):
if STATE.loop_type.get() == "Position":
qPEntry.configure(state="normal")
else:
qPEntry.configure(state="disabled")
# TOP OF WINDOW (FILE SELECTION)
topFrame = Frame(STATE.mainGUI)
topFrame.grid(row=0, column=0, columnspan=4)
Button(topFrame, text="Select Data File", command=getFile).grid(row=0,
column=0,
padx=4)
fileEntry = Entry(topFrame, width=80)
fileEntry.grid(row=0, column=1, columnspan=3)
fileEntry.configure(state="readonly")
# The only current option is rotations
# This made the implementation of everything easier
Label(topFrame, text="Units:", width=10).grid(row=0, column=4)
unitChoices = {"Rotations", "Degrees", "Radians"}
unitsMenu = OptionMenu(topFrame, STATE.units, *sorted(unitChoices))
unitsMenu.configure(width=10)
unitsMenu.grid(row=0, column=5, sticky="ew")
Label(topFrame, text="Subset:", width=15).grid(row=0, column=6)
subsets = {
"Forward",
"Backward",
}
dirMenu = OptionMenu(topFrame, STATE.subset, *sorted(subsets))
dirMenu.configure(width=20)
dirMenu.grid(row=0, column=7)
for child in topFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
# FEEDFORWARD ANALYSIS FRAME
ffFrame = Frame(STATE.mainGUI, bd=2, relief="groove")
ffFrame.grid(row=1, column=0, columnspan=3, sticky="ns")
Label(ffFrame, text="Feedforward Analysis").grid(row=0,
column=0,
columnspan=5)
analyzeButton = Button(ffFrame,
text="Analyze Data",
command=runAnalysis,
state="disabled")
analyzeButton.grid(row=1, column=0, sticky="ew")
timePlotsButton = Button(
ffFrame,
text="Time-Domain Diagnostics",
command=plotTimeDomain,
state="disabled",
)
timePlotsButton.grid(row=2, column=0, sticky="ew")
voltPlotsButton = Button(
ffFrame,
text="Voltage-Domain Diagnostics",
command=plotVoltageDomain,
state="disabled",
)
voltPlotsButton.grid(row=3, column=0, sticky="ew")
fancyPlotButton = Button(ffFrame,
text="3D Diagnostics",
command=plot3D,
state="disabled")
fancyPlotButton.grid(row=4, column=0, sticky="ew")
Label(ffFrame, text="Accel Window Size:", anchor="e").grid(row=1,
column=1,
sticky="ew")
windowEntry = IntEntry(ffFrame, textvariable=STATE.window_size, width=5)
windowEntry.grid(row=1, column=2)
Label(ffFrame, text="Motion Threshold (units/s):",
anchor="e").grid(row=2, column=1, sticky="ew")
thresholdEntry = FloatEntry(ffFrame,
textvariable=STATE.motion_threshold,
width=5)
thresholdEntry.grid(row=2, column=2)
Label(ffFrame, text="kS:", anchor="e").grid(row=1, column=3, sticky="ew")
kSEntry = FloatEntry(ffFrame, textvariable=STATE.ks, width=10)
kSEntry.grid(row=1, column=4)
kSEntry.configure(state="readonly")
Label(ffFrame, text="kV:", anchor="e").grid(row=2, column=3, sticky="ew")
kVEntry = FloatEntry(ffFrame, textvariable=STATE.kv, width=10)
kVEntry.grid(row=2, column=4)
kVEntry.configure(state="readonly")
Label(ffFrame, text="kA:", anchor="e").grid(row=3, column=3, sticky="ew")
kAEntry = FloatEntry(ffFrame, textvariable=STATE.ka, width=10)
kAEntry.grid(row=3, column=4)
kAEntry.configure(state="readonly")
Label(ffFrame, text="r-squared:", anchor="e").grid(row=4,
column=3,
sticky="ew")
rSquareEntry = FloatEntry(ffFrame, textvariable=STATE.r_square, width=10)
rSquareEntry.grid(row=4, column=4)
rSquareEntry.configure(state="readonly")
for child in ffFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
# FEEDBACK ANALYSIS FRAME
fbFrame = Frame(STATE.mainGUI, bd=2, relief="groove")
fbFrame.grid(row=1, column=3, columnspan=5)
Label(fbFrame, text="Feedback Analysis").grid(row=0,
column=0,
columnspan=5)
Label(fbFrame, text="Gain Settings Preset:", anchor="e").grid(row=1,
column=0,
sticky="ew")
presetChoices = {
"Default",
"WPILib (2020-)",
"WPILib (Pre-2020)",
"Talon (2020-)",
"Talon (Pre-2020)",
"Spark MAX",
}
presetMenu = OptionMenu(fbFrame, STATE.gain_units_preset,
*sorted(presetChoices))
presetMenu.grid(row=1, column=1)
presetMenu.config(width=12)
STATE.gain_units_preset.trace_add("write", presetGains)
Label(fbFrame, text="Controller Period (s):", anchor="e").grid(row=2,
column=0,
sticky="ew")
periodEntry = FloatEntry(fbFrame, textvariable=STATE.period, width=10)
periodEntry.grid(row=2, column=1)
Label(fbFrame, text="Max Controller Output:", anchor="e").grid(row=3,
column=0,
sticky="ew")
controllerMaxEntry = FloatEntry(fbFrame,
textvariable=STATE.max_controller_output,
width=10)
controllerMaxEntry.grid(row=3, column=1)
Label(fbFrame, text="Time-Normalized Controller:",
anchor="e").grid(row=4, column=0, sticky="ew")
normalizedButton = Checkbutton(fbFrame,
variable=STATE.controller_time_normalized)
normalizedButton.grid(row=4, column=1)
Label(fbFrame, text="Controller Type:", anchor="e").grid(row=5,
column=0,
sticky="ew")
controllerTypes = {"Onboard", "Talon", "Spark"}
controllerTypeMenu = OptionMenu(fbFrame, STATE.controller_type,
*sorted(controllerTypes))
controllerTypeMenu.grid(row=5, column=1)
STATE.controller_type.trace_add("write", enableOffboard)
Label(fbFrame, text="Post-Encoder Gearing:", anchor="e").grid(row=6,
column=0,
sticky="ew")
gearingEntry = FloatEntry(fbFrame, textvariable=STATE.gearing, width=10)
gearingEntry.configure(state="disabled")
gearingEntry.grid(row=6, column=1)
Label(fbFrame, text="Encoder EPR:", anchor="e").grid(row=7,
column=0,
sticky="ew")
eprEntry = IntEntry(fbFrame, textvariable=STATE.encoder_epr, width=10)
eprEntry.configure(state="disabled")
eprEntry.grid(row=7, column=1)
Label(fbFrame, text="Has Slave:", anchor="e").grid(row=8,
column=0,
sticky="ew")
hasSlave = Checkbutton(fbFrame, variable=STATE.has_slave)
hasSlave.grid(row=8, column=1)
hasSlave.configure(state="disabled")
STATE.has_slave.trace_add("write", enableOffboard)
Label(fbFrame, text="Slave Update Period (s):",
anchor="e").grid(row=9, column=0, sticky="ew")
slavePeriodEntry = FloatEntry(fbFrame,
textvariable=STATE.slave_period,
width=10)
slavePeriodEntry.grid(row=9, column=1)
slavePeriodEntry.configure(state="disabled")
Label(fbFrame, text="Max Acceptable Position Error (units):",
anchor="e").grid(row=1, column=2, columnspan=2, sticky="ew")
qPEntry = FloatEntry(fbFrame, textvariable=STATE.qp, width=10)
qPEntry.grid(row=1, column=4)
qPEntry.configure(state="disabled")
Label(fbFrame, text="Max Acceptable Velocity Error (units/s):",
anchor="e").grid(row=2, column=2, columnspan=2, sticky="ew")
qVEntry = FloatEntry(fbFrame, textvariable=STATE.qv, width=10)
qVEntry.grid(row=2, column=4)
Label(fbFrame, text="Max Acceptable Control Effort (V):",
anchor="e").grid(row=3, column=2, columnspan=2, sticky="ew")
effortEntry = FloatEntry(fbFrame, textvariable=STATE.max_effort, width=10)
effortEntry.grid(row=3, column=4)
Label(fbFrame, text="Loop Type:", anchor="e").grid(row=4,
column=2,
columnspan=2,
sticky="ew")
loopTypes = {"Position", "Velocity"}
loopTypeMenu = OptionMenu(fbFrame, STATE.loop_type, *sorted(loopTypes))
loopTypeMenu.configure(width=8)
loopTypeMenu.grid(row=4, column=4)
STATE.loop_type.trace_add("write", enableErrorBounds)
Label(fbFrame, text="kV:", anchor="e").grid(row=5, column=2, sticky="ew")
kVFBEntry = FloatEntry(fbFrame, textvariable=STATE.kv, width=10)
kVFBEntry.grid(row=5, column=3)
Label(fbFrame, text="kA:", anchor="e").grid(row=6, column=2, sticky="ew")
kAFBEntry = FloatEntry(fbFrame, textvariable=STATE.ka, width=10)
kAFBEntry.grid(row=6, column=3)
calcGainsButton = Button(
fbFrame,
text="Calculate Optimal Controller Gains",
command=calcGains,
state="disabled",
)
calcGainsButton.grid(row=7, column=2, columnspan=3)
Label(fbFrame, text="kP:", anchor="e").grid(row=8, column=2, sticky="ew")
kPEntry = FloatEntry(fbFrame,
textvariable=STATE.kp,
width=10,
state="readonly").grid(row=8, column=3)
Label(fbFrame, text="kD:", anchor="e").grid(row=9, column=2, sticky="ew")
kDEntry = FloatEntry(fbFrame,
textvariable=STATE.kd,
width=10,
state="readonly").grid(row=9, column=3)
for child in fbFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
def configureGUI(self):
mech = frc_characterization.logger_analyzer
def getProjectLocation():
file_path = filedialog.askdirectory(
title="Choose the project location",
initialdir=self.project_path.get())
projLocationEntry.configure(state="normal")
projLocationEntry.delete(0, END)
projLocationEntry.insert(0, file_path)
projLocationEntry.configure(state="readonly")
def getConfigPath():
file_path = tkinter.filedialog.askopenfilename(
title="Choose the config file",
initialdir=self.project_path.get(),
filetypes=(("Python", "*.py"), ),
)
configEntry.configure(state="normal")
configEntry.delete(0, END)
configEntry.insert(0, file_path)
configEntry.configure(state="readonly")
def saveConfig():
with open(self.config_path.get(), "w+") as config:
config.write(self.config.get())
def updateConfigPath(*args):
configEntry.configure(state="normal")
self.config_path.set(
os.path.join(self.project_path.get(), "robotconfig.py"))
configEntry.configure(state="readonly")
def getDefaultConfig(*args):
with resources.open_text(
"frc_characterization.logger_analyzer.templates.configs",
f"{self.control_type.get().lower()}config.py",
) as config:
self.config.set(config.read())
def readConfig():
try:
with open(self.config_path.get(), "r") as config:
self.config.set(config.read())
except:
messagebox.showerror("Error!",
"Could not open/read config file.")
return
def genProject():
config = ast.literal_eval(self.config.get())
config["controlType"] = self.control_type.get()
if self.control_type.get() == "SparkMax":
config["controllerTypes"] = ["CANSparkMax"]
config["rightControllerTypes"] = ["CANSparkMax"]
logger.info(f"Config: {config}")
dst = os.path.join(self.project_path.get(),
"characterization-project")
try:
with resources.path(res, "project") as path:
shutil.copytree(src=path, dst=dst)
with open(
os.path.join(dst, "src", "main", "java", "dc",
"Robot.java"),
"w+",
) as robot:
robot.write(mech.gen_robot_code(config))
with open(os.path.join(dst, "build.gradle"),
"w+") as build:
build.write(
mech.gen_build_gradle(self.team_number.get(), ))
except FileExistsError:
if messagebox.askyesno(
"Warning!",
"Project directory already exists! Do you want to overwrite it?",
):
shutil.rmtree(dst)
genProject()
except Exception as e:
messagebox.showerror(
"Error!",
"Unable to generate project - config may be bad.\n" +
"Details:\n" + repr(e),
)
shutil.rmtree(dst)
def deployProject(queue):
if self.team_number.get() == 0:
cmd = "simulatejava"
else:
cmd = "deploy"
def append_latest_jdk(process_args, jdk_base_path):
possible_jdk_paths = glob.glob(
os.path.join(jdk_base_path, "20[0-9][0-9]"))
if len(possible_jdk_paths) <= 0:
queue.put((
"Warning!",
"You not appear to have any wpilib JDK installed. If your system JDK is the wrong version then the deploy will fail.",
))
return
year = max([
int(os.path.basename(path)) for path in possible_jdk_paths
])
process_args.append(
"-Dorg.gradle.java.home=" +
os.path.join(jdk_base_path, str(year), "jdk"))
if int(year) != datetime.now().year:
queue.put((
"Warning!",
f"Your latest wpilib JDK's year ({year}) doesn't match the current year ({datetime.now().year}). Your deploy may fail.",
))
if os.name == "nt":
process_args = [
os.path.join(
self.project_path.get(),
"characterization-project",
"gradlew.bat",
),
cmd,
"--console=plain",
]
# C:/Users/Public/wpilib/YEAR/jdk is correct *as of* wpilib 2020
# Prior to 2020 the path was C:/Users/Public/frcYEAR/jdk
jdk_base_path = os.path.join(
os.path.abspath(os.path.join(os.path.expanduser("~"),
"..")),
"Public",
"wpilib",
)
append_latest_jdk(process_args, jdk_base_path)
try:
process = Popen(
process_args,
stdout=PIPE,
stderr=STDOUT,
cwd=os.path.join(self.project_path.get(),
"characterization-project"),
)
except Exception as e:
queue.put((
"Error!",
"Could not call gradlew deploy.\n" + "Details:\n" +
repr(e),
))
return
else:
process_args = [
os.path.join(self.project_path.get(),
"characterization-project", "gradlew"),
cmd,
"--console=plain",
]
# This path is correct *as of* wpilib 2020
# Prior to 2020 the path was ~/frcYEAR/jdk
jdk_base_path = os.path.join(os.path.expanduser("~"), "wpilib")
append_latest_jdk(process_args, jdk_base_path)
try:
process = Popen(
process_args,
stdout=PIPE,
stderr=STDOUT,
cwd=os.path.join(self.project_path.get(),
"characterization-project"),
)
except Exception as e:
queue.put((
"Error!",
"Could not call gradlew deploy.\n" + "Details:\n" +
repr(e),
))
return
while process.poll() is None:
time.sleep(0.1)
queue.put(("Console", process.stdout.readline()))
if process.poll() != 0:
queue.put((
"Error!",
"Deployment failed!\n" +
"Check the console for more details.",
))
# finish adding any outputs
out = process.stdout.readline()
while out:
queue.put(("Console", out))
out = process.stdout.readline()
def processError(message):
if "Warning!" == message[0]:
messagebox.showwarning(message[0],
message[1],
parent=self.deploy_window)
else:
messagebox.showerror(message[0],
message[1],
parent=self.deploy_window)
def threadedDeploy():
logger.info("Starting Deploy")
self.queue = queue.Queue()
self.deploy_window = stdoutWindow()
ThreadedTask(self.queue).start()
self.mainGUI.after(10, processQueue)
def processQueue():
try:
msg = self.queue.get_nowait()
if msg != "Task Finished":
if msg[0] != "Console":
processError(msg)
else:
updateStdout(msg[1])
self.mainGUI.after(10, processQueue)
except queue.Empty:
self.mainGUI.after(10, processQueue)
class ThreadedTask(threading.Thread):
def __init__(self, queue):
threading.Thread.__init__(self)
self.queue = queue
def run(self):
deployProject(self.queue)
self.queue.put("Task Finished")
logger.info("Finished Deploy")
class stdoutWindow(tkinter.Toplevel):
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.title("Deploy Progress")
self.stdoutText = ScrolledText(self, width=60, height=15)
self.stdoutText.grid(row=0, column=0)
def updateStdout(out):
if self.deploy_window.winfo_exists():
if out != "":
self.deploy_window.stdoutText.insert(END, out)
def runLogger():
mech.data_logger.main(
self.team_number.get(),
self.project_path.get(),
units=Units(self.units.get()),
units_per_rot=self.units_per_rot.get(),
test=Tests(self.project_type.get()),
)
def runAnalyzer():
mech.data_analyzer.main(self.project_path.get())
def enableUnitPerRot(*args):
units = self.units.get()
if isRotation(units):
ureg = pint.UnitRegistry()
units = Units(units)
unitsRotationEntry.configure(state="readonly")
self.units_per_rot.set(
round((1 * ureg.revolution).to(units.unit).magnitude, 3))
else:
self.units_per_rot.set(0)
unitsRotationEntry.configure(state="normal")
def isRotation(units):
return Units(units) in (Units.ROTATIONS, Units.RADIANS,
Units.DEGREES)
getDefaultConfig()
# TOP OF WINDOW
topFrame = Frame(self.mainGUI)
topFrame.grid(row=0, column=0, sticky="ew")
Button(topFrame,
text="Select Project Location",
command=getProjectLocation).grid(row=0, column=0, sticky="ew")
projLocationEntry = Entry(topFrame,
textvariable=self.project_path,
width=80,
state="readonly")
projLocationEntry.grid(row=0, column=1, columnspan=10)
self.project_path.trace_add("write", updateConfigPath)
Label(topFrame, text="Project Type:", anchor="e").grid(row=0,
column=11,
sticky="ew")
projTypeMenu = OptionMenu(topFrame, self.project_type,
*sorted(test.value for test in Tests))
projTypeMenu.configure(width=25)
projTypeMenu.grid(row=0, column=12, sticky="ew")
Button(topFrame, text="Select Config File",
command=getConfigPath).grid(row=1, column=0, sticky="ew")
configEntry = Entry(topFrame,
textvariable=self.config_path,
width=80,
state="readonly")
configEntry.grid(row=1, column=1, columnspan=10)
Button(topFrame, text="Save Config",
command=saveConfig).grid(row=1, column=11)
readConfigButton = Button(topFrame,
text="Read Config",
command=readConfig)
readConfigButton.grid(row=1, column=12, sticky="ew")
Label(topFrame, text="Team Number:", anchor="e").grid(row=2,
column=0,
sticky="ew")
teamNumberEntry = IntEntry(topFrame, textvariable=self.team_number)
teamNumberEntry.grid(row=2, column=1, sticky="ew")
Label(topFrame, text="Unit Type:", anchor="e").grid(row=2,
column=2,
sticky="ew")
unitMenu = OptionMenu(topFrame, self.units,
*sorted(unit.value for unit in Units))
unitMenu.configure(width=25)
unitMenu.grid(row=2, column=3, sticky="ew")
self.units.trace_add("write", enableUnitPerRot)
Label(topFrame, text="Units per Rotation:",
anchor="e").grid(row=2, column=4, sticky="ew")
unitsRotationEntry = FloatEntry(topFrame,
textvariable=self.units_per_rot)
unitsRotationEntry.grid(row=2, column=5, sticky="ew")
unitsRotationEntry.configure(state="readonly")
Label(topFrame, text="Control Type:", anchor="e").grid(row=2,
column=11,
sticky="ew")
controlMenu = OptionMenu(topFrame, self.control_type,
*["Simple", "CTRE", "SparkMax"])
controlMenu.configure(width=25)
controlMenu.grid(row=2, column=12, sticky="ew")
self.control_type.trace_add("write", getDefaultConfig)
for child in topFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
# Body Frame
bodyFrame = Frame(self.mainGUI, bd=2, relief="groove")
bodyFrame.grid(row=1, column=0, sticky="ew")
genProjButton = Button(bodyFrame,
text="Generate Project",
command=genProject)
genProjButton.grid(row=1, column=0, sticky="ew")
deployButton = Button(bodyFrame,
text="Deploy Project",
command=threadedDeploy)
deployButton.grid(row=2, column=0, sticky="ew")
loggerButton = Button(bodyFrame,
text="Launch Data Logger",
command=runLogger)
loggerButton.grid(row=3, column=0, sticky="ew")
analyzerButton = Button(bodyFrame,
text="Launch Data Analyzer",
command=runAnalyzer)
analyzerButton.grid(row=4, column=0, sticky="ew")
configEditPane = TextExtension(bodyFrame, textvariable=self.config)
configEditPane.grid(row=0, column=1, rowspan=30, columnspan=10)
for child in bodyFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
def configure_gui(STATE):
def getFile():
dataFile = tkinter.filedialog.askopenfile(
parent=STATE.mainGUI,
mode="rb",
title="Choose the data file (.JSON)",
initialdir=STATE.project_path.get(),
)
fileEntry.configure(state="normal")
fileEntry.delete(0, END)
fileEntry.insert(0, dataFile.name)
fileEntry.configure(state="readonly")
try:
data = json.load(dataFile)
try:
# Transform the data into a numpy array to make it easier to use
# -> transpose it so we can deal with it in columns
for k in JSON_DATA_KEYS:
data[k] = np.array(data[k]).transpose()
if len(data[JSON_DATA_KEYS[-1]]) > len(columns):
messagebox.showerror(
"Error!",
"You cannot import characterization data from a different mechanism.",
)
return
STATE.stored_data = data
analyzeButton.configure(state="normal")
except Exception as e:
messagebox.showerror(
"Error!",
"The structure of the data JSON was not recognized.\n" +
"Details\n" + repr(e),
)
return
except Exception as e:
messagebox.showerror(
"Error!",
"The JSON file could not be loaded.\n" + "Details:\n" +
repr(e),
parent=STATE.mainGUI,
)
return
def runAnalysis():
(
STATE.quasi_forward,
STATE.quasi_backward,
STATE.step_forward,
STATE.step_backward,
) = prepare_data(STATE.stored_data,
window=STATE.window_size.get(),
STATE=STATE)
if (STATE.quasi_forward is None or STATE.quasi_backward is None
or STATE.step_forward is None or STATE.step_backward is None):
return
if STATE.subset.get() == "Forward":
ks, kv, ka, rsquare = calcFit(STATE.quasi_forward,
STATE.step_forward)
elif STATE.subset.get() == "Backward":
ks, kv, ka, rsquare = calcFit(STATE.quasi_backward,
STATE.step_backward)
STATE.ks.set(float("%.3g" % ks))
STATE.kv.set(float("%.3g" % kv))
STATE.ka.set(float("%.3g" % ka))
STATE.r_square.set(float("%.3g" % rsquare))
calcGains()
timePlotsButton.configure(state="normal")
voltPlotsButton.configure(state="normal")
fancyPlotButton.configure(state="normal")
calcGainsButton.configure(state="normal")
def plotTimeDomain():
if STATE.subset.get() == "Forward":
_plotTimeDomain("Forward", STATE.quasi_forward, STATE.step_forward)
elif STATE.subset.get() == "Backward":
_plotTimeDomain("Backward", STATE.quasi_backward,
STATE.step_backward)
def plotVoltageDomain():
if STATE.subset.get() == "Forward":
_plotVoltageDomain("Forward", STATE.quasi_forward,
STATE.step_forward, STATE)
elif STATE.subset.get() == "Backward":
_plotVoltageDomain("Backward", STATE.quasi_backward,
STATE.step_backward, STATE)
def plot3D():
if STATE.subset.get() == "Forward":
_plot3D("Forward", STATE.quasi_forward, STATE.step_forward, STATE)
elif STATE.subset.get() == "Backward":
_plot3D("Backward", STATE.quasi_backward, STATE.step_backward,
STATE)
def calcGains():
period = (STATE.period.get()
if not STATE.has_slave.get() else STATE.slave_period.get())
if STATE.loop_type.get() == "Position":
kp, kd = _calcGainsPos(
STATE.kv.get(),
STATE.ka.get(),
STATE.qp.get(),
STATE.qv.get(),
STATE.max_effort.get(),
period,
STATE.measurement_delay.get(),
)
else:
kp, kd = _calcGainsVel(
STATE.kv.get(),
STATE.ka.get(),
STATE.qv.get(),
STATE.max_effort.get(),
period,
STATE.measurement_delay.get(),
)
# Scale gains to output
kp = kp / 12 * STATE.max_controller_output.get()
kd = kd / 12 * STATE.max_controller_output.get()
# Rescale kD if not time-normalized
if not STATE.controller_time_normalized.get():
kd = kd / STATE.period.get()
# Get the correct conversion factor for rotations
if STATE.units.get() == "Radians":
rotation = 2 * math.pi
elif STATE.units.get() == "Rotations":
rotation = 1
elif STATE.units.get() == "Degrees":
rotation = 360
# Convert to controller-native units
if STATE.controller_type.get() == "Talon":
kp = kp * rotation / (STATE.encoder_epr.get() *
STATE.gearing.get())
kd = kd * rotation / (STATE.encoder_epr.get() *
STATE.gearing.get())
if STATE.loop_type.get() == "Velocity":
kp = kp * 10
STATE.kp.set(float("%.3g" % kp))
STATE.kd.set(float("%.3g" % kd))
def presetGains(*args):
def setMeasurementDelay(delay):
STATE.measurement_delay.set(0 if STATE.loop_type.get() ==
"Position" else delay)
# A number of motor controllers use moving average filters; these are types of FIR filters.
# A moving average filter with a window size of N is a FIR filter with N taps.
# The average delay (in taps) of an arbitrary FIR filter with N taps is (N-1)/2.
# All of the delays below assume that 1 T takes 1 ms.
#
# Proof:
# N taps with delays of 0 .. N - 1 T
#
# average delay = (sum 0 .. N - 1) / N T
# = (sum 1 .. N - 1) / N T
#
# note: sum 1 .. n = n(n + 1) / 2
#
# = (N - 1)((N - 1) + 1) / (2N) T
# = (N - 1)N / (2N) T
# = (N - 1)/2 T
presets = {
"Default":
lambda: (
STATE.max_controller_output.set(12),
STATE.period.set(0.02),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Onboard"),
setMeasurementDelay(0),
),
"WPILib (2020-)":
lambda: (
STATE.max_controller_output.set(12),
STATE.period.set(0.02),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Onboard"),
# Note that the user will need to remember to set this if the onboard controller is getting delayed measurements.
setMeasurementDelay(0),
),
"WPILib (Pre-2020)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.05),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Onboard"),
# Note that the user will need to remember to set this if the onboard controller is getting delayed measurements.
setMeasurementDelay(0),
),
"Talon FX":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Talon"),
# https://phoenix-documentation.readthedocs.io/en/latest/ch14_MCSensor.html#changing-velocity-measurement-parameters
# 100 ms sampling period + a moving average window size of 64 (i.e. a 64-tap FIR) = 100/2 ms + (64-1)/2 ms = 81.5 ms.
# See above for more info on moving average delays.
setMeasurementDelay(81.5),
),
"Talon SRX (2020-)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(True),
STATE.controller_type.set("Talon"),
# https://phoenix-documentation.readthedocs.io/en/latest/ch14_MCSensor.html#changing-velocity-measurement-parameters
# 100 ms sampling period + a moving average window size of 64 (i.e. a 64-tap FIR) = 100/2 ms + (64-1)/2 ms = 81.5 ms.
# See above for more info on moving average delays.
setMeasurementDelay(81.5),
),
"Talon SRX (Pre-2020)":
lambda: (
STATE.max_controller_output.set(1023),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Talon"),
# https://phoenix-documentation.readthedocs.io/en/latest/ch14_MCSensor.html#changing-velocity-measurement-parameters
# 100 ms sampling period + a moving average window size of 64 (i.e. a 64-tap FIR) = 100/2 ms + (64-1)/2 ms = 81.5 ms.
# See above for more info on moving average delays.
setMeasurementDelay(81.5),
),
"Spark MAX (brushless)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Spark"),
# According to a Rev employee on the FRC Discord the window size is 40 so delay = (40-1)/2 ms = 19.5 ms.
# See above for more info on moving average delays.
setMeasurementDelay(19.5),
),
"Spark MAX (brushed)":
lambda: (
STATE.max_controller_output.set(1),
STATE.period.set(0.001),
STATE.controller_time_normalized.set(False),
STATE.controller_type.set("Spark"),
# https://www.revrobotics.com/content/sw/max/sw-docs/cpp/classrev_1_1_c_a_n_encoder.html#a7e6ce792bc0c0558fb944771df572e6a
# 64-tap FIR = (64-1)/2 ms = 31.5 ms delay.
# See above for more info on moving average delays.
setMeasurementDelay(31.5),
),
}
presets.get(STATE.gain_units_preset.get(), "Default")()
def enableOffboard(*args):
if STATE.controller_type.get() == "Onboard":
gearingEntry.configure(state="disabled")
eprEntry.configure(state="disabled")
hasSlave.configure(state="disabled")
slavePeriodEntry.configure(state="disabled")
elif STATE.controller_type.get() == "Talon":
gearingEntry.configure(state="normal")
eprEntry.configure(state="normal")
hasSlave.configure(state="normal")
if STATE.has_slave.get():
slavePeriodEntry.configure(state="normal")
else:
slavePeriodEntry.configure(state="disabled")
else:
gearingEntry.configure(state="disabled")
eprEntry.configure(state="disabled")
hasSlave.configure(state="normal")
if STATE.has_slave.get():
slavePeriodEntry.configure(state="normal")
else:
slavePeriodEntry.configure(state="disabled")
def enableErrorBounds(*args):
if STATE.loop_type.get() == "Position":
qPEntry.configure(state="normal")
else:
qPEntry.configure(state="disabled")
# TOP OF WINDOW (FILE SELECTION)
topFrame = Frame(STATE.mainGUI)
topFrame.grid(row=0, column=0, columnspan=4)
Button(topFrame, text="Select Data File", command=getFile).grid(row=0,
column=0,
padx=4)
fileEntry = Entry(topFrame, width=80)
fileEntry.grid(row=0, column=1, columnspan=3)
fileEntry.configure(state="readonly")
# The only current option is rotations
# This made the implementation of everything easier
Label(topFrame, text="Units:", width=10).grid(row=0, column=4)
unitChoices = {"Rotations", "Degrees", "Radians"}
unitsMenu = OptionMenu(topFrame, STATE.units, *sorted(unitChoices))
unitsMenu.configure(width=10)
unitsMenu.grid(row=0, column=5, sticky="ew")
Label(topFrame, text="Subset:", width=15).grid(row=0, column=6)
subsets = {
"Forward",
"Backward",
}
dirMenu = OptionMenu(topFrame, STATE.subset, *sorted(subsets))
dirMenu.configure(width=20)
dirMenu.grid(row=0, column=7)
for child in topFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
# FEEDFORWARD ANALYSIS FRAME
ffFrame = Frame(STATE.mainGUI, bd=2, relief="groove")
ffFrame.grid(row=1, column=0, columnspan=3, sticky="ns")
Label(ffFrame, text="Feedforward Analysis").grid(row=0,
column=0,
columnspan=5)
analyzeButton = Button(ffFrame,
text="Analyze Data",
command=runAnalysis,
state="disabled")
analyzeButton.grid(row=1, column=0, sticky="ew")
timePlotsButton = Button(
ffFrame,
text="Time-Domain Diagnostics",
command=plotTimeDomain,
state="disabled",
)
timePlotsButton.grid(row=2, column=0, sticky="ew")
voltPlotsButton = Button(
ffFrame,
text="Voltage-Domain Diagnostics",
command=plotVoltageDomain,
state="disabled",
)
voltPlotsButton.grid(row=3, column=0, sticky="ew")
fancyPlotButton = Button(ffFrame,
text="3D Diagnostics",
command=plot3D,
state="disabled")
fancyPlotButton.grid(row=4, column=0, sticky="ew")
Label(ffFrame, text="Accel Window Size:", anchor="e").grid(row=1,
column=1,
sticky="ew")
windowEntry = IntEntry(ffFrame, textvariable=STATE.window_size, width=5)
windowEntry.grid(row=1, column=2)
Label(ffFrame, text="Motion Threshold (units/s):",
anchor="e").grid(row=2, column=1, sticky="ew")
thresholdEntry = FloatEntry(ffFrame,
textvariable=STATE.motion_threshold,
width=5)
thresholdEntry.grid(row=2, column=2)
Label(ffFrame, text="kS:", anchor="e").grid(row=1, column=3, sticky="ew")
kSEntry = FloatEntry(ffFrame, textvariable=STATE.ks, width=10)
kSEntry.grid(row=1, column=4)
kSEntry.configure(state="readonly")
Label(ffFrame, text="kV:", anchor="e").grid(row=2, column=3, sticky="ew")
kVEntry = FloatEntry(ffFrame, textvariable=STATE.kv, width=10)
kVEntry.grid(row=2, column=4)
kVEntry.configure(state="readonly")
Label(ffFrame, text="kA:", anchor="e").grid(row=3, column=3, sticky="ew")
kAEntry = FloatEntry(ffFrame, textvariable=STATE.ka, width=10)
kAEntry.grid(row=3, column=4)
kAEntry.configure(state="readonly")
Label(ffFrame, text="r-squared:", anchor="e").grid(row=4,
column=3,
sticky="ew")
rSquareEntry = FloatEntry(ffFrame, textvariable=STATE.r_square, width=10)
rSquareEntry.grid(row=4, column=4)
rSquareEntry.configure(state="readonly")
for child in ffFrame.winfo_children():
child.grid_configure(padx=1, pady=1)
# FEEDBACK ANALYSIS FRAME
fbFrame = Frame(STATE.mainGUI, bd=2, relief="groove")
fbFrame.grid(row=1, column=3, columnspan=5)
Label(fbFrame, text="Feedback Analysis").grid(row=0,
column=0,
columnspan=5)
Label(fbFrame, text="Gain Settings Preset:", anchor="e").grid(row=1,
column=0,
sticky="ew")
presetChoices = {
"Default",
"WPILib (2020-)",
"WPILib (Pre-2020)",
"Talon FX",
"Talon SRX (2020-)",
"Talon SRX (Pre-2020)",
"Spark MAX (brushless)",
"Spark MAX (brushed)",
}
presetMenu = OptionMenu(fbFrame, STATE.gain_units_preset,
*sorted(presetChoices))
presetMenu.grid(row=1, column=1)
presetMenu.config(width=12)
STATE.gain_units_preset.trace_add("write", presetGains)
Label(fbFrame, text="Controller Period (s):", anchor="e").grid(row=2,
column=0,
sticky="ew")
periodEntry = FloatEntry(fbFrame, textvariable=STATE.period, width=10)
periodEntry.grid(row=2, column=1)
Label(fbFrame, text="Max Controller Output:", anchor="e").grid(row=3,
column=0,
sticky="ew")
controllerMaxEntry = FloatEntry(fbFrame,
textvariable=STATE.max_controller_output,
width=10)
controllerMaxEntry.grid(row=3, column=1)
Label(fbFrame, text="Time-Normalized Controller:",
anchor="e").grid(row=4, column=0, sticky="ew")
normalizedButton = Checkbutton(fbFrame,
variable=STATE.controller_time_normalized)
normalizedButton.grid(row=4, column=1)
Label(fbFrame, text="Controller Type:", anchor="e").grid(row=5,
column=0,
sticky="ew")
controllerTypes = {"Onboard", "Talon", "Spark"}
controllerTypeMenu = OptionMenu(fbFrame, STATE.controller_type,
*sorted(controllerTypes))
controllerTypeMenu.grid(row=5, column=1)
STATE.controller_type.trace_add("write", enableOffboard)
Label(fbFrame, text="Measurement delay (ms):",
anchor="e").grid(row=6, column=0, sticky="ew")
velocityDelay = FloatEntry(fbFrame,
textvariable=STATE.measurement_delay,
width=10)
velocityDelay.grid(row=6, column=1)
Label(fbFrame, text="Post-Encoder Gearing:", anchor="e").grid(row=7,
column=0,
sticky="ew")
gearingEntry = FloatEntry(fbFrame, textvariable=STATE.gearing, width=10)
gearingEntry.configure(state="disabled")
gearingEntry.grid(row=7, column=1)
Label(fbFrame, text="Encoder EPR:", anchor="e").grid(row=8,
column=0,
sticky="ew")
eprEntry = IntEntry(fbFrame, textvariable=STATE.encoder_epr, width=10)
eprEntry.configure(state="disabled")
eprEntry.grid(row=8, column=1)
Label(fbFrame, text="Has Slave:", anchor="e").grid(row=9,
column=0,
sticky="ew")
hasSlave = Checkbutton(fbFrame, variable=STATE.has_slave)
hasSlave.grid(row=9, column=1)
hasSlave.configure(state="disabled")
STATE.has_slave.trace_add("write", enableOffboard)
Label(fbFrame, text="Slave Update Period (s):",
anchor="e").grid(row=10, column=0, sticky="ew")
slavePeriodEntry = FloatEntry(fbFrame,
textvariable=STATE.slave_period,
width=10)
slavePeriodEntry.grid(row=10, column=1)
slavePeriodEntry.configure(state="disabled")
Label(fbFrame, text="Max Acceptable Position Error (units):",
anchor="e").grid(row=1, column=2, columnspan=2, sticky="ew")
qPEntry = FloatEntry(fbFrame, textvariable=STATE.qp, width=10)
qPEntry.grid(row=1, column=4)
qPEntry.configure(state="disabled")
Label(fbFrame, text="Max Acceptable Velocity Error (units/s):",
anchor="e").grid(row=2, column=2, columnspan=2, sticky="ew")
qVEntry = FloatEntry(fbFrame, textvariable=STATE.qv, width=10)
qVEntry.grid(row=2, column=4)
Label(fbFrame, text="Max Acceptable Control Effort (V):",
anchor="e").grid(row=3, column=2, columnspan=2, sticky="ew")
effortEntry = FloatEntry(fbFrame, textvariable=STATE.max_effort, width=10)
effortEntry.grid(row=3, column=4)
Label(fbFrame, text="Loop Type:", anchor="e").grid(row=4,
column=2,
columnspan=2,
sticky="ew")
loopTypes = {"Position", "Velocity"}
loopTypeMenu = OptionMenu(fbFrame, STATE.loop_type, *sorted(loopTypes))
loopTypeMenu.configure(width=8)
loopTypeMenu.grid(row=4, column=4)
STATE.loop_type.trace_add("write", enableErrorBounds)
# We reset everything to the selected preset when the user changes the loop type
# This prevents people from forgetting to change measurement delays
STATE.loop_type.trace_add("write", presetGains)
Label(fbFrame, text="kV:", anchor="e").grid(row=5, column=2, sticky="ew")
kVFBEntry = FloatEntry(fbFrame, textvariable=STATE.kv, width=10)
kVFBEntry.grid(row=5, column=3)
Label(fbFrame, text="kA:", anchor="e").grid(row=6, column=2, sticky="ew")
kAFBEntry = FloatEntry(fbFrame, textvariable=STATE.ka, width=10)
kAFBEntry.grid(row=6, column=3)
calcGainsButton = Button(
fbFrame,
text="Calculate Optimal Controller Gains",
command=calcGains,
state="disabled",
)
calcGainsButton.grid(row=7, column=2, columnspan=3)
Label(fbFrame, text="kP:", anchor="e").grid(row=8, column=2, sticky="ew")
kPEntry = FloatEntry(fbFrame,
textvariable=STATE.kp,
width=10,
state="readonly").grid(row=8, column=3)
Label(fbFrame, text="kD:", anchor="e").grid(row=9, column=2, sticky="ew")
kDEntry = FloatEntry(fbFrame,
textvariable=STATE.kd,
width=10,
state="readonly").grid(row=9, column=3)
for child in fbFrame.winfo_children():
child.grid_configure(padx=1, pady=1)