*
This commit is contained in:
@@ -0,0 +1,90 @@
|
||||
|
||||
#include <doctest.h>
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
|
||||
#include "communication/can_helpers.hpp"
|
||||
|
||||
enum InputMode {
|
||||
INPUT_MODE_INACTIVE,
|
||||
INPUT_MODE_PASSTHROUGH,
|
||||
INPUT_MODE_VEL_RAMP,
|
||||
INPUT_MODE_POS_FILTER,
|
||||
INPUT_MODE_MIX_CHANNELS,
|
||||
INPUT_MODE_TRAP_TRAJ,
|
||||
};
|
||||
|
||||
TEST_SUITE("CAN Functions") {
|
||||
TEST_CASE("reverse") {
|
||||
can_Message_t rxmsg;
|
||||
rxmsg.id = 0x000;
|
||||
rxmsg.isExt = false;
|
||||
rxmsg.len = 8;
|
||||
|
||||
rxmsg.buf[0] = 0x12;
|
||||
rxmsg.buf[1] = 0x34;
|
||||
|
||||
std::reverse(std::begin(rxmsg.buf), std::end(rxmsg.buf));
|
||||
CHECK(rxmsg.buf[0] == 0x00);
|
||||
CHECK(rxmsg.buf[6] == 0x34);
|
||||
CHECK(rxmsg.buf[7] == 0x12);
|
||||
}
|
||||
|
||||
TEST_CASE("getSignal") {
|
||||
can_Message_t rxmsg;
|
||||
|
||||
auto val = 0x1234;
|
||||
std::memcpy(rxmsg.buf, &val, sizeof(val));
|
||||
|
||||
val = can_getSignal<uint16_t>(rxmsg, 0, 16, true, 1, 0);
|
||||
CHECK(val == 0x1234);
|
||||
|
||||
val = can_getSignal<uint16_t>(rxmsg, 0, 16, false, 1, 0);
|
||||
CHECK(val == 0x3412);
|
||||
|
||||
float myFloat = 1234.6789f;
|
||||
std::memcpy(rxmsg.buf, &myFloat, sizeof(myFloat));
|
||||
auto floatVal = can_getSignal<float>(rxmsg, 0, 32, true, 1, 0);
|
||||
CHECK(floatVal == 1234.6789f);
|
||||
|
||||
can_Message_t msg;
|
||||
msg.id = 0x00E;
|
||||
msg.buf[0] = 0x96;
|
||||
msg.buf[1] = 0x00;
|
||||
msg.buf[2] = 0x00;
|
||||
msg.buf[3] = 0x00;
|
||||
CHECK(can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0.0f) == 1.50f);
|
||||
}
|
||||
|
||||
TEST_CASE("setSignal") {
|
||||
can_Message_t txmsg;
|
||||
|
||||
can_setSignal<uint16_t>(txmsg, 0x1234, 0, 16, true, 1.0f, 0.0f);
|
||||
CHECK(can_getSignal<uint16_t>(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234);
|
||||
|
||||
can_setSignal<uint16_t>(txmsg, 0xABCD, 16, 16, true, 1.0f, 0.0f);
|
||||
CHECK(can_getSignal<uint16_t>(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234);
|
||||
CHECK(can_getSignal<uint16_t>(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD);
|
||||
|
||||
can_setSignal<float>(txmsg, 1234.5678f, 32, 32, true, 1.0f, 0.0f);
|
||||
CHECK(can_getSignal<uint16_t>(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234);
|
||||
CHECK(can_getSignal<uint16_t>(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD);
|
||||
CHECK(can_getSignal<float>(txmsg, 32, 32, true, 1.0f, 0.0f));
|
||||
|
||||
can_setSignal<uint16_t>(txmsg, 0x1234, 0, 16, false, 1.0f, 0.0f);
|
||||
CHECK(can_getSignal<uint16_t>(txmsg, 0, 16, false, 1.0f, 0.0f) == 0x1234);
|
||||
CHECK(can_getSignal<uint16_t>(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD);
|
||||
CHECK(can_getSignal<float>(txmsg, 32, 32, true, 1.0f, 0.0f));
|
||||
|
||||
can_setSignal<float>(txmsg, 234981.0f, 12, 32, false, 2.0f, 1.1f);
|
||||
CHECK(can_getSignal<float>(txmsg, 12, 32, false, 2.0f, 1.1f) == 234981.0f);
|
||||
}
|
||||
|
||||
TEST_CASE("getSignal enums") {
|
||||
can_Message_t rxmsg;
|
||||
rxmsg.buf[0] = INPUT_MODE_MIX_CHANNELS;
|
||||
rxmsg.buf[1] = INPUT_MODE_PASSTHROUGH;
|
||||
CHECK(static_cast<InputMode>(can_getSignal<InputMode>(rxmsg, 0, 8, true, 1, 0)) == INPUT_MODE_MIX_CHANNELS);
|
||||
CHECK(static_cast<InputMode>(can_getSignal<InputMode>(rxmsg, 8, 8, true, 1, 0)) == INPUT_MODE_PASSTHROUGH);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
#include <doctest.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <iostream>
|
||||
|
||||
TEST_CASE("Rotate Axis State"){
|
||||
std::array<int, 10> testArr = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
const int& currentVal = testArr.front();
|
||||
CHECK(currentVal == testArr[0]);
|
||||
|
||||
std::rotate(testArr.begin(), testArr.begin() + 1, testArr.end());
|
||||
CHECK(currentVal == testArr[0]);
|
||||
CHECK(currentVal == 1);
|
||||
|
||||
CHECK(testArr.back() == 0);
|
||||
|
||||
std::rotate(testArr.begin(), testArr.begin() + 1, testArr.end());
|
||||
CHECK(currentVal == testArr[0]);
|
||||
CHECK(currentVal == 2);
|
||||
CHECK(testArr.back() == 1);
|
||||
|
||||
|
||||
}
|
||||
|
||||
TEST_CASE("Fill Test"){
|
||||
std::array<int, 10> testArr = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
std::fill(testArr.begin(), testArr.end(), 6);
|
||||
for(const auto& val : testArr){
|
||||
CHECK(val == 6);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,157 @@
|
||||
|
||||
|
||||
#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN
|
||||
#define DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING
|
||||
#define DOCTEST_CONFIG_USE_STD_HEADERS
|
||||
#define DOCTEST_CONFIG_NO_TRY_CATCH_IN_ASSERTS
|
||||
#define DOCTEST_CONFIG_NO_EXCEPTIONS
|
||||
#define DOCTEST_CONFIG_NO_WINDOWS_SEH
|
||||
#define DOCTEST_CONFIG_NO_POSIX_SIGNALS
|
||||
// #define DOCTEST_CONFIG_VOID_CAST_EXPRESSIONS
|
||||
|
||||
#include <doctest.h>
|
||||
|
||||
#include <communication/can_helpers.hpp>
|
||||
|
||||
using std::cout;
|
||||
using std::endl;
|
||||
|
||||
TEST_SUITE("delta_enc") {
|
||||
// Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271
|
||||
int mod(int dividend, int divisor) {
|
||||
int r = dividend % divisor;
|
||||
return (r < 0) ? (r + divisor) : r;
|
||||
}
|
||||
|
||||
int getDelta(int pos_abs, int count_in_cpr, int cpr) {
|
||||
int delta_enc = pos_abs - count_in_cpr;
|
||||
delta_enc = mod(delta_enc, cpr);
|
||||
if (delta_enc > (cpr / 2))
|
||||
delta_enc -= cpr;
|
||||
return delta_enc;
|
||||
}
|
||||
|
||||
TEST_CASE("mod") {
|
||||
int cpr = 1000;
|
||||
|
||||
// Check moves around 0
|
||||
CHECK(getDelta(1, 0, cpr) == 1);
|
||||
CHECK(getDelta(0, 1, cpr) == -1);
|
||||
CHECK(getDelta(999, 0, cpr) == -1);
|
||||
CHECK(getDelta(50, 650, cpr) == 400);
|
||||
CHECK(getDelta(650, 50, cpr) == -400);
|
||||
CHECK(getDelta(50, 500, cpr) == -450);
|
||||
CHECK(getDelta(500, 50, cpr) == 450);
|
||||
|
||||
// Test moving a distance larger than cpr / 2
|
||||
CHECK(getDelta(950, 450, cpr) == 500);
|
||||
CHECK(getDelta(451, 950, cpr) == -499);
|
||||
CHECK(getDelta(450, 950, cpr) == 500);
|
||||
|
||||
// Test handling around mid-point
|
||||
CHECK(getDelta(501, 499, cpr) == 2);
|
||||
CHECK(getDelta(499, 501, cpr) == -2);
|
||||
CHECK(getDelta(550, 450, cpr) == 100);
|
||||
CHECK(getDelta(450, 550, cpr) == -100);
|
||||
}
|
||||
}
|
||||
|
||||
TEST_SUITE("velLimiter") {
|
||||
// Velocity limiting in current mode
|
||||
#include <algorithm>
|
||||
using doctest::Approx;
|
||||
|
||||
auto limitVel(float vel_limit, float vel_estimate, float vel_gain, float Iq) {
|
||||
float Imax = (vel_limit - vel_estimate) * vel_gain;
|
||||
float Imin = (-vel_limit - vel_estimate) * vel_gain;
|
||||
return std::clamp(Iq, Imin, Imax);
|
||||
}
|
||||
|
||||
TEST_CASE("limit Vel") {
|
||||
CHECK(limitVel(0, 0, 0, 0) == 0.0f);
|
||||
CHECK(limitVel(1000.0f, 1.0f, 0.0f, 0.0f) == 0.0f);
|
||||
CHECK(limitVel(1000.0f, 500.0f, 1.0f, 1.0f) == 1.0f);
|
||||
CHECK(limitVel(1000.0f, 500.0f, 1.0f, -20.0f) == -20.0f);
|
||||
CHECK(limitVel(1000.0f, 999.0f, 1.0f, 2.0f) == 1.0f);
|
||||
CHECK(limitVel(1000.0f, 999.0f, 1.0f, -5.0f) == -5.0f);
|
||||
CHECK(limitVel(1000.0f, -999.0f, 1.0f, -5.0f) == -1.0f);
|
||||
CHECK(limitVel(1000.0f, -999.0f, 1.0f, 5.0f) == 5.0f);
|
||||
CHECK(limitVel(1000.0f, 0.0f, 1.0f, 1.0f) == 1.0f);
|
||||
CHECK(limitVel(1000.0f, 0.0f, 1.0f, -1.0f) == -1.0f);
|
||||
}
|
||||
|
||||
TEST_CASE("Accelerating") {
|
||||
CHECK(limitVel(200000.0f, 195000.0f, 5.0E-4f, 30.0f) == 2.5f);
|
||||
CHECK(limitVel(200000.0f, 205000.0f, 5.0E-4f, 30.0f) == -2.5f);
|
||||
CHECK(limitVel(200000.0f, -195000.0f, 5.0E-4, -30.0f) == -2.5f);
|
||||
CHECK(limitVel(200000.0f, -205000.0f, 5.0E-4f, -30.0f) == 2.5f);
|
||||
}
|
||||
|
||||
TEST_CASE("Decelerating") {
|
||||
CHECK(limitVel(200000.0f, 195000.0f, 5.0E-4f, -30.0f) == -30.0f);
|
||||
CHECK(limitVel(200000.0f, 205000.0f, 5.0E-4f, -30.0f) == -30.0f);
|
||||
CHECK(limitVel(200000.0f, -195000.0f, 5.0E-4, 30.0f) == 30.0f);
|
||||
CHECK(limitVel(200000.0f, -205000.0f, 5.0E-4f, 30.0f) == 30.0f);
|
||||
}
|
||||
|
||||
TEST_CASE("Over-Center") {
|
||||
CHECK(limitVel(20000.0f, 1000.0f, 5.0E-4f, 30.0f) == 9.5f);
|
||||
CHECK(limitVel(20000.0f, -1000.0f, 5.0E-4f, 30.0f) == Approx(10.5f));
|
||||
}
|
||||
}
|
||||
|
||||
TEST_SUITE("vel_ramp") {
|
||||
float vel_ramp_old(float input_vel_, float vel_setpoint_, float vel_ramp_rate) {
|
||||
float max_step_size = 0.000125f * vel_ramp_rate;
|
||||
float full_step = input_vel_ - vel_setpoint_;
|
||||
float step;
|
||||
if (std::abs(full_step) > max_step_size) {
|
||||
step = std::copysignf(max_step_size, full_step);
|
||||
} else {
|
||||
step = full_step;
|
||||
}
|
||||
return step;
|
||||
}
|
||||
|
||||
float vel_ramp_new(float input_vel_, float vel_setpoint_, float vel_ramp_rate) {
|
||||
float max_step_size = 0.000125f * vel_ramp_rate;
|
||||
float full_step = input_vel_ - vel_setpoint_;
|
||||
return std::clamp(full_step, -max_step_size, max_step_size);
|
||||
}
|
||||
|
||||
uint8_t parity(uint16_t v) {
|
||||
v ^= v >> 8;
|
||||
v ^= v >> 4;
|
||||
v ^= v >> 2;
|
||||
v ^= v >> 1;
|
||||
return v & 1;
|
||||
}
|
||||
|
||||
TEST_CASE("Equivalence") {
|
||||
float vel_setpoint = 0.0f;
|
||||
float vel_ramp_rate = 8000;
|
||||
float input_vel = 0.0f;
|
||||
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
|
||||
|
||||
input_vel = 10.0f;
|
||||
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
|
||||
|
||||
input_vel = 10000.0f;
|
||||
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
|
||||
|
||||
input_vel = -10000.0f;
|
||||
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
|
||||
|
||||
input_vel = -0.1234f;
|
||||
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
|
||||
|
||||
input_vel = 0.1234f;
|
||||
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
|
||||
}
|
||||
|
||||
TEST_CASE("Parity") {
|
||||
CHECK(parity(0x0DDF & 0x7FFF) == 0);
|
||||
CHECK(parity(0x8DDF & 0x7FFF) == 0);
|
||||
CHECK(parity(0x5BFF & 0x7FFF) == 1);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
#include <doctest.h>
|
||||
#include "MotorControl/timer.hpp"
|
||||
#include <stdint.h>
|
||||
|
||||
TEST_CASE_TEMPLATE("Timer2", T, float, int, char, uint32_t){
|
||||
Timer<T> myTimer;
|
||||
myTimer.setTimeout(10);
|
||||
myTimer.setIncrement(1);
|
||||
CHECK(!myTimer.expired());
|
||||
|
||||
myTimer.start();
|
||||
CHECK(!myTimer.expired());
|
||||
for(int i = 0; i < 9; ++i){
|
||||
myTimer.update();
|
||||
CHECK(!myTimer.expired());
|
||||
}
|
||||
|
||||
myTimer.update();
|
||||
CHECK(myTimer.expired());
|
||||
|
||||
myTimer.stop();
|
||||
CHECK(myTimer.expired());
|
||||
|
||||
myTimer.start();
|
||||
CHECK(myTimer.expired());
|
||||
|
||||
myTimer.reset();
|
||||
CHECK(!myTimer.expired());
|
||||
}
|
||||
@@ -0,0 +1,235 @@
|
||||
|
||||
#include <doctest.h>
|
||||
#include <limits.h>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <random>
|
||||
|
||||
#include "MotorControl/utils.hpp"
|
||||
|
||||
// TODO: This is currently a copy-paste of the real code due to non-trivial
|
||||
// include dependencies. Should include real code.
|
||||
|
||||
class TrapezoidalTrajectory {
|
||||
public:
|
||||
struct Step_t {
|
||||
float Y;
|
||||
float Yd;
|
||||
float Ydd;
|
||||
};
|
||||
|
||||
explicit TrapezoidalTrajectory();
|
||||
bool planTrapezoidal(float Xf, float Xi, float Vi,
|
||||
float Vmax, float Amax, float Dmax);
|
||||
Step_t eval(float t);
|
||||
|
||||
float Xi_;
|
||||
float Xf_;
|
||||
float Vi_;
|
||||
|
||||
float Ar_;
|
||||
float Vr_;
|
||||
float Dr_;
|
||||
|
||||
float Ta_;
|
||||
float Tv_;
|
||||
float Td_;
|
||||
float Tf_;
|
||||
|
||||
float yAccel_;
|
||||
|
||||
float t_;
|
||||
};
|
||||
|
||||
|
||||
|
||||
// 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() {}
|
||||
|
||||
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(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
|
||||
//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;
|
||||
}
|
||||
|
||||
static_assert(sizeof(float) * CHAR_BIT == 32);
|
||||
|
||||
|
||||
void run_trajectory_test(float goal, float position, float velocity, float Vmax, float Amax, float Dmax) {
|
||||
float dt = 0.000125f;
|
||||
int replan_interval = 10; // must be > 2 (see note below)
|
||||
float t = 0.0f;
|
||||
float Vmax_test = std::max(Vmax, std::abs(velocity));
|
||||
|
||||
TrapezoidalTrajectory traj{};
|
||||
|
||||
int replan_counter = 0;
|
||||
|
||||
do {
|
||||
if (replan_counter <= 0) {
|
||||
CHECK(traj.planTrapezoidal(goal, position, velocity, Vmax, Amax, Dmax));
|
||||
t = 0.0f;
|
||||
replan_counter = replan_interval;
|
||||
} else {
|
||||
replan_counter--;
|
||||
}
|
||||
|
||||
TrapezoidalTrajectory::Step_t step = traj.eval(t);
|
||||
t += dt;
|
||||
|
||||
//std::cerr << "vel: " << step.Yd << ", pos: " << step.Y << "\n";
|
||||
|
||||
// Check if acceleration within bounds
|
||||
if (velocity >= 0.0f) {
|
||||
CHECK(step.Ydd <= Amax);
|
||||
CHECK(step.Ydd >= -Dmax);
|
||||
CHECK((step.Yd - velocity) / dt <= Amax * 1.002f);
|
||||
CHECK((step.Yd - velocity) / dt >= -Dmax * 1.002f);
|
||||
} else {
|
||||
CHECK(step.Ydd <= Dmax);
|
||||
CHECK(step.Ydd >= -Amax);
|
||||
CHECK((step.Yd - velocity) / dt <= Dmax * 1.002f);
|
||||
CHECK((step.Yd - velocity) / dt >= -Amax * 1.002f);
|
||||
}
|
||||
|
||||
// Check if velocity within bounds
|
||||
CHECK(step.Yd >= -Vmax_test);
|
||||
CHECK(step.Yd <= Vmax_test);
|
||||
CHECK((step.Y - position) / dt >= -Vmax_test * 1.002f);
|
||||
CHECK((step.Y - position) / dt <= Vmax_test * 1.002f);
|
||||
velocity = step.Yd;
|
||||
|
||||
// Check if position is making progress
|
||||
// TODO: the trajectory planner currently needs three "warm-up" iterations
|
||||
// until its position makes progress. This should probably be revisited.
|
||||
// TODO: this is disabled currently because there are legitimate trajectories
|
||||
// where the position first moves in the wrong direction.
|
||||
//if ((replan_counter < replan_interval - 2) && (t <= traj.Tf_)) {
|
||||
// CHECK(std::abs(step.Y - goal) < std::abs(position - goal));
|
||||
//}
|
||||
position = step.Y;
|
||||
|
||||
} while (t <= traj.Tf_);
|
||||
|
||||
CHECK(position >= goal - 1.0f);
|
||||
CHECK(position <= goal + 1.0f);
|
||||
CHECK(velocity >= -Dmax * dt);
|
||||
CHECK(velocity <= Dmax * dt);
|
||||
}
|
||||
|
||||
|
||||
TEST_SUITE("Trajectory Planner") {
|
||||
// these form a triangle trajectory because 2*v^2/(2*a) = 2 * 27712^2 / (2*22288) = 34456 > 16384
|
||||
TEST_CASE("neg-dir-triangle") {
|
||||
run_trajectory_test(-8192.0f, 8192.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f);
|
||||
}
|
||||
TEST_CASE("pos-dir-triangle") {
|
||||
run_trajectory_test(8192.0f, -8192.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f);
|
||||
}
|
||||
|
||||
// these form a trapezoid trajectory because 2*v^2/(2*a) = 2 * 27712^2 / (2*22288) = 34456 < 16384
|
||||
TEST_CASE("neg-dir-trapezoid") {
|
||||
run_trajectory_test(-25000.0f, 25000.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f);
|
||||
}
|
||||
TEST_CASE("pos-dir-trapezoid") {
|
||||
run_trajectory_test(25000.0f, -25000.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f);
|
||||
}
|
||||
|
||||
// for the following tests note that v^2/(2*a) = 27712^2 / (2*22288) = 17227 > 16384
|
||||
TEST_CASE("neg-dir-not-enough-braking-distance") {
|
||||
run_trajectory_test(-8192.0f, 8192.0f, -27712.0f, 27712.0f, 22288.0f, 22288.0f);
|
||||
}
|
||||
TEST_CASE("pos-dir-not-enough-braking-distance") {
|
||||
run_trajectory_test(8192.0f, -8192.0f, 27712.0f, 27712.0f, 22288.0f, 22288.0f);
|
||||
}
|
||||
|
||||
TEST_CASE("neg-dir-over-speed") {
|
||||
run_trajectory_test(-8192.0f, 8192.0f, -40000.0f, 27712.0f, 22288.0f, 22288.0f);
|
||||
}
|
||||
TEST_CASE("pos-dir-over-speed") {
|
||||
run_trajectory_test(8192.0f, -8192.0f, 40000.0f, 27712.0f, 22288.0f, 22288.0f);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user