Merge pull request #252 from madcowswe/traptraj

Trapezoidal Trajectory

CHANGELOG.md

@@ -4,6 +4,7 @@ Please add a note of your changes below this heading if you make a Pull Request.
 # Unreleased
 
 ### Added
+* **Trapezoidal Trajectory Planner**
 * -Wdouble-promotion warning to compilation
 
 ### Changed

Firmware/LICENSE

@@ -1,6 +1,6 @@
 The MIT License (MIT)
 
-Copyright (c) 2016 Oskar Weigl (madcowswe)
+Copyright (c) 2016-2018 Oskar Weigl
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal

Firmware/MotorControl/axis.cpp

@@ -7,22 +7,25 @@
 #include "odrive_main.h"
 
 Axis::Axis(const AxisHardwareConfig_t& hw_config,
-           AxisConfig_t& config,
+           Config_t& 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) {
@@ -179,7 +182,7 @@ bool Axis::run_sensorless_spin_up() {
 bool Axis::run_sensorless_control_loop() {
     set_step_dir_enabled(config_.enable_step_dir);
     run_control_loop([this](){
-        if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL)
+        if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL)
             return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false;
 
         // Note that all estimators are updated in the loop prefix in run_control_loop

Firmware/MotorControl/axis.hpp

@@ -5,40 +5,6 @@
 #error "This file should not be included directly. Include odrive_main.h instead."
 #endif
 
-// Warning: Do not reorder these enum values.
-// The state machine uses ">" comparision on them.
-enum AxisState_t {
-    AXIS_STATE_UNDEFINED = 0,           //<! will fall through to idle
-    AXIS_STATE_IDLE = 1,                //<! disable PWM and do nothing
-    AXIS_STATE_STARTUP_SEQUENCE = 2, //<! the actual sequence is defined by the config.startup_... flags
-    AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3,   //<! run all calibration procedures, then idle
-    AXIS_STATE_MOTOR_CALIBRATION = 4,   //<! run motor calibration
-    AXIS_STATE_SENSORLESS_CONTROL = 5,  //<! run sensorless control
-    AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
-    AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
-    AXIS_STATE_CLOSED_LOOP_CONTROL = 8  //<! run closed loop control
-};
-
-struct AxisConfig_t {
-    bool startup_motor_calibration = false;   //<! run motor calibration at startup, skip otherwise
-    bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
-                                               // this only has an effect if encoder.config.use_index is also true
-    bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
-    bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
-    bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
-    bool enable_step_dir = false; //<! enable step/dir input after calibration
-                                 //   For M0 this has no effect if enable_uart is true
-
-    float counts_per_step = 2.0f;
-
-    // Spinup settings
-    float ramp_up_time = 0.4f;            // [s]
-    float ramp_up_distance = 4 * M_PI;    // [rad]
-    float spin_up_current = 10.0f;        // [A]
-    float spin_up_acceleration = 400.0f;  // [rad/s^2]
-    float spin_up_target_vel = 400.0f;    // [rad/s]
-};
-
 class Axis {
 public:
     enum Error_t {
@@ -56,16 +22,51 @@ public:
         ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
     };
 
+    // Warning: Do not reorder these enum values.
+    // The state machine uses ">" comparision on them.
+    enum State_t {
+        AXIS_STATE_UNDEFINED = 0,           //<! will fall through to idle
+        AXIS_STATE_IDLE = 1,                //<! disable PWM and do nothing
+        AXIS_STATE_STARTUP_SEQUENCE = 2, //<! the actual sequence is defined by the config.startup_... flags
+        AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3,   //<! run all calibration procedures, then idle
+        AXIS_STATE_MOTOR_CALIBRATION = 4,   //<! run motor calibration
+        AXIS_STATE_SENSORLESS_CONTROL = 5,  //<! run sensorless control
+        AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
+        AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
+        AXIS_STATE_CLOSED_LOOP_CONTROL = 8  //<! run closed loop control
+    };
+
+    struct Config_t {
+        bool startup_motor_calibration = false;   //<! run motor calibration at startup, skip otherwise
+        bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
+                                                // this only has an effect if encoder.config.use_index is also true
+        bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
+        bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
+        bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
+        bool enable_step_dir = false; //<! enable step/dir input after calibration
+                                    //   For M0 this has no effect if enable_uart is true
+
+        float counts_per_step = 2.0f;
+
+        // Spinup settings
+        float ramp_up_time = 0.4f;            // [s]
+        float ramp_up_distance = 4 * M_PI;    // [rad]
+        float spin_up_current = 10.0f;        // [A]
+        float spin_up_acceleration = 400.0f;  // [rad/s^2]
+        float spin_up_target_vel = 400.0f;    // [rad/s]
+    };
+
     enum thread_signals {
         M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
     };
 
     Axis(const AxisHardwareConfig_t& hw_config,
-            AxisConfig_t& config,
+            Config_t& config,
             Encoder& encoder,
             SensorlessEstimator& sensorless_estimator,
             Controller& controller,
-            Motor& motor);
+            Motor& motor,
+            TrapezoidalTrajectory& trap);
 
     void setup();
     void start_thread();
@@ -144,12 +145,13 @@ public:
     void run_state_machine_loop();
 
     const AxisHardwareConfig_t& hw_config_;
-    AxisConfig_t& config_;
+    Config_t& config_;
 
     Encoder& encoder_;
     SensorlessEstimator& sensorless_estimator_;
     Controller& controller_;
     Motor& motor_;
+    TrapezoidalTrajectory& trap_;
 
     osThreadId thread_id_;
     volatile bool thread_id_valid_ = false;
@@ -157,9 +159,9 @@ public:
     // variables exposed on protocol
     Error_t error_ = ERROR_NONE;
     bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir
-    AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
-    AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
-    AxisState_t& current_state_ = task_chain_[0];
+    State_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
+    State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
+    State_t& current_state_ = task_chain_[0];
     uint32_t loop_counter_ = 0;
 
     // Communication protocol definitions
@@ -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())
         );
     }
 };

