122 lines
4.5 KiB
C++
122 lines
4.5 KiB
C++
// Motion planning: jerk-limited body velocities, a speed-dependent steering
|
|
// curve, and closed-loop distance / rotation moves driven by wheel odometry.
|
|
#pragma once
|
|
|
|
#include <Arduino.h>
|
|
#include "robot_config.h"
|
|
|
|
// Wheel travel accumulated from the firmware's hall tick counters.
|
|
class Odometry {
|
|
public:
|
|
void reset(); // zero the accumulators
|
|
void update(int16_t rawLeft, int16_t rawRight);
|
|
|
|
double left() const { return left_m_; }
|
|
double right() const { return right_m_; }
|
|
double distance() const { return 0.5 * (left_m_ + right_m_); } // metres
|
|
double heading() const { return (right_m_ - left_m_) / TRACK_WIDTH_M; } // radians, CCW+
|
|
|
|
private:
|
|
bool started_ = false;
|
|
int16_t prevL_ = 0, prevR_ = 0;
|
|
double left_m_ = 0, right_m_ = 0;
|
|
};
|
|
|
|
// One axis of a jerk-limited profile: acceleration itself is rate-limited, so
|
|
// the command has no step in its second derivative and the load never lurches.
|
|
class JerkAxis {
|
|
public:
|
|
JerkAxis(float vmax, float amax, float jmax) : vmax_(vmax), amax_(amax), jmax_(jmax) {}
|
|
|
|
void update(float target, float dt);
|
|
void hardZero() { v_ = 0; a_ = 0; }
|
|
void setVMax(float vmax) { vmax_ = vmax; }
|
|
void setLimits(float amax, float jmax, float amaxDec, float jmaxDec) {
|
|
amax_ = amax; jmax_ = jmax; amaxDec_ = amaxDec; jmaxDec_ = jmaxDec;
|
|
}
|
|
float velocity() const { return v_; }
|
|
float accel() const { return a_; }
|
|
|
|
private:
|
|
float v_ = 0, a_ = 0;
|
|
float vmax_, amax_, jmax_;
|
|
float amaxDec_ = 0, jmaxDec_ = 0; // 0 until setLimits: fall back to accel
|
|
};
|
|
|
|
class Motion {
|
|
public:
|
|
enum class Mode { Idle, Drive, Velocity, MoveLinear, MoveAngular, EStop };
|
|
|
|
// Pushes the default limits into both axes. Without this the axes start with
|
|
// no braking limits set and fall back to the acceleration ones, while the
|
|
// move planner already assumes the boost — it then brakes too late.
|
|
Motion();
|
|
|
|
// Operator-style input, -1..1 each. `steer` is scaled by the speed-dependent
|
|
// curve, so the same stick deflection spins on the spot at a standstill and
|
|
// only leans the robot at speed.
|
|
void setDrive(float throttle, float steer);
|
|
|
|
// Programmatic input in physical units, no steering curve applied.
|
|
void setVelocity(float mps, float radps);
|
|
|
|
// Closed-loop moves. Both return to Idle on arrival; busy() reports progress.
|
|
void moveDistance(float metres, const Odometry& odo);
|
|
void rotate(float radians, const Odometry& odo);
|
|
|
|
// 0 = soft (long hauls), 1 = sharp (indoors). Scales acceleration and jerk
|
|
// on both axes; top speed is a separate setting.
|
|
void setResponse(float r);
|
|
float response() const { return response_; }
|
|
|
|
// How much firmer braking is than acceleration. 1.0 = symmetric.
|
|
void setDecelBoost(float x);
|
|
float decelBoost() const { return decelBoost_; }
|
|
|
|
void stop(); // ramp to zero through the jerk limits
|
|
void emergencyStop(); // zero the command this instant, then latch
|
|
void clearEStop();
|
|
|
|
void update(const Odometry& odo, float dt);
|
|
|
|
// True once per abandoned move, so the caller can report it.
|
|
bool takeAbort() { const bool a = aborted_; aborted_ = false; return a; }
|
|
|
|
int16_t cmdLeft() const { return cmdL_; }
|
|
int16_t cmdRight() const { return cmdR_; }
|
|
|
|
Mode mode() const { return mode_; }
|
|
const char* modeName() const;
|
|
bool busy() const { return mode_ == Mode::MoveLinear || mode_ == Mode::MoveAngular; }
|
|
float linear() const { return lin_.velocity(); }
|
|
float angular() const { return ang_.velocity(); }
|
|
float steerGain() const;
|
|
double remaining() const { return remaining_; }
|
|
|
|
private:
|
|
JerkAxis lin_{V_MAX, A_MAX_MPS2, J_MAX_MPS3};
|
|
JerkAxis ang_{W_MAX_RADS, ALPHA_MAX, J_MAX_ALPHA};
|
|
|
|
Mode mode_ = Mode::Idle;
|
|
float reqThrottle_ = 0, reqSteer_ = 0; // Drive mode, normalised
|
|
float reqV_ = 0, reqW_ = 0; // Velocity mode, physical units
|
|
double moveFrom_ = 0, moveTo_ = 0; // metres or radians
|
|
double holdRef_ = 0; // heading (linear move) or distance (rotation)
|
|
double remaining_ = 0;
|
|
double lastRemaining_ = 0; // for stall detection
|
|
float stallTimer_ = 0;
|
|
bool aborted_ = false;
|
|
int moveDir_ = 1; // sign of the move as commanded
|
|
|
|
float response_ = RESPONSE_DEFAULT;
|
|
float decelBoost_ = DECEL_BOOST_DEFAULT;
|
|
float aMax_ = A_MAX_MPS2, jMax_ = J_MAX_MPS3;
|
|
float alphaMax_ = ALPHA_MAX, jAlpha_ = J_MAX_ALPHA;
|
|
|
|
int16_t cmdL_ = 0, cmdR_ = 0;
|
|
|
|
void mixToWheels();
|
|
bool stalled(float dt);
|
|
bool overshot() const;
|
|
};
|