diff --git a/CHANGELOG.md b/CHANGELOG.md index baaf20c1..ff881b35 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,6 +3,10 @@ Please add a note of your changes below this heading if you make a Pull Request. # Unreleased +## Added +* Trapezoidal Trajectory Planner + + # Releases ## [0.4.4] - 2018-09-18 ### Fixed diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 7d79b3d6..8813aa60 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -11,18 +11,21 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, - Motor& motor) + Motor& motor, + TrapezoidalTrajectory& trap) : hw_config_(hw_config), config_(config), encoder_(encoder), sensorless_estimator_(sensorless_estimator), controller_(controller), - motor_(motor) + motor_(motor), + trap_(trap) { encoder_.axis_ = this; sensorless_estimator_.axis_ = this; controller_.axis_ = this; motor_.axis_ = this; + trap_.axis_ = this; } static void step_cb_wrapper(void* ctx) { diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 111f9e26..4eac2c4b 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -65,7 +65,8 @@ public: Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, - Motor& motor); + Motor& motor, + TrapezoidalTrajectory& trap); void setup(); void start_thread(); @@ -150,6 +151,7 @@ public: SensorlessEstimator& sensorless_estimator_; Controller& controller_; Motor& motor_; + TrapezoidalTrajectory& trap_; osThreadId thread_id_; volatile bool thread_id_valid_ = false; @@ -188,7 +190,8 @@ public: make_protocol_object("motor", motor_.make_protocol_definitions()), make_protocol_object("controller", controller_.make_protocol_definitions()), make_protocol_object("encoder", encoder_.make_protocol_definitions()), - make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()) + make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()), + make_protocol_object("trap_traj", trap_.make_protocol_definitions()) ); } }; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 43bb206b..d7740a5e 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -44,6 +44,19 @@ void Controller::set_current_setpoint(float current_setpoint) { #endif } +void Controller::move_to_pos(float goal_point) { + planned_move_end_time_ = axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, + vel_setpoint_, axis_->trap_.config_.vel_limit, + axis_->trap_.config_.accel_limit, axis_->trap_.config_.decel_limit); + config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL; + TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); + pos_setpoint_ = myTraj.Y; + vel_setpoint_ = myTraj.Yd; + current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.cpss_to_A; + + planned_move_timer_ = axis_->loop_counter_ * current_meas_period; +} + void Controller::start_anticogging_calibration() { // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) { @@ -82,7 +95,24 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { // Only runs if anticogging_.calib_anticogging is true; non-blocking anticogging_calibration(pos_estimate, vel_estimate); - + float anticogging_pos = pos_estimate; + + // Controlled Move + if (config_.control_mode >= CTRL_MODE_PLANNED_MOVE_CONTROL) { + float time_now = axis_->loop_counter_ * current_meas_period; + if ((time_now - planned_move_timer_) > planned_move_end_time_) { + config_.control_mode = CTRL_MODE_POSITION_CONTROL; + vel_setpoint_ = 0.0f; + current_setpoint_ = 0.0f; + } else { + TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); + pos_setpoint_ = myTraj.Y; + vel_setpoint_ = myTraj.Yd; + current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.cpss_to_A; + } + anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate + } + // Position control // TODO Decide if we want to use encoder or pll position here float vel_des = vel_setpoint_; @@ -103,7 +133,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) if (anticogging_.use_anticogging) { - Iq += anticogging_.cogging_map[mod(static_cast(pos_estimate), axis_->encoder_.config_.cpr)]; + Iq += anticogging_.cogging_map[mod(static_cast(anticogging_pos), axis_->encoder_.config_.cpr)]; } float v_err = vel_des - vel_estimate; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 80f5d25a..dd3f76a5 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -11,7 +11,8 @@ typedef enum { CTRL_MODE_VOLTAGE_CONTROL = 0, CTRL_MODE_CURRENT_CONTROL = 1, CTRL_MODE_VELOCITY_CONTROL = 2, - CTRL_MODE_POSITION_CONTROL = 3 + CTRL_MODE_POSITION_CONTROL = 3, + CTRL_MODE_PLANNED_MOVE_CONTROL = 4 } Motor_control_mode_t; struct ControllerConfig_t { @@ -31,6 +32,9 @@ public: void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); void set_vel_setpoint(float vel_setpoint, float current_feed_forward); void set_current_setpoint(float current_setpoint); + + // Trajectory-Planned control + void move_to_pos(float goal_point); // TODO: make this more similar to other calibration loops void start_anticogging_calibration(); @@ -71,6 +75,9 @@ public: float vel_integrator_current_ = 0.0f; // [A] float current_setpoint_ = 0.0f; // [A] + float planned_move_timer_ = 0.0f; + float planned_move_end_time_ = 0.0f; + // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( @@ -86,11 +93,15 @@ public: make_protocol_property("vel_limit", &config_.vel_limit) ), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, - "pos_setpoint", "vel_feed_forward", "current_feed_forward"), + "pos_setpoint", + "vel_feed_forward", + "current_feed_forward"), make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint, - "vel_setpoint", "current_feed_forward"), + "vel_setpoint", + "current_feed_forward"), make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, - "current_setpoint"), + "current_setpoint"), + make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"), make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) ); } diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 18b88433..1f9b38cb 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -14,6 +14,7 @@ SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; ControllerConfig_t controller_configs[AXIS_COUNT]; MotorConfig_t motor_configs[AXIS_COUNT]; AxisConfig_t axis_configs[AXIS_COUNT]; +TrapTrajConfig_t trap_configs[AXIS_COUNT]; bool user_config_loaded_; SystemStats_t system_stats_ = { 0 }; @@ -26,6 +27,7 @@ typedef Config< SensorlessEstimator::Config_t[AXIS_COUNT], ControllerConfig_t[AXIS_COUNT], MotorConfig_t[AXIS_COUNT], + TrapTrajConfig_t[AXIS_COUNT], AxisConfig_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { @@ -35,6 +37,7 @@ void save_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, + &trap_configs, &axis_configs)) { //printf("saving configuration failed\r\n"); osDelay(5); } else { @@ -51,6 +54,7 @@ void load_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, + &trap_configs, &axis_configs)) { //If loading failed, restore defaults board_config = BoardConfig_t(); @@ -59,6 +63,7 @@ void load_configuration(void) { sensorless_configs[i] = SensorlessEstimator::Config_t(); controller_configs[i] = ControllerConfig_t(); motor_configs[i] = MotorConfig_t(); + trap_configs[i] = TrapTrajConfig_t(); axis_configs[i] = AxisConfig_t(); } } else { @@ -162,8 +167,9 @@ int odrive_main(void) { Motor *motor = new Motor(hw_configs[i].motor_config, hw_configs[i].gate_driver_config, motor_configs[i]); + TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]); axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i], - *encoder, *sensorless_estimator, *controller, *motor); + *encoder, *sensorless_estimator, *controller, *motor, *trap); } // Start ADC for temperature measurements and user measurements diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 4e4db160..27be8455 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -109,9 +109,11 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c #include #include #include +#include #include #include + #endif // __cplusplus diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp new file mode 100644 index 00000000..0c5f3f35 --- /dev/null +++ b/Firmware/MotorControl/trapTraj.cpp @@ -0,0 +1,102 @@ +#include +#include "odrive_main.h" + +// Standard sign function, implemented to match the Python impelmentation +template +int sign(T val) { + if (val == T(0)) + return T(0); + else + return (std::signbit(val)) ? -1 : 1; +} + +TrapezoidalTrajectory::TrapezoidalTrajectory(TrapTrajConfig_t &config) : config_(config) {} + +float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, + float Vi, float Vmax, + float Amax, float Dmax) { + float dx_stop = (Vi * Vi) / (Dmax * 2.0f); + float dX = Xf - Xi; + int s = sign(dX); + + float Ar = s * Amax; // Maximum Acceleration (signed) + float Dr = -1.0f * s * Dmax; // Maximum Deceleration (signed) + float Vr = s * Vmax; // Maximum Velocity (signed) + + float Ta; + float Tv; + float Td; + + // Checking for overshoot on "minimum stop" + if (fabs(dX) <= dx_stop) { + Ta = 0; + Tv = 0; + Vr = Vi; + Dr = -1.0f * sign(Vi) * Dmax; + Td = fabs(Vi) / Dmax; + } else { + // Handle the case where initial velocity > Max velocity + if ((s * Vi) > (s * Vr)) { + Ar = -1.0f * s * Amax; + } + + Ta = (Vr - Vi) / Ar; // Acceleration time + Td = (-Vr) / Dr; // Deceleration time + + // Peak Velocity handling + float dXmin = Ta * (Vr + Vi) / 2.0f + Td * Vr / 2.0f; + + // Short move handling + if (fabs(dX) < fabs(dXmin)) { + Vr = s * sqrt((-((Vi * Vi) / Ar) - 2.0f * dX) / (1.0f / Dr - 1.0f / Ar)); + Ta = std::max(0.0f, (Vr - Vi) / Ar); + Tv = 0; + Td = std::max(0.0f, -Vr / Dr); + } else { + Tv = (dX - dXmin) / Vr; + } + } + + // Populate object's values + + Xf_ = Xf; + Xi_ = Xi; + Vi_ = Vi; + + Ar_ = Ar; + Dr_ = Dr; + Vr_ = Vr; + + Ta_ = Ta; + Tv_ = Tv; + Td_ = Td; + + yAccel_ = (Ar * Ta * Ta) / 2.0f + (Vi * Ta) + Xi; + Tav_ = Ta + Tv; + + return Ta + Tv + Td; +} + +TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { + TrapTrajStep_t trajStep; + if (t < 0.0f) { // Initial Conditions + trajStep.Y = Xi_; + trajStep.Yd = Vi_; + trajStep.Ydd = Ar_; + } else if (t < Ta_) { // Accelerating + trajStep.Y = (Ar_ * (t * t) / 2.0f) + (Vi_ * t) + Xi_; + trajStep.Yd = (Ar_ * t) + Vi_; + trajStep.Ydd = Ar_; + } else if (t < Ta_ + Tv_) { // Coasting + trajStep.Y = yAccel_ + (Vr_ * (t - Ta_)); + trajStep.Yd = Vr_; + trajStep.Ydd = 0; + } else if (t < Ta_ + Tv_ + Td_) { // Deceleration + float Tdc = t - Tav_; + trajStep.Y = yAccel_ + (Vr_ * (t - Ta_)) + Dr_ * (Tdc * Tdc) / 2.0f; + trajStep.Yd = Vr_ + Dr_ * Tdc; + trajStep.Ydd = Dr_; + } + + return trajStep; +} \ No newline at end of file diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp new file mode 100644 index 00000000..a30a8616 --- /dev/null +++ b/Firmware/MotorControl/trapTraj.hpp @@ -0,0 +1,55 @@ +#ifndef _TRAP_TRAJ_H +#define _TRAP_TRAJ_H + +struct TrapTrajConfig_t { + float vel_limit = 20000.0f; + float accel_limit = 5000.0f; + float decel_limit = 5000.0f; + float cpss_to_A = 0.0f; +}; + +struct TrapTrajStep_t { + float Y; + float Yd; + float Ydd; +}; + +class TrapezoidalTrajectory { + public: + Axis *axis_ = nullptr; // set by Axis constructor + TrapTrajConfig_t &config_; + + TrapezoidalTrajectory(TrapTrajConfig_t &config); + + float planTrapezoidal(float Xf, float Xi, + float Vi, float Vmax, + float Amax, float Dmax); + + TrapTrajStep_t evalTrapTraj(float t); + + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_object("config", + make_protocol_property("vel_limit", &config_.vel_limit), + make_protocol_property("accel_limit", &config_.accel_limit), + make_protocol_property("decel_limit", &config_.decel_limit))); + } + + private: + float yAccel_; + + float Xi_; + float Xf_; + float Vi_; + + float Ar_; + float Dr_; + float Vr_; + + float Ta_; + float Tv_; + float Td_; + float Tav_; +}; + +#endif \ No newline at end of file diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index f242b259..5c573d6a 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -155,6 +155,7 @@ build{ 'MotorControl/encoder.cpp', 'MotorControl/controller.cpp', 'MotorControl/sensorless_estimator.cpp', + 'MotorControl/trapTraj.cpp', 'MotorControl/main.cpp', 'communication/communication.cpp', 'communication/ascii_protocol.cpp', diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/Motion Planning/FIR_Planner.py new file mode 100644 index 00000000..5cd54468 --- /dev/null +++ b/tools/Motion Planning/FIR_Planner.py @@ -0,0 +1,203 @@ +# Copyright (c) 2018 Paul Guénette + +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import numpy as np +import math +import matplotlib.pyplot as plt +import random + +# Symbol Description +# Ta, Tv and Td Duration of the stages of the AL profile +# q0f , v0f and a0f Initial conditions of the jerk-limited trajectory +# q0 and v0 Adapted initial conditions for the AL profile +# qe Position set-point +# s Direction (sign) of the trajectory +# vmax, amax, dmax and jmax Kinematic bounds +# vr, ar and dr Reached values of velocity and acceleration +# Tj , Tja, Tjv and Tjd Length of the constant jerk stages (FIR filter time) + + +def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): + + dX_stop = Vi**2 / (2*Dmax) # Minimum stopping distance + dX = Xf - Xi # Distance to travel + + s = np.sign(dX) # Sign of travel direction + + Ar = s*Amax # Maximum Acceleration (signed) + Dr = -s*Dmax # Maximum Deceleration (signed) + Vr = s*Vmax # Maximum Velocity (signed) + + if abs(dX) <= dX_stop: # Check for an overshoot condition (decelerate only) + Ta = 0 + Tv = 0 + Vr = Vi + Dr = -np.sign(Vi)*Dmax + Td = abs(Vi) / Dmax + + print("Overshoot Move:") + print("dX: {:.3f}\tdx_Stop: {:.3f}".format(dX, dX_stop)) + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( + Xf, Xi, Vi, Vmax, Amax)) + print("Ta: {:.3f}\tTv: {:.3f}\tTd: {:.3f}".format(Ta, Tv, Td)) + print("Ar: {:.3f}\tDr: {:.3f}\tVr: {:.3f}".format(Ar, Dr, Vr)) + print() + + else: + # Correct initial acceleration direction if needed + if s*Vi > s*Vr: + Ar = -s*Amax + + Ta = (Vr - Vi)/Ar # Acceleration Time + Td = -Vr/Dr # Deceleration Time + + # Peak velocity handling + dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0 + + # Short move handling + if abs(dX) < abs(dXmin): + print("Short Move:") + print("dX: {:.3f}\tdXmin: {:.3f}".format(dX, dXmin)) + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( + Xf, Xi, Vi, Vmax, Amax)) + print("Ta: {:.3f}\tTd: {:.3f}\tVr: {:.3f}".format(Ta, Td, Vr)) + print() + + Vr = s*math.sqrt((-(Vi**2/Ar)-2*dX)/(1/Dr-1/Ar)) + Ta = max(0, (Vr - Vi)/Ar) + Tv = 0 + Td = max(0, -Vr/Dr) + else: + Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v + + # We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory + # Create the time series and preallocate the position, velocity, and acceleration arrays + t_traj = np.linspace(0, Ta+Tv+Td, 10000) + y = [None]*len(t_traj) + yd = [None]*len(t_traj) + ydd = [None]*len(t_traj) + + # We only know acceleration (Ar and Dr), so we integrate to create + # the velocity and position curves + y_Accel = (Ar*Ta*Ta) / 2 + (Vi * Ta) + Xi + Tav = Ta + Tv + + for i in range(len(t_traj)): + t = t_traj[i] + if(t < 0): # Initial conditions + y[i] = Xi + yd[i] = Vi + ydd[i] = Ar + elif(t < Ta): # Acceleration + y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi + yd[i] = (Ar * t) + Vi + ydd[i] = Ar + elif(t < Ta+Tv): # Coasting + y[i] = y_Accel + (Vr * (t - Ta)) + yd[i] = Vr + ydd[i] = 0 + elif(t <= Ta+Tv+Td): # Deceleration + Tdc = t - Tav + y[i] = y_Accel + (Vr * (t - Ta)) + Dr*((Tdc)*(Tdc))/2 + yd[i] = Vr + Dr*(Tdc) + ydd[i] = Dr + + return (y, yd, ydd, t_traj) + + +numRows = 2 +numCols = 2 +fig, axes = plt.subplots(numRows, numCols) +random.seed() +for x in range(numRows*numCols): + + # Vmax = random.uniform(0.1, 20) + # Amax = random.uniform(0.1, 4) + # Dmax = Amax + + # Xf = random.uniform(-100.0, 100.0) + # Xi = random.uniform(-100.0, 100.0) + # Vi = random.uniform(-Vmax*2, Vmax*2) + + Vmax = 100000.0 + Amax = 100000.0 + Dmax = Amax + Xf = 0 + Xi = 1000000 + Vi = 0 + + (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax) + + if(abs(Xf-Xi) <= Vi**2 / (2*Dmax)): + print("Overshoot: ", Xf) + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( + Xf, Xi, Vi, Vmax, Amax)) + print("Y: {:.3f}\tYd: {:.3f}\tYdd: {:.3f}".format( + Y[-1], Yd[-1], Ydd[-1])) + print() + (Y2, Yd2, Ydd2, t2) = FIR_trapPlan(Xf, Y[-1], Yd[-1], Vmax, Amax, Dmax) + Y.extend(Y2) + Yd.extend(Yd2) + Ydd.extend(Ydd2) + t2 = t2 + t[-1] + t = np.append(t, t2, axis=0) + + if(abs(Xf-Y[-1]) > 0.0001): + print("Bad Final Position") + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( + Xf, Xi, Vi, Vmax, Amax)) + print() + # plt.figure() + # plt.subplot(2,1,1) + # plt.plot(t, Y) + # plt.plot(t, Yd) + # plt.plot(t[-1], Xf, 'b*') + # plt.plot(t[-1], 0, 'r*') + + # plt.subplot(2,1,2) + # plt.plot(t, Ydd) + + # plt.show() + elif(abs(Yd[-1]) > 0.0001): + print("Bad Final Velocity") + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( + Xf, Xi, Vi, Vmax, Amax)) + print() + # plt.figure() + # plt.plot(t, Y) + # plt.plot(t, Yd) + # # plt.plot(t, Ydd) + # plt.show() + + ax1 = axes[int(x/numCols), x % numCols] + ax1.plot(t, Y) + ax1.plot(t, Yd) + ax1.plot(t[-1], Xf, 'b*') + ax1.plot(t[-1], 0, 'r*') + + # ax2 = ax1.twinx() + # ax2.plot(t, Ydd, color='tab:green') + # ax2.tick_params(axis='y', labelcolor='tab:green') + dX = abs(Xf - Y[-1]) + dV = abs(0 - Yd[-1]) + axes[int(x/numCols), x % numCols].set_title( + 'Xf: {:.3f} Xi: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xf, Xi, dX, dV)) + +plt.show()