Firmware/MotorControl/controller.cpp

@@ -2,7 +2,7 @@
 #include "odrive_main.h"
 
 
-Controller::Controller(ControllerConfig_t& config) :
+Controller::Controller(Config_t& config) :
     config_(config)
 {}
 
@@ -44,6 +44,15 @@ void Controller::set_current_setpoint(float current_setpoint) {
 #endif
 }
 
+void Controller::move_to_pos(float goal_point) {
+    axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_,
+                                 axis_->trap_.config_.vel_limit,
+                                 axis_->trap_.config_.accel_limit,
+                                 axis_->trap_.config_.decel_limit);
+    traj_start_loop_count_ = axis_->loop_counter_;
+    config_.control_mode = CTRL_MODE_TRAJECTORY_CONTROL;
+}
+
 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 +91,28 @@ 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;
+
+    // Trajectory control
+    if (config_.control_mode == CTRL_MODE_TRAJECTORY_CONTROL) {
+        // Note: uint32_t loop count delta is OK across overflow
+        // Beware of negative deltas, as they will not be well behaved due to uint!
+        float t = (axis_->loop_counter_ - traj_start_loop_count_) * current_meas_period;
+        if (t > axis_->trap_.Tf_) {
+            // Drop into position control mode when done to avoid problems on loop counter delta overflow
+            config_.control_mode = CTRL_MODE_POSITION_CONTROL;
+            // pos_setpoint already set by trajectory
+            vel_setpoint_ = 0.0f;
+            current_setpoint_ = 0.0f;
+        } else {
+            TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(t);
+            pos_setpoint_ = traj_step.Y;
+            vel_setpoint_ = traj_step.Yd;
+            current_setpoint_ = traj_step.Ydd * axis_->trap_.config_.A_per_css;
+        }
+        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<int>(pos_estimate), axis_->encoder_.config_.cpr)];
+        Iq += anticogging_.cogging_map[mod(static_cast<int>(anticogging_pos), axis_->encoder_.config_.cpr)];
     }
 
     float v_err = vel_des - vel_estimate;

Firmware/MotorControl/controller.hpp

@@ -5,32 +5,36 @@
 #error "This file should not be included directly. Include odrive_main.h instead."
 #endif
 
-// Note: these should be sorted from lowest level of control to
-// highest level of control, to allow "<" style comparisons.
-typedef enum {
-    CTRL_MODE_VOLTAGE_CONTROL = 0,
-    CTRL_MODE_CURRENT_CONTROL = 1,
-    CTRL_MODE_VELOCITY_CONTROL = 2,
-    CTRL_MODE_POSITION_CONTROL = 3
-} Motor_control_mode_t;
-
-struct ControllerConfig_t {
-    Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL;  //see: Motor_control_mode_t
-    float pos_gain = 20.0f;  // [(counts/s) / counts]
-    float vel_gain = 5.0f / 10000.0f;  // [A/(counts/s)]
-    // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
-    float vel_integrator_gain = 10.0f / 10000.0f;  // [A/(counts/s * s)]
-    float vel_limit = 20000.0f;           // [counts/s]
-};
-
 class Controller {
 public:
-    Controller(ControllerConfig_t& config);
+    // Note: these should be sorted from lowest level of control to
+    // highest level of control, to allow "<" style comparisons.
+    enum ControlMode_t{
+        CTRL_MODE_VOLTAGE_CONTROL = 0,
+        CTRL_MODE_CURRENT_CONTROL = 1,
+        CTRL_MODE_VELOCITY_CONTROL = 2,
+        CTRL_MODE_POSITION_CONTROL = 3,
+        CTRL_MODE_TRAJECTORY_CONTROL = 4
+    };
+
+    struct Config_t {
+        ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL;  //see: Motor_control_mode_t
+        float pos_gain = 20.0f;  // [(counts/s) / counts]
+        float vel_gain = 5.0f / 10000.0f;  // [A/(counts/s)]
+        // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
+        float vel_integrator_gain = 10.0f / 10000.0f;  // [A/(counts/s * s)]
+        float vel_limit = 20000.0f;           // [counts/s]
+    };
+
+    Controller(Config_t& config);
     void reset();
 
     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();
@@ -38,7 +42,7 @@ public:
 
     bool update(float pos_estimate, float vel_estimate, float* current_setpoint);
 
-    ControllerConfig_t& config_;
+    Config_t& config_;
     Axis* axis_ = nullptr; // set by Axis constructor
 
     // TODO: anticogging overhaul:
@@ -71,6 +75,8 @@ public:
     float vel_integrator_current_ = 0.0f;  // [A]
     float current_setpoint_ = 0.0f;        // [A]
 
+    uint32_t traj_start_loop_count_ = 0;
+
     // Communication protocol definitions
     auto make_protocol_definitions() {
         return make_protocol_member_list(
@@ -91,6 +97,7 @@ public:
                 "vel_setpoint", "current_feed_forward"),
             make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint,
                 "current_setpoint"),
+            make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "goal_point"),
             make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration)
         );
     }

