From 4ed06c8a5738e03dde054dbd0af9785754e51a25 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 9 Nov 2017 21:10:40 -0800 Subject: [PATCH] make first version of liveplotter --- Firmware/MotorControl/commands.cpp | 10 +++++--- Firmware/MotorControl/low_level.c | 5 ++++ Firmware/MotorControl/low_level.h | 1 + Firmware/MotorControl/protocol.hpp | 8 ++++++ tools/liveplotter.py | 41 ++++++++++++++++++++++++++++++ tools/rate_test.py | 20 +++++++++++++++ 6 files changed, 81 insertions(+), 4 deletions(-) create mode 100644 tools/liveplotter.py create mode 100644 tools/rate_test.py diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 672eb6cc..582e2b1a 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -93,8 +93,8 @@ const Endpoint endpoints[] = { Endpoint::make_property("calibration_current", &motors[0].calibration_current), Endpoint::make_property("phase_inductance", const_cast(&motors[0].phase_inductance)), Endpoint::make_property("phase_resistance", const_cast(&motors[0].phase_resistance)), - Endpoint::make_property("current_meas.phB", const_cast(&motors[0].current_meas.phB)), - Endpoint::make_property("current_meas.phC", const_cast(&motors[0].current_meas.phC)), + Endpoint::make_property("current_meas_phB", const_cast(&motors[0].current_meas.phB)), + Endpoint::make_property("current_meas_phC", const_cast(&motors[0].current_meas.phC)), Endpoint::make_property("DC_calib.phB", &motors[0].DC_calib.phB), Endpoint::make_property("DC_calib.phC", &motors[0].DC_calib.phC), Endpoint::make_property("shunt_conductance", &motors[0].shunt_conductance), @@ -102,6 +102,7 @@ const Endpoint endpoints[] = { Endpoint::make_property("thread_ready", reinterpret_cast(&motors[0].thread_ready)), Endpoint::make_property("control_deadline", &motors[0].control_deadline), Endpoint::make_property("last_cpu_time", &motors[0].last_cpu_time), + Endpoint::make_property("loop_counter", &motors[0].loop_counter), Endpoint::make_object("current_control"), Endpoint::make_property("current_lim", &motors[0].current_control.current_lim), Endpoint::make_property("p_gain", &motors[0].current_control.p_gain), @@ -146,8 +147,8 @@ const Endpoint endpoints[] = { Endpoint::make_property("calibration_current", &motors[1].calibration_current), Endpoint::make_property("phase_inductance", const_cast(&motors[1].phase_inductance)), Endpoint::make_property("phase_resistance", const_cast(&motors[1].phase_resistance)), - Endpoint::make_property("current_meas.phB", const_cast(&motors[1].current_meas.phB)), - Endpoint::make_property("current_meas.phC", const_cast(&motors[1].current_meas.phC)), + Endpoint::make_property("current_meas_phB", const_cast(&motors[1].current_meas.phB)), + Endpoint::make_property("current_meas_phC", const_cast(&motors[1].current_meas.phC)), Endpoint::make_property("DC_calib.phB", &motors[1].DC_calib.phB), Endpoint::make_property("DC_calib.phC", &motors[1].DC_calib.phC), Endpoint::make_property("shunt_conductance", &motors[1].shunt_conductance), @@ -155,6 +156,7 @@ const Endpoint endpoints[] = { Endpoint::make_property("thread_ready", reinterpret_cast(&motors[1].thread_ready)), Endpoint::make_property("control_deadline", &motors[1].control_deadline), Endpoint::make_property("last_cpu_time", &motors[1].last_cpu_time), + Endpoint::make_property("loop_counter", &motors[1].loop_counter), Endpoint::make_object("current_control"), Endpoint::make_property("current_lim", &motors[1].current_control.current_lim), Endpoint::make_property("p_gain", &motors[1].current_control.p_gain), diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index fc317f25..55a556f9 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -126,6 +126,7 @@ Motor_t motors[] = { .spin_up_acceleration = 400.0f, // [rad/s^2] .spin_up_target_vel = 400.0f, // [rad/s] }, + .loop_counter = 0, .timing_log_index = 0, .timing_log = {0}, .anticogging = { @@ -217,6 +218,7 @@ Motor_t motors[] = { .spin_up_acceleration = 400.0f, // [rad/s^2] .spin_up_target_vel = 400.0f, // [rad/s] }, + .loop_counter = 0, .timing_log_index = 0, .timing_log = {0}, .anticogging = { @@ -873,6 +875,7 @@ void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { motor->error = ERROR_FOC_VOLTAGE_TIMING; return; } + ++(motor->loop_counter); } } @@ -1282,6 +1285,8 @@ void control_motor_loop(Motor_t* motor) { if(!FOC_current(motor, 0.0f, Iq)){ break; // in case of error exit loop, motor->error has been set by FOC_current } + + ++(motor->loop_counter); } //We are exiting control, reset Ibus, and update brake current diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index e3cfd829..8ac58b27 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -153,6 +153,7 @@ typedef struct { Rotor_mode_t rotor_mode; Encoder_t encoder; Sensorless_t sensorless; + uint32_t loop_counter; int timing_log_index; uint16_t timing_log[TIMING_LOG_SIZE]; // Cache for remote procedure calls arguments diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index 3ff2058c..1ab4577b 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -377,6 +377,14 @@ inline const char* get_default_json_modifier() { return "\"type\":\"int32\",\"access\":\"rw\""; } template<> +inline const char* get_default_json_modifier() { + return "\"type\":\"uint32\",\"access\":\"r\""; +} +template<> +inline const char* get_default_json_modifier() { + return "\"type\":\"uint32\",\"access\":\"rw\""; +} +template<> inline const char* get_default_json_modifier() { return "\"type\":\"uint16\",\"access\":\"r\""; } diff --git a/tools/liveplotter.py b/tools/liveplotter.py new file mode 100644 index 00000000..7e294dd8 --- /dev/null +++ b/tools/liveplotter.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 +""" +Liveplotter +""" + +import time +import odrive.core +import matplotlib.pyplot as plt +import numpy as np +import threading +from IPython import embed + +data_rate = 100 +plot_rate = 10 +num_samples = 1000 + +myOdrive = odrive.core.find_any() + +plt.ion() +global vals +vals = [] + +def fetch_data(): + global vals + while True: + vals.append(myOdrive.motor0.encoder.pll_pos) + if len(vals) > num_samples: + vals = vals[-num_samples:] + time.sleep(1/data_rate) + +def plot_data(): + global vals + while True: + plt.clf() + plt.plot(vals) + plt.pause(1/plot_rate) + +fetch_thread = threading.Thread(target=fetch_data, daemon=True) +fetch_thread.start() + +plot_data() \ No newline at end of file diff --git a/tools/rate_test.py b/tools/rate_test.py new file mode 100644 index 00000000..191125ed --- /dev/null +++ b/tools/rate_test.py @@ -0,0 +1,20 @@ +import time +import odrive.core +import matplotlib.pyplot as plt +import numpy as np + +myOdrive = odrive.core.find_any() + +plt.ion() + +numFrames = 10000 +vals = [] +for _ in range(numFrames): + vals.append(myOdrive.motor0.loop_counter) + +plt.plot(vals) + +loopsPerFrame = (vals[-1] - vals[0])/numFrames +loopsPerSec = (168000000/(2*10192)) +FramePerSec = loopsPerSec/loopsPerFrame +print(FramePerSec) \ No newline at end of file