Firmware/MotorControl/encoder.cpp

@@ -95,9 +95,9 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
 // TODO: Do the scan with current, not voltage!
 bool Encoder::run_index_search() {
     float voltage_magnitude;
-    if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
+    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT)
         voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
-    else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL)
+    else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL)
         voltage_magnitude = axis_->motor_.config_.calibration_current;
     else
         return false;
@@ -142,9 +142,9 @@ bool Encoder::run_offset_calibration() {
     shadow_count_ = count_in_cpr_;
 
     float voltage_magnitude;
-    if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
+    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT)
         voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
-    else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL)
+    else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL)
         voltage_magnitude = axis_->motor_.config_.calibration_current;
     else
         return false;

Firmware/MotorControl/main.cpp

@@ -11,9 +11,10 @@
 BoardConfig_t board_config;
 Encoder::Config_t encoder_configs[AXIS_COUNT];
 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];
+Controller::Config_t controller_configs[AXIS_COUNT];
+Motor::Config_t motor_configs[AXIS_COUNT];
+Axis::Config_t axis_configs[AXIS_COUNT];
+TrapezoidalTrajectory::Config_t trap_configs[AXIS_COUNT];
 bool user_config_loaded_;
 
 SystemStats_t system_stats_ = { 0 };
@@ -24,9 +25,10 @@ typedef Config<
     BoardConfig_t,
     Encoder::Config_t[AXIS_COUNT],
     SensorlessEstimator::Config_t[AXIS_COUNT],
-    ControllerConfig_t[AXIS_COUNT],
-    MotorConfig_t[AXIS_COUNT],
-    AxisConfig_t[AXIS_COUNT]> ConfigFormat;
+    Controller::Config_t[AXIS_COUNT],
+    Motor::Config_t[AXIS_COUNT],
+    TrapezoidalTrajectory::Config_t[AXIS_COUNT],
+    Axis::Config_t[AXIS_COUNT]> ConfigFormat;
 
 void save_configuration(void) {
     if (ConfigFormat::safe_store_config(
@@ -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,15 +54,17 @@ void load_configuration(void) {
                 &sensorless_configs,
                 &controller_configs,
                 &motor_configs,
+                &trap_configs,
                 &axis_configs)) {
         //If loading failed, restore defaults
         board_config = BoardConfig_t();
         for (size_t i = 0; i < AXIS_COUNT; ++i) {
             encoder_configs[i] = Encoder::Config_t();
             sensorless_configs[i] = SensorlessEstimator::Config_t();
-            controller_configs[i] = ControllerConfig_t();
-            motor_configs[i] = MotorConfig_t();
-            axis_configs[i] = AxisConfig_t();
+            controller_configs[i] = Controller::Config_t();
+            motor_configs[i] = Motor::Config_t();
+            trap_configs[i] = TrapezoidalTrajectory::Config_t();
+            axis_configs[i] = Axis::Config_t();
         }
     } else {
         user_config_loaded_ = true;
@@ -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

Firmware/MotorControl/motor.cpp

@@ -6,8 +6,8 @@
 
 
 Motor::Motor(const MotorHardwareConfig_t& hw_config,
-         const GateDriverHardwareConfig_t& gate_driver_config,
-         MotorConfig_t& config) :
+             const GateDriverHardwareConfig_t& gate_driver_config,
+             Config_t& config) :
         hw_config_(hw_config),
         gate_driver_config_(gate_driver_config),
         config_(config),
@@ -298,10 +298,11 @@ bool Motor::FOC_voltage(float v_d, float v_q, float phase) {
 }
 
 bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
-    Current_control_t* ictrl = &current_control_;
+    // Syntactic sugar
+    CurrentControl_t& ictrl = current_control_;
 
     // For Reporting
-    ictrl->Iq_setpoint = Iq_des;
+    ictrl.Iq_setpoint = Iq_des;
 
     // Clarke transform
     float Ialpha = -current_meas_.phB - current_meas_.phC;
@@ -312,7 +313,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
     float s = arm_sin_f32(phase);
     float Id = c * Ialpha + s * Ibeta;
     float Iq = c * Ibeta - s * Ialpha;
-    ictrl->Iq_measured = Iq;
+    ictrl.Iq_measured = Iq;
 
     // Current error
     float Ierr_d = Id_des - Id;
@@ -320,8 +321,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
 
     // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau)
     // Apply PI control
-    float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain;
-    float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain;
+    float Vd = ictrl.v_current_control_integral_d + Ierr_d * ictrl.p_gain;
+    float Vq = ictrl.v_current_control_integral_q + Ierr_q * ictrl.p_gain;
 
     float mod_to_V = (2.0f / 3.0f) * vbus_voltage;
     float V_to_mod = 1.0f / mod_to_V;
@@ -335,23 +336,23 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
         mod_d *= mod_scalefactor;
         mod_q *= mod_scalefactor;
         // TODO make decayfactor configurable
-        ictrl->v_current_control_integral_d *= 0.99f;
-        ictrl->v_current_control_integral_q *= 0.99f;
+        ictrl.v_current_control_integral_d *= 0.99f;
+        ictrl.v_current_control_integral_q *= 0.99f;
     } else {
-        ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period);
-        ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period);
+        ictrl.v_current_control_integral_d += Ierr_d * (ictrl.i_gain * current_meas_period);
+        ictrl.v_current_control_integral_q += Ierr_q * (ictrl.i_gain * current_meas_period);
     }
 
     // Compute estimated bus current
-    ictrl->Ibus = mod_d * Id + mod_q * Iq;
+    ictrl.Ibus = mod_d * Id + mod_q * Iq;
 
     // Inverse park transform
     float mod_alpha = c * mod_d - s * mod_q;
     float mod_beta = c * mod_q + s * mod_d;
 
     // Report final applied voltage in stationary frame (for sensorles estimator)
-    ictrl->final_v_alpha = mod_to_V * mod_alpha;
-    ictrl->final_v_beta = mod_to_V * mod_beta;
+    ictrl.final_v_alpha = mod_to_V * mod_alpha;
+    ictrl.final_v_beta = mod_to_V * mod_beta;
 
     // Apply SVM
     if (!enqueue_modulation_timings(mod_alpha, mod_beta))

Firmware/MotorControl/motor.hpp

@@ -7,51 +7,6 @@
 
 #include "drv8301.h"
 
-typedef enum {
-    MOTOR_TYPE_HIGH_CURRENT = 0,
-    // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented
-    MOTOR_TYPE_GIMBAL = 2
-} Motor_type_t;
-
-typedef struct {
-    float phB;
-    float phC;
-} Iph_BC_t;
-
-typedef struct {
-    float p_gain; // [V/A]
-    float i_gain; // [V/As]
-    float v_current_control_integral_d; // [V]
-    float v_current_control_integral_q; // [V]
-    float Ibus; // DC bus current [A]
-    // Voltage applied at end of cycle:
-    float final_v_alpha; // [V]
-    float final_v_beta; // [V]
-    float Iq_setpoint;
-    float Iq_measured;
-    float max_allowed_current;
-} Current_control_t;
-
-// NOTE: for gimbal motors, all units of A are instead V.
-// example: vel_gain is [V/(count/s)] instead of [A/(count/s)]
-// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
-typedef struct {
-    bool pre_calibrated = false; // can be set to true to indicate that all values here are valid
-    int32_t pole_pairs = 7;
-    float calibration_current = 10.0f;    // [A]
-    float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
-    float phase_inductance = 0.0f;        // to be set by measure_phase_inductance
-    float phase_resistance = 0.0f;        // to be set by measure_phase_resistance
-    int32_t direction = 1;                // 1 or -1
-    Motor_type_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
-    // Read out max_allowed_current to see max supported value for current_lim.
-    // float current_lim = 70.0f; //[A]
-    float current_lim = 10.0f;  //[A]
-    // Value used to compute shunt amplifier gains
-    float requested_current_range = 70.0f; // [A]
-    float current_control_bandwidth = 1000.0f;  // [rad/s]
-} MotorConfig_t;
-
 class Motor {
 public:
     enum Error_t {
@@ -68,6 +23,51 @@ public:
         ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200
     };
 
+    enum MotorType_t {
+        MOTOR_TYPE_HIGH_CURRENT = 0,
+        // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented
+        MOTOR_TYPE_GIMBAL = 2
+    };
+
+    struct Iph_BC_t {
+        float phB;
+        float phC;
+    };
+
+    struct CurrentControl_t{
+        float p_gain; // [V/A]
+        float i_gain; // [V/As]
+        float v_current_control_integral_d; // [V]
+        float v_current_control_integral_q; // [V]
+        float Ibus; // DC bus current [A]
+        // Voltage applied at end of cycle:
+        float final_v_alpha; // [V]
+        float final_v_beta; // [V]
+        float Iq_setpoint;
+        float Iq_measured;
+        float max_allowed_current;
+    };
+
+    // NOTE: for gimbal motors, all units of A are instead V.
+    // example: vel_gain is [V/(count/s)] instead of [A/(count/s)]
+    // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
+    struct Config_t {
+        bool pre_calibrated = false; // can be set to true to indicate that all values here are valid
+        int32_t pole_pairs = 7;
+        float calibration_current = 10.0f;    // [A]
+        float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
+        float phase_inductance = 0.0f;        // to be set by measure_phase_inductance
+        float phase_resistance = 0.0f;        // to be set by measure_phase_resistance
+        int32_t direction = 1;                // 1 or -1
+        MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
+        // Read out max_allowed_current to see max supported value for current_lim.
+        // float current_lim = 70.0f; //[A]
+        float current_lim = 10.0f;  //[A]
+        // Value used to compute shunt amplifier gains
+        float requested_current_range = 70.0f; // [A]
+        float current_control_bandwidth = 1000.0f;  // [rad/s]
+    };
+
     enum TimingLog_t {
         TIMING_LOG_GENERAL,
         TIMING_LOG_ADC_CB_I,
@@ -90,7 +90,7 @@ public:
 
     Motor(const MotorHardwareConfig_t& hw_config,
          const GateDriverHardwareConfig_t& gate_driver_config,
-         MotorConfig_t& config);
+         Config_t& config);
 
     bool arm();
     void disarm();
@@ -119,7 +119,7 @@ public:
 
     const MotorHardwareConfig_t& hw_config_;
     const GateDriverHardwareConfig_t gate_driver_config_;
-    MotorConfig_t& config_;
+    Config_t& config_;
     Axis* axis_ = nullptr; // set by Axis constructor
 
 //private:
@@ -144,7 +144,7 @@ public:
     Iph_BC_t current_meas_ = {0.0f, 0.0f};
     Iph_BC_t DC_calib_ = {0.0f, 0.0f};
     float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup)
-    Current_control_t current_control_ = {
+    CurrentControl_t current_control_ = {
         .p_gain = 0.0f,        // [V/A] should be auto set after resistance and inductance measurement
         .i_gain = 0.0f,        // [V/As] should be auto set after resistance and inductance measurement
         .v_current_control_integral_d = 0.0f,

Firmware/MotorControl/odrive_main.h

@@ -109,6 +109,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_c
 #include <sensorless_estimator.hpp>
 #include <controller.hpp>
 #include <motor.hpp>
+#include <trapTraj.hpp>
 #include <axis.hpp>
 #include <communication/communication.h>
 

Firmware/MotorControl/trapTraj.cpp

@@ -0,0 +1,94 @@
+#include <math.h>
+#include "odrive_main.h"
+#include "utils.h"
+
+// A sign function where input 0 has positive sign (not 0)
+float sign_hard(float val) {
+    return (std::signbit(val)) ? -1.0f : 1.0f;
+}
+
+// Symbol                     Description
+// Ta, Tv and Td              Duration of the stages of the AL profile
+// Xi and Vi                  Adapted initial conditions for the AL profile
+// Xf                         Position set-point
+// s                          Direction (sign) of the trajectory
+// Vmax, Amax, Dmax and jmax  Kinematic bounds
+// Ar, Dr and Vr              Reached values of acceleration and velocity
+
+TrapezoidalTrajectory::TrapezoidalTrajectory(Config_t& config) : config_(config) {}
+
+bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
+                                            float Vmax, float Amax, float Dmax) {
+    float dX = Xf - Xi;  // Distance to travel
+    float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance
+    float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement
+    float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)
+    Ar_ = s * Amax;  // Maximum Acceleration (signed)
+    Dr_ = -s * Dmax; // Maximum Deceleration (signed)
+    Vr_ = s * Vmax;  // Maximum Velocity (signed)
+
+    // If we start with a speed faster than cruising, then we need to decel instead of accel
+    // aka "double deceleration move" in the paper
+    if ((s * Vi) > (s * Vr_)) {
+        Ar_ = -s * Amax;
+    }
+
+    // Time to accel/decel to/from Vr (cruise speed)
+    Ta_ = (Vr_ - Vi) / Ar_;
+    Td_ = -Vr_ / Dr_;
+
+    // Integral of velocity ramps over the full accel and decel times to get
+    // minimum displacement required to reach cuising speed
+    float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;
+
+    // Are we displacing enough to reach cruising speed?
+    if (s*dX < s*dXmin) {
+        // Short move (triangle profile)
+        Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
+        Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
+        Td_ = std::max(0.0f, -Vr_ / Dr_);
+        Tv_ = 0.0f;
+    } else {
+        // Long move (trapezoidal profile)
+        Tv_ = (dX - dXmin) / Vr_;
+    }
+
+    // Fill in the rest of the values used at evaluation-time
+    Tf_ = Ta_ + Tv_ + Td_;
+    Xi_ = Xi;
+    Xf_ = Xf;
+    Vi_ = Vi;
+    yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase
+
+    return true;
+}
+
+TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) {
+    Step_t trajStep;
+    if (t < 0.0f) {  // Initial Condition
+        trajStep.Y   = Xi_;
+        trajStep.Yd  = Vi_;
+        trajStep.Ydd = 0.0f;
+    } else if (t < Ta_) {  // Accelerating
+        trajStep.Y   = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);
+        trajStep.Yd  = Vi_ + Ar_*t;
+        trajStep.Ydd = Ar_;
+    } else if (t < Ta_ + Tv_) {  // Coasting
+        trajStep.Y   = yAccel_ + Vr_*(t - Ta_);
+        trajStep.Yd  = Vr_;
+        trajStep.Ydd = 0.0f;
+    } else if (t < Tf_) {  // Deceleration
+        float td     = t - Tf_;
+        trajStep.Y   = Xf_ + 0.5f*Dr_*SQ(td);
+        trajStep.Yd  = Dr_*td;
+        trajStep.Ydd = Dr_;
+    } else if (t >= Tf_) {  // Final Condition
+        trajStep.Y   = Xf_;
+        trajStep.Yd  = 0.0f;
+        trajStep.Ydd = 0.0f;
+    } else {
+        // TODO: report error here
+    }
+
+    return trajStep;
+}

Firmware/MotorControl/trapTraj.hpp

@@ -0,0 +1,53 @@
+#ifndef _TRAP_TRAJ_H
+#define _TRAP_TRAJ_H
+
+class TrapezoidalTrajectory {
+public:
+    struct Config_t {
+        float vel_limit = 20000.0f;  // [count/s]
+        float accel_limit = 5000.0f; // [count/s^2]
+        float decel_limit = 5000.0f; // [count/s^2]
+        float A_per_css = 0.0f;      // [A/(count/s^2)]
+    };
+    struct Step_t {
+        float Y;
+        float Yd;
+        float Ydd;
+    };
+
+    TrapezoidalTrajectory(Config_t& config);
+    bool planTrapezoidal(float Xf, float Xi, float Vi,
+                         float Vmax, float Amax, float Dmax);
+    Step_t eval(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),
+                make_protocol_property("A_per_css", &config_.A_per_css)
+            )
+        );
+    }
+
+    Axis* axis_ = nullptr;  // set by Axis constructor
+    Config_t& config_;
+
+    float Xi_;
+    float Xf_;
+    float Vi_;
+
+    float Ar_;
+    float Vr_;
+    float Dr_;
+
+    float Ta_;
+    float Tv_;
+    float Td_;
+    float Tf_;
+
+    float yAccel_;
+};
+
+#endif

Firmware/MotorControl/utils.h

@@ -63,6 +63,8 @@ extern "C" {
 #define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y))
 #define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y))
 
+#define SQ(x) ((x) * (x))
+
 static const float one_by_sqrt3 = 0.57735026919f;
 static const float two_by_sqrt3 = 1.15470053838f;
 static const float sqrt3_by_2 = 0.86602540378f;

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',

Firmware/communication/ascii_protocol.cpp

@@ -128,6 +128,18 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
             axes[motor_number]->controller_.set_current_setpoint(current_setpoint);
         }
 
+    } else if (cmd[0] == 't') { // trapezoidal trajectory
+        unsigned motor_number;
+        float goal_point;
+        int numscan = sscanf(cmd, "t %u %f", &motor_number, &goal_point);
+        if (numscan < 2) {
+            respond(response_channel, use_checksum, "invalid command format");
+        } else if (motor_number >= AXIS_COUNT) {
+            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
+        } else {
+            axes[motor_number]->controller_.move_to_pos(goal_point);
+        }
+
     } else if (cmd[0] == 'h') {  // Help
         respond(response_channel, use_checksum, "Please see documentation for more details");
         respond(response_channel, use_checksum, "");

docs/ascii-protocol.md

@@ -24,6 +24,18 @@ command *42 ; comment [new line character]
 
 ## Command Reference
 
+#### Motor trajectory command
+```
+t motor destination
+```
+* `t` for trajectory
+* `motor` is the motor number, `0` or `1`.
+* `destination` is the goal position, in encoder counts.
+
+Example: `t 0 -20000`
+
+For general moving around of the axis, this is the recommended command.
+
 #### Motor Position command
 ```
 p motor position velocity_ff current_ff

docs/getting-started.md

@@ -47,7 +47,6 @@ permalink: /
   </div></details>
 
 ## Wiring up the ODrive
-
 <div class="alert">
 Make sure you have a good mechanical connection between the encoder and the motor, slip can cause disastrous oscillations or runaway.
 </div>
@@ -65,11 +64,9 @@ Connect the encoder(s) to J4. The A,B phases are required, and the Z (index puls
 ![Image of ODrive all hooked up](https://docs.google.com/drawings/d/e/2PACX-1vTCD0P40Cd-wvD7Fl8UYEaxp3_UL81oI4qUVqrrCJPi6tkJeSs2rsffIXQRpdu6rNZs6-2mRKKYtILG/pub?w=1716&h=1281)
 
 ## Downloading and Installing Tools
-
 Most instructions in this guide refer to a utility called `odrivetool`, so you should install that first.
 
 ### Windows
-
 1. Install Python 3. We recommend the Anaconda distribution because it packs a lot of useful scientific tools, however you can also install the standalone python.
   * __Anaconda__: Download the installer from [here](https://www.anaconda.com/download/#windows). Execute the downloaded file and follow the instructions.
   * __Standalone Python__: Download the installer from [here](https://www.python.org/downloads/). Execute the downloaded file and follow the instructions.
@@ -121,7 +118,6 @@ Try step 5 again
 
 
 ### Linux
-
 1. [Install Python 3](https://www.python.org/downloads/).
 2. Install the ODrive tools by opening a terminal and typing `pip install odrive` <kbd>Enter</kbd>
 3. Set up USB permissions
@@ -133,10 +129,10 @@ Try step 5 again
 
 ## Firmware
 #### ODrive v3.5 and later
-Your board should come preflashed with firmware. If you run into problems, follow the instructions [here](odrivetool.md#device-firmware-update) on the DFU procedure before you continue.</div>
+Your board should come preflashed with firmware. If you run into problems, follow the instructions [here](odrivetool.md#device-firmware-update) on the DFU procedure before you continue.
 
 #### ODrive v3.4 and earlier
-Your board does **not** come preflashed with any firmware. Follow the instructions [here](odrivetool.md#device-firmware-update) on the STP Link procedure before you continue.</div>
+Your board does **not** come preflashed with any firmware. Follow the instructions [here](odrivetool.md#device-firmware-update) on the STP Link procedure before you continue.
 
 ## Start `odrivetool`
 To launch the main interactive ODrive tool, type `odrivetool` <kbd>Enter</kbd>. Connect your ODrive and wait for the tool to find it. Now you can, for instance type `odrv0.vbus_voltage` <kbd>Enter</kbd> to inpect the boards main supply voltage.
@@ -258,9 +254,7 @@ You can now control the current with `odrv0.axis0.controller.current_setpoint =
 *Note: There is no velocity limiting in current control mode. Make sure that you don't overrev the motor, or exceed the max speed for your encoder.*
 
 ## What's next?
-
 You can now:
-
 * See what other [commands and parameters](commands.md) are available, including setting tuning parameters for better performance.
 * Control the ODrive from your own program or hook it up to an existing system through one of it's [interfaces](interfaces.md).
 * See how you can improve the behavior during the startup procedure, like [bypassing encoder calibration](encoders.md#encoder-with-index-signal).

tools/motion_planning/PlanTrap.py

@@ -0,0 +1,225 @@
+# Copyright (c) 2018 Paul Guénette
+# Copyright (c) 2018 Oskar Weigl
+
+# 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.
+
+# This algorithm is based on:
+# FIR filter-based online jerk-constrained trajectory generation
+# https://www.researchgate.net/profile/Richard_Bearee/publication/304358769_FIR_filter-based_online_jerk-controlled_trajectory_generation/links/5770ccdd08ae10de639c0ff7/FIR-filter-based-online-jerk-controlled-trajectory-generation.pdf
+
+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
+# Xi and Vi                  Adapted initial conditions for the AL profile
+# Xf                         Position set-point
+# s                          Direction (sign) of the trajectory
+# Vmax, Amax, Dmax and jmax  Kinematic bounds
+# Ar, Dr and Vr              Reached values of acceleration and velocity
+
+# Test scales:
+pos_range  = 10000.0
+Vmax_range = 8000.0
+Amax_range = 10000.0
+plot_range = 10000.0
+
+
+def PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax):
+    dX = Xf - Xi    # Distance to travel
+    stop_dist = Vi**2 / (2*Dmax)  # Minimum stopping distance
+    dXstop = np.sign(Vi)*stop_dist # Minimum stopping displacement
+    s = np.sign(dX - dXstop)   # Sign of coast velocity (if any)
+    Ar = s*Amax     # Maximum Acceleration (signed)
+    Dr = -s*Dmax    # Maximum Deceleration (signed)
+    Vr = s*Vmax     # Maximum Velocity (signed)
+
+    # If we start with a speed faster than cruising, then we need to decel instead of accel
+    # aka "double deceleration move" in the paper
+    if s*Vi > s*Vr:
+        print("Handbrake!")
+        Ar = -s*Amax
+
+    # Time to accel/decel to/from Vr (cruise speed)
+    Ta = (Vr-Vi)/Ar
+    Td = -Vr/Dr
+
+    # Integral of velocity ramps over the full accel and decel times to get
+    # minimum displacement required to reach cuising speed
+    dXmin = Ta*(Vr+Vi)/2.0 + Td*(Vr)/2.0
+
+    # Are we displacing enough to reach cruising speed?
+    if s*dX < s*dXmin:
+        print("Short Move:")
+        # From paper:
+        # Vr = s*math.sqrt((-(Vi**2/Ar)-2*dX)/(1/Dr-1/Ar))
+        # Simplified for less divisions:
+        Vr = s*math.sqrt((Dr*Vi**2 + 2*Ar*Dr*dX) / (Dr-Ar))
+        Ta = max(0, (Vr - Vi)/Ar)
+        Td = max(0, -Vr/Dr)
+        Tv = 0
+    else:
+        print("Long move:")
+        Tv = (dX - dXmin)/Vr # Coasting time
+
+    Tf = Ta+Tv+Td
+
+    print("Xi: {:.2f}\tXf: {:.2f}\tVi: {:.2f}".format(Xi, Xf, Vi))
+    print("Amax: {:.2f}\tVmax: {:.2f}\tDmax: {:.2f}".format(Amax, Vmax, Dmax))
+    print("dX: {:.2f}\tdXst: {:.2f}\tdXmin: {:.2f}".format(dX, dXstop, dXmin))
+    print("Ar: {:.2f}\tVr: {:.2f}\tDr: {:.2f}".format(Ar, Vr, Dr))
+    print("Ta: {:.2f}\tTv: {:.2f}\tTd: {:.2f}".format(Ta, Tv, Td))
+
+    return (Ar, Vr, Dr, Ta, Tv, Td, Tf)
+
+def EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf):
+    # Create the time series and preallocate the position, velocity, and acceleration arrays
+    t_traj = np.arange(0, Tf+0.1, 1/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 = Xi + Vi*Ta + 0.5*Ar*Ta**2
+
+    for i in range(len(t_traj)):
+        t = t_traj[i]
+        if t < 0: # Initial conditions
+            y[i]   = Xi
+            yd[i]  = Vi
+            ydd[i] = 0
+        elif t < Ta: # Acceleration
+            y[i]   = Xi + Vi*t + 0.5*Ar*t**2
+            yd[i]  = Vi + Ar*t
+            ydd[i] = Ar
+        elif t < Ta+Tv: # Coasting
+            y[i]   = y_Accel + Vr*(t-Ta)
+            yd[i]  = Vr
+            ydd[i] = 0
+        elif t < Tf: # Deceleration
+            td     = t-Tf
+            y[i]   = Xf + 0*td + 0.5*Dr*td**2
+            yd[i]  = 0 + Dr*td
+            ydd[i] = Dr
+        elif t >= Tf: # Final condition
+            y[i]   = Xf
+            yd[i]  = 0
+            ydd[i] = 0
+        else:
+            raise ValueError("t = {} is outside of considered range".format(t))
+        
+    dy = np.diff(y)
+    dy_max = np.max(np.abs(dy))
+    dyd = np.diff(yd)
+    dyd_max = np.max(np.abs(dyd))
+    print("dy_max: {:.2f}\tdyd_max: {:.2f}".format(dy_max, dyd_max))
+
+    error = False
+    if dy_max/pos_range > 0.001:
+        print("---------- Bad Pos Continuity --------------------")
+        error = True
+    if dyd_max/Vmax_range > 0.001:
+        print("---------- Bad Vel Continuity --------------------")
+        error = True
+    if abs(Xi-y[0]) > 0.0001:
+        print("---------- Bad Initial Position --------------------")
+        error = True
+    if abs(Xf-y[-1]) > 0.0001:
+        print("---------- Bad Final Position --------------------")
+        error = True
+    if abs(Vi-yd[0]) > 0.0001:
+        print("---------- Bad Initial Velocity --------------------")
+        error = True
+    if abs(yd[-1]) > 0.0001:
+        print("---------- Bad Final Velocity --------------------")
+        error = True
+
+    if error:
+        import ipdb; ipdb.set_trace()
+
+    return (y, yd, ydd, t_traj)
+
+def graphical_test():
+    numRows = 3
+    numCols = 5
+    fig, axes = plt.subplots(numRows, numCols)
+    random.seed(3) # Repeatable tests by using specific seed
+    for x in range(numRows*numCols):
+        rownow = int(x/numCols)
+        colnow = x % numCols
+        print("row: {}, col: {}".format(rownow, colnow))
+
+        Vmax = random.uniform(0.1*Vmax_range, Vmax_range)
+        Amax = random.uniform(0.1*Amax_range, Amax_range)
+        Dmax = Amax
+        Xf = random.uniform(-pos_range, pos_range)
+        Xi = random.uniform(-pos_range, pos_range)
+        if random.random() <= 0.5:
+            Vi = random.uniform(-Vmax*1.5, Vmax*1.5)
+        else:
+            Vi = 0
+
+        (Ar, Vr, Dr, Ta, Tv, Td, Tf) = PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax)
+        (Y, Yd, Ydd, t) = EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf)
+
+        # Plotting
+        ax1 = axes[rownow, colnow]
+        # Vel limits (draw first for clearer z-order)
+        ax1.plot([t[0], t[-1]], [Vmax, Vmax], 'g--')
+        ax1.plot([t[0], t[-1]], [-Vmax, -Vmax], 'g--')
+
+        ax1.plot(t, Y) # Pos
+        ax1.plot(t, Yd) # Vel
+        ax1.plot(0, Xi, 'bo') # Pos Initial
+        ax1.plot(0, Vi, 'ro') # Vel Initial
+        ## TODO: pull out Ta+Td+Td from planner for correct plot points
+        ax1.plot(t[-1]-0.1, Xf, 'b*') # Pos Final
+        ax1.plot(t[-1]-0.1, 0, 'r*') # Vel Final
+
+        ax1.set_ylim(-plot_range, plot_range)
+
+        print()
+
+    plt.show()
+
+def large_test():
+    random.seed(1) # Repeatable tests by using specific seed
+    for x in range(100):
+        print("Test {}".format(x))
+        Vmax = random.uniform(0.1*Vmax_range, Vmax_range)
+        Amax = random.uniform(0.1*Amax_range, Amax_range)
+        Dmax = Amax
+        Xf = random.uniform(-pos_range, pos_range)
+        Xi = random.uniform(-pos_range, pos_range)
+        if random.random() <= 0.5:
+            Vi = random.uniform(-Vmax*1.5, Vmax*1.5)
+        else:
+            Vi = 0
+
+        (Ar, Vr, Dr, Ta, Tv, Td, Tf) = PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax)
+        (Y, Yd, Ydd, t) = EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf)
+
+        print()
+
+if __name__ == '__main__':
+    large_test()
+    graphical_test()