From ea191451a554d900ed688fcede6c471625007681 Mon Sep 17 00:00:00 2001 From: Pascal Meyer Date: Wed, 30 Apr 2025 13:41:07 +0200 Subject: [PATCH 1/7] added optimization for a sinusoidal wave --- schnell.cpp | 1041 ++++++++++++++++++++++++++++++++++++++++++++------- viswrap.py | 229 +++++++++-- 2 files changed, 1113 insertions(+), 157 deletions(-) diff --git a/schnell.cpp b/schnell.cpp index bf77a81..cd0ef0b 100644 --- a/schnell.cpp +++ b/schnell.cpp @@ -5,7 +5,7 @@ #if __cplusplus >= 202302L #include #else -#include +#include #include #endif @@ -62,24 +62,63 @@ struct std::formatter>: std::formatter { ); } }; -using std::println; +using std::print; using std::format; #else -template -struct fmt::formatter>: fmt::formatter { - auto format(const Eigen::Vector& v, fmt::format_context& ctx) const { + +// formater for jet data type +template +struct fmt::formatter> : fmt::formatter { + auto format(const ceres::Jet& jet, fmt::format_context& ctx) const { + std::string v_str = "["; + for (int i = 0; i < N; ++i) { + v_str += fmt::format("{}", jet.v[i]); + if (i < N - 1) v_str += ", "; + } + v_str += "]"; + return fmt::formatter::format(fmt::format("Jet(a: {}, v: {})", jet.a, v_str), ctx); + } +}; + +// // formater for Eigen::vector data type +// template +// struct fmt::formatter>: fmt::formatter { +// auto format(const Eigen::Vector& v, fmt::format_context& ctx) const { +// return fmt::formatter::format( +// std::accumulate( +// std::next(v.begin()), +// v.end(), +// fmt::format("[{}", v[0]), +// [](std::string a, const Type& x) { return fmt::format("{}, {}", std::move(a), x); } +// ) + ']', +// ctx +// ); +// } +// }; + +template +struct fmt::formatter> : fmt::formatter { + static_assert(Cols == 1, "This formatter only supports column vectors."); + + auto format(const Eigen::Matrix& v, fmt::format_context& ctx) const { + if (v.size() == 0) + return fmt::formatter::format("[]", ctx); + return fmt::formatter::format( std::accumulate( - std::next(v.begin()), - v.end(), + std::next(v.data()), // skip first + v.data() + v.size(), fmt::format("[{}", v[0]), - [](std::string a, const Type& x) { return fmt::format("{}, {}", std::move(a), x); } + [](std::string a, const Scalar& x) { + return fmt::format("{}, {}", std::move(a), x); + } ) + ']', ctx ); } }; -using fmt::println; + +using fmt::print; using fmt::format; #endif @@ -291,20 +330,50 @@ constexpr bool almost_zero(T x) { return ceres::abs(x) <= atol; } +// Helper +template +inline double get_value(const T& t) { + return t; +} + +template +inline T get_value(const ceres::Jet& jet) { + return jet.a; +} + template class Line { std::optional> _pluecker; public: Vector3 dir, pt; + Line(const Vector3& direction, const Vector3& point): dir(direction.normalized()), pt(point) {} + + Line(const Eigen::Matrix& line): + Line( + Vector3(line(0), line(1), line(2)), + Vector3(line(3), line(4), line(5)) + ) {} + + // Cast to another scalar type (e.g., double <-> Jet) + template + Line cast() const { + return Line( + dir.template cast(), + pt.template cast() + ); + } + Vector3 distance_to(const Vector3& other_pt) const { T proj_len = (other_pt - pt).dot(dir); Vector3 closest_pt = pt + proj_len * dir; + return other_pt - closest_pt; } + const Eigen::Matrix4 pluecker() { if (!_pluecker.has_value()) { Vector4 a = (pt + dir).homogeneous(), b = pt.homogeneous(); @@ -314,8 +383,269 @@ class Line { return _pluecker.value(); } + + Eigen::Matrix getLine() const { + return { dir.x(), dir.y(), dir.z(), pt.x(), pt.y(), pt.z()}; + } + + Eigen::Matrix getLineJet() const { + Eigen::Matrix out; + out << + get_value(dir.x()), get_value(dir.y()), get_value(dir.z()), + get_value(pt.x()), get_value(pt.y()), get_value(pt.z()); + return out; + } + +}; + +template +struct SinusoidalWaveSurface { + + // p(x,y) = origin ​+ u⋅x + v⋅y + A⋅sin(kx + ly + phase) ⋅ n + Vector3 origin; // Reference point on the surface // mean of pca for first guess + Vector3 u, v; // Tangent vectors defining the local x, y direction + Vector3 normal; // Direction of wave displacement + T amplitude; // A + T frequency; // k, affects wavelength + T phase; // φ + + // init as zero matrix if no variable given + SinusoidalWaveSurface(): origin(Vector3::Zero()), u(Vector3::Zero()), v(Vector3::Zero()), + normal(Vector3::Zero()), amplitude(T(0)), frequency(T(0)), phase(T(0)) {} + + // Constructor for given values + SinusoidalWaveSurface(const Vector3& origin, + const Vector3& u, + const Vector3& v, + const Vector3& normal, + T amplitude, + T frequency, + T phase = T(0)) + : origin(origin), + u(u.normalized()), + v(v.normalized()), + normal(normal.normalized()), + amplitude(amplitude), + frequency(frequency), + phase(phase) {} + + // Constructor for direction of wave + normal + sin wave params + SinusoidalWaveSurface(const Vector3& origin, + const Vector3& u, + const Vector3& normal, + T amplitude, + T frequency, + T phase = T(0)) + : origin(origin), + u(u.normalized()), v(u.cross(normal).normalized()), + normal(normal.normalized()), + amplitude(amplitude), + frequency(frequency), + phase(phase) {} + + SinusoidalWaveSurface(const Eigen::Matrix& sin_params): + SinusoidalWaveSurface( + Vector3(sin_params(0), sin_params(1), sin_params(2)), + Vector3(sin_params(3), sin_params(4), sin_params(5)), + Vector3(sin_params(6), sin_params(7), sin_params(8)), + Vector3(sin_params(9), sin_params(10), sin_params(11)), + sin_params(12), + sin_params(13), + sin_params(14) + ) {} + + SinusoidalWaveSurface(T ox, T oy, T oz, T ux, T uy, T uz, T nx, T ny, T nz, T amplitude, T frequency, T phase): + SinusoidalWaveSurface( + Vector3(ox, oy, oz), + Vector3(ux, uy, uz), + Vector3(nx, ny, nz), + amplitude, + frequency, + phase + ) {} + + SinusoidalWaveSurface(const std::vector& sin_params): + SinusoidalWaveSurface( + Vector3(sin_params.at(0), sin_params.at(1), sin_params.at(2)), + Vector3(sin_params.at(3), sin_params.at(4), sin_params.at(5)), + Vector3(sin_params.at(6), sin_params.at(7), sin_params.at(8)), + sin_params.at(9), + sin_params.at(10), + sin_params.at(11) + ) {} + + SinusoidalWaveSurface(const T* sin_params): + SinusoidalWaveSurface( + Vector3(sin_params[0], sin_params[1], sin_params[2]), + Vector3(sin_params[3], sin_params[4], sin_params[5]), + Vector3(sin_params[6], sin_params[7], sin_params[8]), + sin_params[9], + sin_params[10], + sin_params[11] + ) {} + + template + SinusoidalWaveSurface cast() const { + return SinusoidalWaveSurface( + origin.template cast(), + u.template cast(), + v.template cast(), + normal.template cast(), + static_cast(amplitude), + static_cast(frequency), + static_cast(phase) + ); + } + + Eigen::Matrix wave_param() const { + return Eigen::Matrix{ + origin.x(), origin.y(), origin.z(), + u.x(), u.y(), u.z(), + v.x(), v.y(), v.z(), + normal.x(), normal.y(), normal.z(), + amplitude, frequency, phase + }; + } + + Eigen::Matrix jet_param() const { + Eigen::Matrix out; + out << + get_value(origin.x()), get_value(origin.y()), get_value(origin.z()), + get_value(u.x()), get_value(u.y()), get_value(u.z()), + get_value(v.x()), get_value(v.y()), get_value(v.z()), + get_value(normal.x()), get_value(normal.y()), get_value(normal.z()), + get_value(amplitude), get_value(frequency), get_value(phase); + return out; + } + + template + Vector3 evaluate(J x, J y) const { + + J wave = amplitude * ceres::sin(frequency * x + phase); + + return origin + x * u + y * v + wave * normal; + } + + // get normal at a point (approximation) + Vector3 normal_at(T x, T y, T dx = T(1e-4)) const { + Vector3 p = evaluate(x, y); + Vector3 px = evaluate(x + dx, y); + Vector3 py = evaluate(x, y + dx); + return (px - p).cross(py - p).normalized(); + } + + Vector3 refract(const Line& line, bool backwards = false) const { + double r = 1.33; // air -> water + Vector3 n = normal_at(line.pt.x(), line.pt.y()); // get aproximate norm on the sin surface + + if (backwards) { + n = n * -1.0; + } else + r = 1.0 / r; + + T c = -n.dot(line.dir); + return r * line.dir + n * (r * c - ceres::sqrt(1.0 - r * r * (1.0 - c * c))); + } +}; + +// Residual for the intersection of a line with a sinusoidal wave surface +template +struct IntersectionResidual { + Line line_d; + SinusoidalWaveSurface surface_d; + + IntersectionResidual(const Line& l, const SinusoidalWaveSurface& s) + :line_d(l), surface_d(s) {} + + template + bool operator()(const J* const xy, J* residuals) const { + J x = xy[0]; + J y = xy[1]; + + // Convert stored double-typed objects to Jet-typed + Line line = line_d.template cast(); + SinusoidalWaveSurface surface = surface_d.template cast(); + + // Calculate surface point (Evaluate the wave surface at (x, y)) + Vector3 p = surface.evaluate(x, y); + Vector3 o = line.pt; // Point on line + Vector3 d = line.dir; // Direction of line + + // Project p onto the line direction to find the closest point + J t = (p - o).dot(d); + Vector3 proj = o + t * d; + + // Calculate difference between point on the surface and the projection + Vector3 diff = p - proj; + + // Set residuals (difference in 3D space) + residuals[0] = diff.x(); + residuals[1] = diff.y(); + residuals[2] = diff.z(); + + return true; + } }; +/** + * @brief Computes the intersection point of a line with a sinusoidal wave surface. + * + * @param line The line object represented as a parameterized line in 3D space. + * @param surface The sinusoidal wave surface object to intersect with. + * @param isec Output parameter to store the computed intersection point in 3D space. + * @return true If the intersection is successfully computed and within a reasonable distance. + * @return false If the intersection is too far away or the computation fails. + */ +template +bool intersectWithSinPlane(Line line, SinusoidalWaveSurface surface, Vector3& isec) { + + // since only solution with double: + const SinusoidalWaveSurface tempplane(surface.jet_param()); + const Line templine(line.getLineJet()); + + // Initial guess for (x, y) parameters to evaluate on the surface + double xy[2] = {0.0, 0.0}; + + ceres::Problem problem; + + // Add the residual block + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction, 3, 2>( + new IntersectionResidual{templine, tempplane}, + ceres::Ownership::TAKE_OWNERSHIP + ), + nullptr, // no loss function + xy // the (x, y) on the surface + ); + + // TODO: see if better parameter can be chosen + ceres::Solver::Options options; + options.minimizer_type = ceres::MinimizerType::TRUST_REGION; + options.linear_solver_type = ceres::LinearSolverType::DENSE_QR; + options.minimizer_progress_to_stdout = true; + options.logging_type = ceres::SILENT; + options.use_explicit_schur_complement = true; + options.update_state_every_iteration = true; + options.function_tolerance = 1e-30; + options.gradient_tolerance = 1e-30; + options.parameter_tolerance = 1e-30; + options.max_num_iterations = 100; + options.num_threads = sysconf(_SC_NPROCESSORS_ONLN); + + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // print("isec sum: {}\n", summary.FullReport()); + + // Get the resulting point on the surface + Vector3 intersection = tempplane.evaluate(xy[0], xy[1]); + + isec = intersection.template cast(); + + return true; +} + template struct Plane { Vector3 abc; @@ -323,6 +653,7 @@ struct Plane { Plane(): abc(Vector3::Zero()) {} + // costroctur for Vector3D Plane(const Vector3& perpvec, const Vector3& point) { // a(x - px) + b(y - py) + c(z - pz) = 0 // d = -a*px - b*py - c*pz @@ -331,24 +662,29 @@ struct Plane { d = point.dot(-abc); } + // costructor for std::vector Plane(const std::vector& perpvec, const std::vector& point): Plane( Vector3 { perpvec.at(0), perpvec.at(1), perpvec.at(2) }, Vector3 { point.at(0), point.at(1), point.at(2) } ) {} - + + // constructor for variables Plane(T a, T b, T c, T d): abc(a, b, c), d(d) { abc.normalize(); } + // from a pointer Plane(const T* abcd): Plane(abcd[0], abcd[1], abcd[2], abcd[3]) {} Plane(const std::vector& abcd): Plane(abcd.at(0), abcd.at(1), abcd.at(2), abcd.at(3)) {} + // return the plane as a 4D vector Vector4 abcd() const { return { abc.x(), abc.y(), abc.z(), d }; } + // NOTE: not used Vector3 some_point() const { Vector3 ret; size_t maxidx; @@ -358,7 +694,7 @@ struct Plane { double max = abc[maxidx]; if (almost_zero(max)) - println(stderr, "Almost zero maximum coefficient"); + print(stderr, "Almost zero maximum coefficient\n"); switch (maxidx) { case 0: // a @@ -409,6 +745,7 @@ struct Plane { } }; +// NOTE: not used? void to_json(nlohmann::json& j, const Plane& p) { j = { { "pt", p.some_point() }, { "abcd", p.abcd() } }; } @@ -422,8 +759,9 @@ bool forward_refract_estimate( Vector3& lestimate, Vector3& restimate, Vector3& lisect, - Vector3& risect -) { + Vector3& risect) + { + Line lline(pt - T0, T0), rline(pt - T1, T1); if (!plane.intersect_with(lline, lisect)) @@ -445,6 +783,44 @@ bool forward_refract_estimate( return true; } +template +bool forward_refract_estimate_sin( + const Vector3& pt, + const Vector3& T0, + const Vector3& T1, + const SinusoidalWaveSurface& plane, + Vector3& lestimate, + Vector3& restimate, + Vector3& lisect, + Vector3& risect) + { + + Line lline(pt - T0, T0), rline(pt - T1, T1); + + if (!intersectWithSinPlane(lline, plane, lisect)) + return false; + if (!intersectWithSinPlane(rline, plane, risect)) + return false; + + // print("lisect: {}\n", lisect); + // print("lline dir: {}\n", lline.dir); + // print("lline origin: {}\n", lline.pt); + // print("plane: {}\n", plane.wave_param()); + + Vector3 lrefr = plane.refract(lline), rrefr = plane.refract(rline); + + Plane rrefrplane(rline.dir.cross(rrefr), risect), lrefrplane(lline.dir.cross(lrefr), lisect); + + Line lrefrline(lrefr, lisect), rrefrline(rrefr, risect); + + if (!rrefrplane.intersect_with(lrefrline, lestimate)) + return false; + if (!lrefrplane.intersect_with(rrefrline, restimate)) + return false; + + return true; +} + template void back_refract( const Vector3& estimate, @@ -463,6 +839,24 @@ void back_refract( distance = Line(*backrefraction, isect).distance_to(baseline); } +template +void back_refract_sin( + const Vector3& estimate, + const Vector3& isect, + const Vector3& baseline, + const SinusoidalWaveSurface& someplane, + Vector3& distance, + Vector3* backrefraction +) { + static thread_local Vector3 backrefraction_; + if (!backrefraction) + backrefraction = &backrefraction_; + + *backrefraction = someplane.refract(Line { isect - estimate, estimate }, true); + + distance = Line(*backrefraction, isect).distance_to(baseline); +} + struct MyCostFunctor { const Vector3d T0, T1; const Vector3d scene; @@ -473,11 +867,13 @@ struct MyCostFunctor { scene(warped) {} }; +// cost for plane waves struct EstimatedDistanceCostFunctor: public MyCostFunctor { using MyCostFunctor::MyCostFunctor; template bool operator()(const T* const abcd, T* residuals) const { + const Plane someplane(abcd); Vector3 _lestimates, _restimates, _lisects, _risects, _back; @@ -502,6 +898,45 @@ struct EstimatedDistanceCostFunctor: public MyCostFunctor { residuals[1] = distance.y(); residuals[2] = distance.z(); + // fmt::print("Res est distance: {}\n", distance); + + return true; + } +}; + +// cost function for sin waves +struct EstimatedDistanceCostFunctorSinWave: public MyCostFunctor { + using MyCostFunctor::MyCostFunctor; + + template + bool operator()(const T* const sin_params, T* residuals) const { + + const SinusoidalWaveSurface someplane(sin_params); + + Vector3 _lestimates, _restimates, _lisects, _risects, _back; + + bool ok = forward_refract_estimate_sin( + scene.cast(), + T0.cast(), + T1.cast(), + someplane, + _lestimates, + _restimates, + _lisects, + _risects + ); + + if (!ok) + return false; + + Vector3 distance = _lestimates - _restimates; + + residuals[0] = distance.x(); + residuals[1] = distance.y(); + residuals[2] = distance.z(); + + // fmt::print("Res est distance: {}\n", distance); + return true; } }; @@ -509,9 +944,9 @@ struct EstimatedDistanceCostFunctor: public MyCostFunctor { template struct BackrefractionCostFunctor: public MyCostFunctor { using MyCostFunctor::MyCostFunctor; - template bool operator()(const T* const abcd, T* residuals) const { + const Plane someplane(abcd); Vector3 _lestimates, _restimates, _lisects, _risects, _back; @@ -539,8 +974,51 @@ struct BackrefractionCostFunctor: public MyCostFunctor { residuals[1] = _back.y(); residuals[2] = _back.z(); + // fmt::print("Res back: {}{}{}\n", _back.x(), _back.y(), _back.z()); + return true; } + +}; + +template +struct BackrefractionCostFunctorSin: public MyCostFunctor { + using MyCostFunctor::MyCostFunctor; + template + bool operator()(const T* const sin_param, T* residuals) const { + + const SinusoidalWaveSurface someplane(sin_param); + + Vector3 _lestimates, _restimates, _lisects, _risects, _back; + + bool ok = forward_refract_estimate_sin( + scene.cast(), + T0.cast(), + T1.cast(), + someplane, + _lestimates, + _restimates, + _lisects, + _risects + ); + + if (!ok) + return false; + + if constexpr (LEFT) + back_refract_sin(_lestimates, _risects, T0.cast(), someplane, _back, nullptr); + else + back_refract_sin(_restimates, _lisects, T1.cast(), someplane, _back, nullptr); + + residuals[0] = _back.x(); + residuals[1] = _back.y(); + residuals[2] = _back.z(); + + // fmt::print("Res back: {}{}{}\n", _back.x(), _back.y(), _back.z()); + + return true; + } + }; enum class DetectionType { APRIL, CCTAG, SIFT }; @@ -653,13 +1131,32 @@ class Recorder: public ceres::IterationCallback { } }; +class RecorderSin: public ceres::IterationCallback { + private: + std::vector> _steps; + const double* const sin_params; + + public: + RecorderSin(const double* sin_params): sin_params(sin_params) {} + ceres::CallbackReturnType operator()(const ceres::IterationSummary& _ [[maybe_unused]] + ) override { + _steps.push_back({ sin_params }); + return ceres::CallbackReturnType::SOLVER_CONTINUE; + } + auto consume() { + return std::move(_steps); + } + }; + template using StereoMap = std::map, TVal>; int main(int argc, const char** argv) { + // clang-format off - cxxopts::Options options("schnell"); + + // possible argument passde with --argument not only argument options.add_options() ("datapath", "path to data", cxxopts::value()) ("abcd", "initial plane values", cxxopts::value>()) @@ -667,20 +1164,27 @@ int main(int argc, const char** argv) { ("perpvec", "xyz components of vector perpendicular to plane", cxxopts::value>()) ("sift", "enable sift detection") ("solve", "runs solver") + ("sin", "enable sinusoidal wave surface") + //("huber", "huber loss coefficient", cxxopts::value()->default_value("0.1")) - ("lone", "softlone loss coefficient", cxxopts::value()->default_value("0.1")); + ("lone", "softlone loss coefficient", cxxopts::value()->default_value("0.04")); options.parse_positional({"datapath"}); const auto args = options.parse(argc, argv); - + // clang-format on bool solve = args.count("solve"); + bool sin = args.count("sin"); DetectionType detection_type = args.count("sift") ? DetectionType::SIFT : DetectionType::APRIL; - std::string datapath = args["datapath"].as(); + print(stderr, "data path : {}\n", datapath); + print(stderr, "type of detection: {}\n", args.count("sift") ? "sift" : "apriltag"); + print(stderr, "lone: {}\n", args.count("lone") ? args["lone"].as() : 0.01); + print(stderr, "sin: {}\n", args.count("sin") ? "optimizing sin plane" : "optimizing flat plane"); + StereoMap combos; for (size_t i = 0; i < camidxs.size(); ++i) { for (size_t j = i + 1; j < camidxs.size(); ++j) { @@ -703,12 +1207,15 @@ int main(int argc, const char** argv) { } } + // get how to estimate the fisrt guess of the plane -> if no argument given pca of the plane bool guess_plane = !(args.count("abcd") || (args.count("perpvec") && args.count("point"))); - + + // init the plane Plane someplane; - + SinusoidalWaveSurface somesinplane; StereoMap triangulations; StereoMap> camera_positions; + for (const auto& [key, combo]: combos) { camera_positions[key] = combo.get_baseline_in_reference_frame(); triangulations[key] = combo.triangulate_into_referece_frame(detection_type); @@ -716,110 +1223,313 @@ int main(int argc, const char** argv) { if (guess_plane) { Vectors3d pattern_center_vectors; - Vectors3d pattern_evecs; + + // TODO see if it the right assigned -> works for tested case should be the biggest eigenvalue first + Vectors3d pattern_evecs_Z; + Vectors3d pattern_evecs_Y; + Vectors3d pattern_evecs_X; for (const auto& [key, combo]: combos) { const auto& [T0, T1] = camera_positions[key]; const auto [mean, evecs] = principal_components(triangulations[key]); pattern_center_vectors.push_back(mean - T0); pattern_center_vectors.push_back(mean - T1); - pattern_evecs.push_back(evecs[0]); + pattern_evecs_Z.push_back(evecs[0]); + pattern_evecs_Y.push_back(evecs[1]); + pattern_evecs_X.push_back(evecs[2]); } Vector3d mean_pcv = std::accumulate( pattern_center_vectors.cbegin(), pattern_center_vectors.cend(), Vector3d::Zero().eval() - ) - / pattern_center_vectors.size(); - Vector3d mean_evec = - std::accumulate(pattern_evecs.cbegin(), pattern_evecs.cend(), Vector3d::Zero().eval()) - / pattern_evecs.size(); - - someplane = Plane((-mean_evec).eval(), (mean_pcv / 2).eval()); + ) / pattern_center_vectors.size(); + + Vector3d mean_evec_Z = + std::accumulate(pattern_evecs_Z.cbegin(), pattern_evecs_Z.cend(), Vector3d::Zero().eval()) + / pattern_evecs_Z.size(); + Vector3d mean_evec_Y = + std::accumulate(pattern_evecs_Y.cbegin(), pattern_evecs_Y.cend(), Vector3d::Zero().eval()) + / pattern_evecs_Y.size(); + Vector3d mean_evec_X = + std::accumulate(pattern_evecs_X.cbegin(), pattern_evecs_X.cend(), Vector3d::Zero().eval()) + / pattern_evecs_X.size(); + + print("mean: {}\n", (mean_pcv / 2).eval()); + print("evecsZ: {}\n", mean_evec_Z); + print("evecsY: {}\n", mean_evec_Y); + print("evecsX: {}\n", mean_evec_X); + + someplane = Plane((-mean_evec_Z).eval(), (mean_pcv / 2).eval()); // flat plane only one vector in Z direction + somesinplane = SinusoidalWaveSurface( + (mean_pcv / 2).eval(), + mean_evec_X.normalized(), + mean_evec_Y.normalized(), + mean_evec_Z.normalized(), + 0, 0, 0 // amplitude, frequency, phase + ); + + // Does not work for sin } else if (args.count("abcd")) { someplane = Plane(args["abcd"].as>()); + } else if (args.count("perpvec") && args.count("point")) { someplane = Plane( args["perpvec"].as>(), args["point"].as>() ); - } else + } else { throw std::invalid_argument("bad args"); + } - println("{}", someplane.abcd()); + print("fist guess: {}\n", someplane.abcd()); + print("fist sin guess: {}\n", somesinplane.wave_param()); + + // //NOTE: HERE ----------------------------------- + // Vector3 intersections; + // SinusoidalWaveSurface wave_surface = { + // {0.1522135796926206, 0.07386223835536908, 0.2721447693021134}, // origin + // {-0.8779893174123757, 0.11769968478070637, 0.46398442076461244}, // u -> propagation is in this direction + // {-0.007492214882922366, 0.9658229358896685, -0.25909442916745534}, // v + // {0.47911459271736, 0.23195390399035876, 0.8465498174761541}, // normal + // 0, 0, 0 // amplitude, frequency, phase + // }; + // Line line({0, 0, 1}, {0,1,1}); // vec , origin + // // print("sin wave: {}\n", wave_surface.wave_param()); + // bool res = intersectWithSinPlane(line, wave_surface, intersections); + // print("intersection: {}\n", intersections); + // //NOTE: HERE ----------------------------------- std::vector> steps; + std::vector> steps_sin; + // solve for every combo in multiple steps -> optimization if (solve) { google::InitGoogleLogging(argv[0]); ceres::Problem problem; + ceres::Solver::Options solver_opts; auto abcd_vec = someplane.abcd(); - double abcd[] = { abcd_vec.coeff(0), - abcd_vec.coeff(1), - abcd_vec.coeff(2), - abcd_vec.coeff(3) }; + double abcd[] = {abcd_vec.coeff(0), + abcd_vec.coeff(1), + abcd_vec.coeff(2), + abcd_vec.coeff(3) }; + + auto sine_vec = somesinplane.wave_param(); + double sin_params[] = { sine_vec.coeff(0), + sine_vec.coeff(1), + sine_vec.coeff(2), + sine_vec.coeff(3), + sine_vec.coeff(4), + sine_vec.coeff(5), + sine_vec.coeff(6), + sine_vec.coeff(7), + sine_vec.coeff(8), + sine_vec.coeff(9), + sine_vec.coeff(10), + sine_vec.coeff(11),}; - auto backrefraction_loss = new ceres::SoftLOneLoss(args["lone"].as()); - auto distance_loss = nullptr; // new ceres::HuberLoss(args["huber"].as()); + auto callback = std::make_unique(abcd); + auto callback_sin = std::make_unique(sin_params); + + if(!sin){ + + auto backrefraction_loss = new ceres::SoftLOneLoss(args["lone"].as()); + auto distance_loss = nullptr; // new ceres::HuberLoss(args["huber"].as()); + + problem.AddParameterBlock(abcd, 4); + // Set bounds + problem.SetParameterLowerBound(abcd, 0, -1.0); + problem.SetParameterUpperBound(abcd, 0, 1.0); + problem.SetParameterLowerBound(abcd, 1, -1.0); + problem.SetParameterUpperBound(abcd, 1, 1.0); + problem.SetParameterLowerBound(abcd, 2, -1.0); + problem.SetParameterUpperBound(abcd, 2, 1.0); + problem.SetParameterLowerBound(abcd, 3, 0.0); + problem.SetParameterUpperBound(abcd, 3, 40.0); + + // iterate through all the combinations of cameras + for (const auto& [key, combo]: combos) { + const auto& [T0, T1] = camera_positions[key]; + + // iterate through all the triangulated points + for (const auto& point: triangulations[key]) { + + // estimates the ditstance between the right and left estimated points + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new EstimatedDistanceCostFunctor { point, T0, T1 }, + ceres::Ownership::TAKE_OWNERSHIP + ), + distance_loss, + abcd + ); + + // calculates the distance to the left camera of the backrefraction + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction, 3, 4>( + new BackrefractionCostFunctor {point, T0, T1 }, + ceres::Ownership::TAKE_OWNERSHIP + ), + backrefraction_loss, + abcd + ); + + // calculates the distance to the right camera of the backrefraction + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction, 3, 4>( + new BackrefractionCostFunctor{ point, T0, T1 }, + ceres::Ownership::TAKE_OWNERSHIP + ), + backrefraction_loss, + abcd + ); + } + } - for (const auto& [key, combo]: combos) { - const auto& [T0, T1] = camera_positions[key]; + solver_opts.num_threads = sysconf(_SC_NPROCESSORS_ONLN); + solver_opts.minimizer_progress_to_stdout = true; + solver_opts.update_state_every_iteration = true; + solver_opts.callbacks = { callback.get() }; + + solver_opts.max_num_iterations = INT_MAX; + solver_opts.function_tolerance = 1e-30; + solver_opts.parameter_tolerance = 1e-30; + solver_opts.gradient_tolerance = 1e-30; + + + solver_opts.minimizer_type = ceres::MinimizerType::TRUST_REGION; + solver_opts.linear_solver_type = ceres::LinearSolverType::DENSE_QR; + solver_opts.use_explicit_schur_complement = true; + + steps = callback->consume(); - for (const auto& point: triangulations[key]) { - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction( - new EstimatedDistanceCostFunctor { point, T0, T1 }, - ceres::Ownership::TAKE_OWNERSHIP - ), - distance_loss, - abcd - ); - - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction, 3, 4>( - new BackrefractionCostFunctor { point, T0, T1 }, - ceres::Ownership::TAKE_OWNERSHIP - ), - backrefraction_loss, - abcd - ); - - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction, 3, 4>( - new BackrefractionCostFunctor { point, T0, T1 }, - ceres::Ownership::TAKE_OWNERSHIP - ), - backrefraction_loss, - abcd - ); - } } - auto callback = std::make_unique(abcd); + // if sin wave + else + { - ceres::Solver::Options solver_opts; - solver_opts.num_threads = sysconf(_SC_NPROCESSORS_ONLN); - solver_opts.minimizer_progress_to_stdout = true; - solver_opts.update_state_every_iteration = true; - solver_opts.callbacks = { callback.get() }; + auto backrefraction_loss = new ceres::SoftLOneLoss(args["lone"].as()); + auto distance_loss = nullptr; // new ceres::HuberLoss(args["huber"].as()); + + problem.AddParameterBlock(sin_params, 12); + + // set bound + // origin + problem.SetParameterLowerBound(sin_params, 0, -1.0); + problem.SetParameterUpperBound(sin_params, 0, 1.0); + problem.SetParameterLowerBound(sin_params, 1, -1.0); + problem.SetParameterUpperBound(sin_params, 1, 1.0); + problem.SetParameterLowerBound(sin_params, 2, 0.0); + problem.SetParameterUpperBound(sin_params, 2, 1.0); + + // propagation direction + problem.SetParameterLowerBound(sin_params, 3, -1.0); + problem.SetParameterUpperBound(sin_params, 3, 1.0); + problem.SetParameterLowerBound(sin_params, 4, -1.0); + problem.SetParameterUpperBound(sin_params, 4, 1.0); + problem.SetParameterLowerBound(sin_params, 5, -1.0); + problem.SetParameterUpperBound(sin_params, 5, 1.0); + + // normal -> Note: need to ensure that it is pointing towards the cameras + problem.SetParameterLowerBound(sin_params, 6, 0.3); + problem.SetParameterUpperBound(sin_params, 6, 0.6); + problem.SetParameterLowerBound(sin_params, 7, 0.1); + problem.SetParameterUpperBound(sin_params, 7, 0.3); + problem.SetParameterLowerBound(sin_params, 8, 0.7); + problem.SetParameterUpperBound(sin_params, 8, 1.0); + + // amplitude + problem.SetParameterLowerBound(sin_params, 9, 0.0); + problem.SetParameterUpperBound(sin_params, 9, 0.1); + + // frequency + problem.SetParameterLowerBound(sin_params, 10, 0.0); + problem.SetParameterUpperBound(sin_params, 10, 0.1); + + // phase + problem.SetParameterLowerBound(sin_params, 11, 0.0); + problem.SetParameterUpperBound(sin_params, 11, 0.1); + + // iterate through all the combinations of cameras + for (const auto& [key, combo]: combos) { + const auto& [T0, T1] = camera_positions[key]; + + // iterate through all the triangulated points + for (const auto& point: triangulations[key]) { + + // estimates the ditstance between the right and left estimated points + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new EstimatedDistanceCostFunctorSinWave{ point, T0, T1 }, + ceres::Ownership::TAKE_OWNERSHIP + ), + distance_loss, + sin_params + ); + + // calculates the distance to the left camera of the backrefraction + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction, 3, 12>( + new BackrefractionCostFunctorSin {point, T0, T1 }, + ceres::Ownership::TAKE_OWNERSHIP + ), + backrefraction_loss, + sin_params + ); + + // calculates the distance to the right camera of the backrefraction + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction, 3, 12>( + new BackrefractionCostFunctorSin{ point, T0, T1 }, + ceres::Ownership::TAKE_OWNERSHIP + ), + backrefraction_loss, + sin_params + ); + } + } - solver_opts.max_num_iterations = INT_MAX; - solver_opts.function_tolerance = 1e-30; - solver_opts.parameter_tolerance = 1e-20; + solver_opts.num_threads = sysconf(_SC_NPROCESSORS_ONLN); + solver_opts.minimizer_progress_to_stdout = true; + solver_opts.update_state_every_iteration = true; + solver_opts.callbacks = { callback_sin.get() }; + + solver_opts.max_num_iterations = INT_MAX; + solver_opts.function_tolerance = 1e-30; + solver_opts.parameter_tolerance = 1e-30; + solver_opts.gradient_tolerance = 1e-30; + + solver_opts.minimizer_type = ceres::MinimizerType::TRUST_REGION; + solver_opts.line_search_direction_type = ceres::LineSearchDirectionType::LBFGS; + solver_opts.linear_solver_type = ceres::LinearSolverType::DENSE_QR; + solver_opts.use_explicit_schur_complement = true; + + steps_sin = callback_sin->consume(); + } + + // ceres::Solver::Options solver_opts; + // solver_opts.num_threads = sysconf(_SC_NPROCESSORS_ONLN); + // solver_opts.minimizer_progress_to_stdout = true; + // solver_opts.update_state_every_iteration = true; + // solver_opts.callbacks = { callback.get() }; + + // solver_opts.max_num_iterations = INT_MAX; + // solver_opts.function_tolerance = 1e-30; + // solver_opts.parameter_tolerance = 1e-20; - // solver_opts.minimizer_type = ceres::MinimizerType::LINE_SEARCH; - // solver_opts.line_search_direction_type = - // ceres::LineSearchDirectionType::NONLINEAR_CONJUGATE_GRADIENT; + // solver_opts.minimizer_type = ceres::MinimizerType::TRUST_REGION; + // solver_opts.linear_solver_type = ceres::LinearSolverType::DENSE_QR; + // solver_opts.use_explicit_schur_complement = true; - solver_opts.minimizer_type = ceres::MinimizerType::TRUST_REGION; - //solver_opts.linear_solver_type = ceres::LinearSolverType::DENSE_NORMAL_CHOLESKY; + // solver_opts.line_search_direction_type = ceres::LineSearchDirectionType::NONLINEAR_CONJUGATE_GRADIENT; std::string error; + if (!solver_opts.IsValid(&error)) { - println(stderr, "{}", error); + print(stderr, "error : {}\n", error); return 1; } @@ -828,62 +1538,133 @@ int main(int argc, const char** argv) { ceres::Solver::Summary summary; ceres::Solve(solver_opts, &problem, &summary); + + print("ceres report: {}\n", summary.FullReport()); - println("{}", summary.FullReport()); + someplane = {abcd}; + print("best plane: {}\n", someplane.abcd()); + somesinplane = {sin_params}; + print("best sin plane: {}\n", somesinplane.wave_param()); - someplane = { abcd }; steps = callback->consume(); + steps_sin = callback_sin->consume(); } - steps.push_back(someplane); - - json serialized = { { "steps", json::array() } }; - - for (size_t i = 0; i < steps.size(); ++i) { - const auto& plane = steps[i]; - - serialized["steps"].push_back({ { "plane", plane }, { "stereopairs", json::array() } }); - auto& stereopairs = serialized["steps"][i]["stereopairs"]; - - for (const auto& [_, combo]: combos) { - Vectors3d lestimates, restimates, lisects, risects, rbacks, lbacks, rbackrefrs, - lbackrefrs; - - const auto warped3D = combo.triangulate_into_referece_frame(detection_type); - const auto [T0, T1] = combo.get_baseline_in_reference_frame(); - - for (const auto& point: warped3D) { - Vector3d lestimate, restimate, lisect, risect, rback, lback, rbackrefr, lbackrefr; + // print the results to the serial as json + if(!sin){ + + // create json file for passing to the python program + steps.push_back(someplane); + + json serialized = { { "steps", json::array() } }; + + for (size_t i = 0; i < steps.size(); ++i) { + const auto& plane = steps[i]; + + serialized["steps"].push_back({ { "plane", plane }, { "stereopairs", json::array() } }); + auto& stereopairs = serialized["steps"][i]["stereopairs"]; + + for (const auto& [_, combo]: combos) { + Vectors3d lestimates, restimates, lisects, risects, rbacks, lbacks, rbackrefrs, + lbackrefrs; + + const auto warped3D = combo.triangulate_into_referece_frame(detection_type); + const auto [T0, T1] = combo.get_baseline_in_reference_frame(); + + for (const auto& point: warped3D) { + Vector3d lestimate, restimate, lisect, risect, rback, lback, rbackrefr, lbackrefr; + + forward_refract_estimate(point, T0, T1, plane, lestimate, restimate, lisect, risect); + back_refract(lestimate, risect, T1, plane, rback, &rbackrefr); + back_refract(restimate, lisect, T0, plane, lback, &lbackrefr); + + lestimates.push_back(lestimate); + restimates.push_back(restimate); + lisects.push_back(lisect); + risects.push_back(risect); + rbacks.push_back(rback); + lbacks.push_back(lback); + rbackrefrs.push_back(rbackrefr); + lbackrefrs.push_back(lbackrefr); + } + + stereopairs.push_back(json { { "idxs", combo.idxs }, + { "scenepoints", warped3D }, + { "lestimates", lestimates }, + { "restimates", restimates }, + { "T0", T0 }, + { "T1", T1 }, + { "lback", lbacks }, + { "rback", rbacks }, + { "lbackrefr", lbackrefrs }, + { "rbackrefr", rbackrefrs }, + { "lisects", lisects }, + { "risects", risects } }); + } + } - forward_refract_estimate< - double>(point, T0, T1, plane, lestimate, restimate, lisect, risect); - back_refract(lestimate, risect, T1, plane, rback, &rbackrefr); - back_refract(restimate, lisect, T0, plane, lback, &lbackrefr); + print("DELIMITER{}\n", serialized.dump()); - lestimates.push_back(lestimate); - restimates.push_back(restimate); - lisects.push_back(lisect); - risects.push_back(risect); - rbacks.push_back(rback); - lbacks.push_back(lback); - rbackrefrs.push_back(rbackrefr); - lbackrefrs.push_back(lbackrefr); + } + else{ + + // create json file for passing to the python program + steps_sin.push_back(somesinplane); + + json serialized = { { "steps", json::array() } }; + + for (size_t i = 0; i < steps_sin.size(); ++i) { + const auto& plane = steps_sin[i]; + + serialized["steps"].push_back({ { "plane", plane.wave_param() }, { "stereopairs", json::array() } }); + auto& stereopairs = serialized["steps"][i]["stereopairs"]; + + for (const auto& [_, combo]: combos) { + + Vectors3d lestimates, restimates, lisects, risects, rbacks, lbacks, rbackrefrs, + lbackrefrs; + + const auto warped3D = combo.triangulate_into_referece_frame(detection_type); + const auto [T0, T1] = combo.get_baseline_in_reference_frame(); + + for (const auto& point: warped3D) { + + Vector3d lestimate, restimate, lisect, risect, rback, lback, rbackrefr, lbackrefr; + + forward_refract_estimate_sin(point, T0, T1, plane, lestimate, restimate, lisect, risect); + back_refract_sin(lestimate, risect, T1, plane, rback, &rbackrefr); + back_refract_sin(restimate, lisect, T0, plane, lback, &lbackrefr); + + // print("lestimate: {}\n", lestimate); + // print("restimate: {}\n", restimate); + // print("lback: {}\n", lback); + // print("rback: {}\n", rback); + + lestimates.push_back(lestimate); + restimates.push_back(restimate); + lisects.push_back(lisect); + risects.push_back(risect); + rbacks.push_back(rback); + lbacks.push_back(lback); + rbackrefrs.push_back(rbackrefr); + lbackrefrs.push_back(lbackrefr); + } + + stereopairs.push_back(json { { "idxs", combo.idxs }, + { "scenepoints", warped3D }, + { "lestimates", lestimates }, + { "restimates", restimates }, + { "T0", T0 }, + { "T1", T1 }, + { "lback", lbacks }, + { "rback", rbacks }, + { "lbackrefr", lbackrefrs }, + { "rbackrefr", rbackrefrs }, + { "lisects", lisects }, + { "risects", risects } }); } - - stereopairs.push_back(json { { "idxs", combo.idxs }, - { "scenepoints", warped3D }, - { "lestimates", lestimates }, - { "restimates", restimates }, - { "T0", T0 }, - { "T1", T1 }, - { "lback", lbacks }, - { "rback", rbacks }, - { "lbackrefr", lbackrefrs }, - { "rbackrefr", rbackrefrs }, - { "lisects", lisects }, - { "risects", risects } }); } - } - println("DELIMITER{}", serialized.dump()); + print("DELIMITER{}\n", serialized.dump()); + } } diff --git a/viswrap.py b/viswrap.py index b77f2bd..0ebda61 100755 --- a/viswrap.py +++ b/viswrap.py @@ -7,10 +7,35 @@ import numpy as np import os import sys +import math + +from scipy.optimize import least_squares os.environ['QT_QPA_PLATFORM'] = 'xcb' -datadir = os.environ["HOME"] + '/Daten/wasserkiste/foureyes/april' +datadir = os.environ["HOME"] + '/Documents/schnell/Daten/wasserkiste/foureyes/april' + +def parser(): + + sin = False + + if len(sys.argv) < 2: + + sin = False + + else: + + # get the arguments to the wrigth variabales + for elements in sys.argv[1:]: + + print(elements) + + if(elements == "--sin" ): + sin = True + + return sin + +SIN = parser() schnell = subprocess.Popen( ['builddir/schnell', datadir] + sys.argv, @@ -26,6 +51,11 @@ stdout, jsondump = stdout.decode().strip().split('DELIMITER') print(stdout) + +output_path ="output.json" +with open(output_path, "w") as f: + f.write(jsondump) + data = json.loads(jsondump) steps = data["steps"] @@ -63,8 +93,6 @@ def circgrid(pt, abcd, scale = 1): return np.array(xs), np.array(ys), np.array(zs) -### - # https://math.stackexchange.com/a/897677 def vector_align(a, b): a /= np.linalg.norm(a) @@ -88,15 +116,22 @@ def intersect(abcd, p, n): n = n / np.linalg.norm(n) ax, ay, az = p + n bx, by, bz = p - a = np.array([ax, ay, az, 1]).reshape(-1, 1) - b = np.array([bx, by, bz, 1]).reshape(-1, 1) + a = np.append(p + n, 1).reshape(-1, 1) + b = np.append(p, 1).reshape(-1, 1) + + # pluecker = a @ b.T - b @ a.T + # x, y, z, w = pluecker.T @ abcd + + assert a.shape == b.shape == (4,1), f"{a.shape} {b.shape} not (4,1)" + pluecker = a @ b.T - b @ a.T - x, y, z, w = pluecker.T @ abcd + isect = pluecker.T @ abcd - if np.isclose(w, 0): - return None + # if np.isclose(w, 0): + # return None - return np.array([x / w, y / w, z / w]) + # return np.array([x / w, y / w, z / w]) + return isect[:-1] / isect[-1] drawn_cams = set() cam_colors = [ @@ -106,19 +141,73 @@ def intersect(abcd, p, n): draw_idx = 0 curscene = [] +# to get the surface of the sin wave +def evaluate(x, y, plane_param): + + origin = np.array(plane_param[:3]) + u = np.array(plane_param[3:6]) + v = np.array(plane_param[6:9]) + normal = np.array(plane_param[9:12]) + amplitude = plane_param[12] + frequency = plane_param[13] + phase = plane_param[14] + + base = origin + x * u + y * v + wave = amplitude * np.sin(frequency * x + phase) + return base + wave * normal + +def intersect_line_with_surface(plane_param, line_origin, line_dir, initial_guess=(0.0, 0.0)): + line_origin = np.array(line_origin) + line_dir = np.array(line_dir) + line_dir = line_dir / np.linalg.norm(line_dir) # normalize + + def residual(xy): + x, y = xy + surface_point = evaluate(x, y, plane_param) + o = line_origin + d = line_dir + + # Project surface point onto line + t = np.dot(surface_point - o, d) + closest_point = o + t * d + + diff = surface_point - closest_point + return diff # 3 residuals: (dx, dy, dz) + + result = least_squares(residual, initial_guess) + + if result.success: + x_opt, y_opt = result.x + intersection_point = evaluate(x_opt, y_opt, plane_param) + return intersection_point + else: + raise RuntimeError("Intersection optimization failed!") + def draw(_): + global draw_idx global curscene j = steps[draw_idx] - plane_pt = np.array(j["plane"]["pt"]) - plane_abcd = np.array(j["plane"]["abcd"]) + + if SIN: + + plane_pt = np.array(j["plane"][:3]) + plane_param = np.array(j["plane"]) + plane_n = np.array(plane_param[9:12]) + + print("plane_param", plane_param) + + else: + plane_pt = np.array(j["plane"]["pt"]) + plane_abcd = np.array(j["plane"]["abcd"]) + plane_n = plane_abcd[:-1] + print(draw_idx) draw_idx += 1 xlim, ylim = np.array([np.inf, -np.inf]), np.array([np.inf, -np.inf]) - plane_n = plane_abcd[:-1] for elem in curscene: elem.parent = None @@ -150,7 +239,8 @@ def draw(_): evals, evecs = np.linalg.eig(np.cov(restimates.T)) rnormal = evecs[:, np.argmin(evals)] - normals.append((lnormal + rnormal) / 2) + normals.append(lnormal) + normals.append(rnormal) means.append( (restimates.mean(axis=0) + lestimates.mean(axis=0)) / 2 ) # xlim[0] = min(np.min(triangulations[:,0]), xlim[0]) @@ -177,20 +267,26 @@ def draw(_): parent=view.scene, face_color='blue' )) + + filtered = [vec for vec in pair["lback"] if all((x is not None) and (not math.isnan(x)) for x in vec)] curscene.append(scene.visuals.Markers( - pos=-np.array(pair["lback"]) + T0, + pos= -np.array(filtered) + T0, parent=view.scene, size=3.5, edge_width_rel=0.5, edge_color=cam_colors[idx1] )) + + filtered = [vec for vec in pair["rback"] if all((x is not None) and (not math.isnan(x)) for x in vec)] curscene.append(scene.visuals.Markers( - pos=-np.array(pair["rback"]) + T1, + pos = -np.array(filtered) + T1, + # pos= -np.array(pair["rback"]) + T1, parent=view.scene, size=3.5, edge_width_rel=0.5, edge_color=cam_colors[idx2] )) + # for lbr, rbr, li, ri in zip(lbackrefr, rbackrefr, lisects, risects): # scene.visuals.Line( # pos=(li, li + lbr), @@ -202,6 +298,7 @@ def draw(_): # color=(0.5,0.5,0.5,0.75), # parent=view.scene # ) + if idx1 not in drawn_cams: cam1 = scene.visuals.Markers( pos=T0.reshape(1, -1), @@ -219,14 +316,59 @@ def draw(_): ) drawn_cams.add(idx2) - normal = np.sum(normals, axis=0) + # turn the normal vector arround if in the other direction -> check if this allways works + norm_nomals= [] + for n in normals: + n /= np.linalg.norm(n) + if n[2] > 0: + n *= -1 + norm_nomals.append(n) + normal = np.sum(norm_nomals, axis=0) normal /= np.linalg.norm(normal) mean = np.mean(means, axis=0) - isect = intersect(plane_abcd, mean, normal) - if isect is not None: - print(np.linalg.norm( mean - isect ) * 100, "cm") + isect = None + + if SIN: + isect = intersect_line_with_surface(plane_param, mean, normal) + else: + isect = intersect(plane_abcd, mean, normal) + + # black camera is np.zero(3) + # print("normal", normal) + # print("guess plane", plane_abcd) + # print("mean", mean) + if isect is not None: + print("vec", (mean.reshape(1, -1) - isect.reshape(1, -1))/ np.linalg.norm((mean.reshape(1, -1) - isect.reshape(1, -1))), "water height", np.linalg.norm(mean.reshape(1, -1) - isect.reshape(1, -1)), "m") + + for n in norm_nomals: + dist = np.vstack([mean.reshape(1, -1), mean.reshape(1, -1) + n.reshape(1, -1)]) + curscene.append(scene.visuals.Line( + pos=dist, + color='green', + width=3, + # method='gl', # GPU-accelerated line rendering + parent=view.scene + )) + dist = np.vstack([mean.reshape(1, -1), isect.reshape(1, -1)]) + curscene.append(scene.visuals.Line( + pos=dist, + color='red', + width=3, + # method='gl', # GPU-accelerated line rendering + parent=view.scene + )) + curscene.append(scene.visuals.Markers( + pos=isect.reshape(1, -1), + parent=view.scene, + face_color='red' + )) + curscene.append(scene.visuals.Markers( + pos=mean.reshape(1, -1), + parent=view.scene, + face_color='orange' + )) curscene.append(scene.visuals.Arrow( pos=( plane_pt, @@ -239,18 +381,51 @@ def draw(_): # x=xx, y=yy, z=zz, # parent=view.scene # ) - plane = scene.visuals.Plane( - direction='+x', - parent=view.scene - ) - plane.transform = vp.scene.ChainTransform( - vp.scene.STTransform(translate=plane_pt), - vp.scene.MatrixTransform(np.vstack((np.hstack((vector_align([1, 0, 0], plane_n), np.zeros((3,1)))), np.array([0, 0, 0, 1])))) - ) + + plane = None + + if SIN: + + res = 100 + x_vals = np.linspace(-1, 1, res) + y_vals = np.linspace(-1, 1, res) + X, Y = np.meshgrid(x_vals, y_vals) + Z = np.zeros_like(X) + + # Compute 3D surface points + points = np.zeros((res, res, 3), dtype=np.float32) + for i in range(res): + for k in range(res): + points[i, k] = evaluate(X[i, k], Y[i, k], plane_param) + + # Flatten the grid for VisPy + vertices = points.reshape(-1, 3) + + # Build face indices + faces = [] + for i in range(res - 1): + for k in range(res - 1): + idx = i * res + k + faces.append([idx, idx + 1, idx + res]) + faces.append([idx + 1, idx + res + 1, idx + res]) + faces = np.array(faces) + + plane = scene.visuals.Mesh(vertices=vertices, faces=faces, color=(0.5, 0.7, 1, 1), shading='smooth', parent=view.scene) + + else: + plane = scene.visuals.Plane( + direction='+x', + parent=view.scene + ) + plane.transform = vp.scene.ChainTransform( + vp.scene.STTransform(translate=plane_pt), + vp.scene.MatrixTransform(np.vstack((np.hstack((vector_align([1, 0, 0], plane_n), np.zeros((3,1)))), np.array([0, 0, 0, 1])))) + ) plane.attach(vp.visuals.filters.Alpha(0.5)) curscene.append(plane) + my_app = app.use_app() timer = app.Timer(connect=draw, app=my_app) canvas = scene.SceneCanvas(keys='interactive', show=True, app=my_app) From 8eaf17c1a0308cd5f4a008718cba06f426abb8d0 Mon Sep 17 00:00:00 2001 From: Christoph Liebender Date: Wed, 30 Apr 2025 14:09:14 +0200 Subject: [PATCH 2/7] ditch std::{format, println} in favour of fmt, update fmt to 11.1.4 --- meson.build | 2 +- schnell.cpp | 107 ++++++++++++------------------------------- subprojects/fmt.wrap | 18 ++++---- 3 files changed, 39 insertions(+), 88 deletions(-) diff --git a/meson.build b/meson.build index c2be59e..d85c8f1 100644 --- a/meson.build +++ b/meson.build @@ -55,7 +55,7 @@ executable( dependency('opencv4'), dependency('apriltag'), dependency('nlohmann_json'), - dependency('fmt', version: '>=11.1.1'), + dependency('fmt', version: '>=11.1.4'), dependency('cxxopts'), ceres_dep, cctag.dependency('CCTag'), diff --git a/schnell.cpp b/schnell.cpp index cd0ef0b..78792e1 100644 --- a/schnell.cpp +++ b/schnell.cpp @@ -2,12 +2,8 @@ #include #include -#if __cplusplus >= 202302L -#include -#else #include #include -#endif #include #include @@ -47,25 +43,6 @@ using Vectors3d = std::vector; using nlohmann::json; using std::ranges::transform; -#if __cplusplus >= 202302L -template -struct std::formatter>: std::formatter { - auto format(const Eigen::Vector& v, std::format_context& ctx) const { - return std::formatter::format( - std::accumulate( - std::next(v.begin()), - v.end(), - std::format("[{}", v[0]), - [](std::string a, const Type& x) { return std::format("{}, {}", std::move(a), x); } - ) + ']', - ctx - ); - } -}; -using std::print; -using std::format; -#else - // formater for jet data type template struct fmt::formatter> : fmt::formatter { @@ -80,48 +57,22 @@ struct fmt::formatter> : fmt::formatter { } }; -// // formater for Eigen::vector data type -// template -// struct fmt::formatter>: fmt::formatter { -// auto format(const Eigen::Vector& v, fmt::format_context& ctx) const { -// return fmt::formatter::format( -// std::accumulate( -// std::next(v.begin()), -// v.end(), -// fmt::format("[{}", v[0]), -// [](std::string a, const Type& x) { return fmt::format("{}, {}", std::move(a), x); } -// ) + ']', -// ctx -// ); -// } -// }; - -template -struct fmt::formatter> : fmt::formatter { - static_assert(Cols == 1, "This formatter only supports column vectors."); - - auto format(const Eigen::Matrix& v, fmt::format_context& ctx) const { - if (v.size() == 0) - return fmt::formatter::format("[]", ctx); - +// formater for Eigen::vector data type +template +struct fmt::formatter>: fmt::formatter { + auto format(const Eigen::Vector& v, fmt::format_context& ctx) const { return fmt::formatter::format( std::accumulate( - std::next(v.data()), // skip first - v.data() + v.size(), + std::next(v.begin()), + v.end(), fmt::format("[{}", v[0]), - [](std::string a, const Scalar& x) { - return fmt::format("{}, {}", std::move(a), x); - } + [](std::string a, const Type& x) { return fmt::format("{}, {}", std::move(a), x); } ) + ']', ctx ); } }; -using fmt::print; -using fmt::format; -#endif - std::tuple> principal_components(const Vectors3d& vecs) { const Vector3d mean = std::accumulate(vecs.begin(), vecs.end(), Vector3d::Zero().eval()) / vecs.size(); @@ -694,7 +645,7 @@ struct Plane { double max = abc[maxidx]; if (almost_zero(max)) - print(stderr, "Almost zero maximum coefficient\n"); + fmt::println(stderr, "Almost zero maximum coefficient"); switch (maxidx) { case 0: // a @@ -1037,13 +988,13 @@ struct Combo { Combo(int idx1, int idx2, const std::filesystem::path& datapath): idxs(std::make_tuple(idx1, idx2)), - i1(cv::imread(datapath / format("{}.png", idx1), cv::IMREAD_GRAYSCALE)), - i2(cv::imread(datapath / format("{}.png", idx2), cv::IMREAD_GRAYSCALE)), - m1(cv::imread(datapath / format("{}_mask.png", idx1), cv::IMREAD_GRAYSCALE)), - m2(cv::imread(datapath / format("{}_mask.png", idx2), cv::IMREAD_GRAYSCALE)), + i1(cv::imread(datapath / fmt::format("{}.png", idx1), cv::IMREAD_GRAYSCALE)), + i2(cv::imread(datapath / fmt::format("{}.png", idx2), cv::IMREAD_GRAYSCALE)), + m1(cv::imread(datapath / fmt::format("{}_mask.png", idx1), cv::IMREAD_GRAYSCALE)), + m2(cv::imread(datapath / fmt::format("{}_mask.png", idx2), cv::IMREAD_GRAYSCALE)), RefTrans(Eigen::Matrix4d::Identity()) { cv::FileStorage fs( - datapath / format("{}-to-{}.json", idx1, idx2), + datapath / fmt::format("{}-to-{}.json", idx1, idx2), cv::FileStorage::READ ); @@ -1180,10 +1131,10 @@ int main(int argc, const char** argv) { DetectionType detection_type = args.count("sift") ? DetectionType::SIFT : DetectionType::APRIL; std::string datapath = args["datapath"].as(); - print(stderr, "data path : {}\n", datapath); - print(stderr, "type of detection: {}\n", args.count("sift") ? "sift" : "apriltag"); - print(stderr, "lone: {}\n", args.count("lone") ? args["lone"].as() : 0.01); - print(stderr, "sin: {}\n", args.count("sin") ? "optimizing sin plane" : "optimizing flat plane"); + fmt::print(stderr, "data path : {}", datapath); + fmt::print(stderr, "type of detection: {}", args.count("sift") ? "sift" : "apriltag"); + fmt::print(stderr, "lone: {}", args["lone"].as()); + fmt::print(stderr, "sin: {}", args.count("sin") ? "optimizing sin plane" : "optimizing flat plane"); StereoMap combos; for (size_t i = 0; i < camidxs.size(); ++i) { @@ -1255,10 +1206,10 @@ int main(int argc, const char** argv) { std::accumulate(pattern_evecs_X.cbegin(), pattern_evecs_X.cend(), Vector3d::Zero().eval()) / pattern_evecs_X.size(); - print("mean: {}\n", (mean_pcv / 2).eval()); - print("evecsZ: {}\n", mean_evec_Z); - print("evecsY: {}\n", mean_evec_Y); - print("evecsX: {}\n", mean_evec_X); + fmt::println("mean: {}", (mean_pcv / 2).eval()); + fmt::print("evecsZ: {}", mean_evec_Z); + fmt::println("evecsY: {}", mean_evec_Y); + fmt::println("evecsX: {}", mean_evec_X); someplane = Plane((-mean_evec_Z).eval(), (mean_pcv / 2).eval()); // flat plane only one vector in Z direction somesinplane = SinusoidalWaveSurface( @@ -1282,8 +1233,8 @@ int main(int argc, const char** argv) { throw std::invalid_argument("bad args"); } - print("fist guess: {}\n", someplane.abcd()); - print("fist sin guess: {}\n", somesinplane.wave_param()); + fmt::println("fist guess: {}", someplane.abcd()); + fmt::println("fist sin guess: {}", somesinplane.wave_param()); // //NOTE: HERE ----------------------------------- // Vector3 intersections; @@ -1529,7 +1480,7 @@ int main(int argc, const char** argv) { std::string error; if (!solver_opts.IsValid(&error)) { - print(stderr, "error : {}\n", error); + fmt::println(stderr, "error : {}", error); return 1; } @@ -1539,12 +1490,12 @@ int main(int argc, const char** argv) { ceres::Solve(solver_opts, &problem, &summary); - print("ceres report: {}\n", summary.FullReport()); + fmt::println("ceres report: {}", summary.FullReport()); someplane = {abcd}; - print("best plane: {}\n", someplane.abcd()); + fmt::print("best plane: {}", someplane.abcd()); somesinplane = {sin_params}; - print("best sin plane: {}\n", somesinplane.wave_param()); + fmt::print("best sin plane: {}", somesinplane.wave_param()); steps = callback->consume(); steps_sin = callback_sin->consume(); @@ -1603,7 +1554,7 @@ int main(int argc, const char** argv) { } } - print("DELIMITER{}\n", serialized.dump()); + fmt::println("DELIMITER{}", serialized.dump()); } else{ @@ -1665,6 +1616,6 @@ int main(int argc, const char** argv) { } } - print("DELIMITER{}\n", serialized.dump()); + fmt::println("DELIMITER{}", serialized.dump()); } } diff --git a/subprojects/fmt.wrap b/subprojects/fmt.wrap index 89661cc..237b934 100644 --- a/subprojects/fmt.wrap +++ b/subprojects/fmt.wrap @@ -1,13 +1,13 @@ [wrap-file] -directory = fmt-11.1.1 -source_url = https://github.com/fmtlib/fmt/archive/11.1.1.tar.gz -source_filename = fmt-11.1.1.tar.gz -source_hash = 482eed9efbc98388dbaee5cb5f368be5eca4893456bb358c18b7ff71f835ae43 -patch_filename = fmt_11.1.1-2_patch.zip -patch_url = https://wrapdb.mesonbuild.com/v2/fmt_11.1.1-2/get_patch -patch_hash = eee2e90d5d43061a0a1f0b9f8eb188c5b8820ef3e1b15e4b8a4eb791ef82b325 -source_fallback_url = https://github.com/mesonbuild/wrapdb/releases/download/fmt_11.1.1-2/fmt-11.1.1.tar.gz -wrapdb_version = 11.1.1-2 +directory = fmt-11.1.4 +source_url = https://github.com/fmtlib/fmt/archive/11.1.4.tar.gz +source_filename = fmt-11.1.4.tar.gz +source_hash = ac366b7b4c2e9f0dde63a59b3feb5ee59b67974b14ee5dc9ea8ad78aa2c1ee1e +patch_filename = fmt_11.1.4-1_patch.zip +patch_url = https://wrapdb.mesonbuild.com/v2/fmt_11.1.4-1/get_patch +patch_hash = 213b395a95502e02d950315f49eb2e29bc9ac3a91bacc4610ccd251a97530957 +source_fallback_url = https://github.com/mesonbuild/wrapdb/releases/download/fmt_11.1.4-1/fmt-11.1.4.tar.gz +wrapdb_version = 11.1.4-1 [provide] fmt = fmt_dep From 5c069cc385673f6738deea3ab3308334b32c3f68 Mon Sep 17 00:00:00 2001 From: Christoph Liebender Date: Wed, 30 Apr 2025 16:07:27 +0200 Subject: [PATCH 3/7] implement experimental intersection with newto --- schnell.cpp | 111 ++++++++++++---------------------------------------- viswrap.py | 22 +---------- 2 files changed, 25 insertions(+), 108 deletions(-) diff --git a/schnell.cpp b/schnell.cpp index 78792e1..a780bf9 100644 --- a/schnell.cpp +++ b/schnell.cpp @@ -499,102 +499,42 @@ struct SinusoidalWaveSurface { } }; -// Residual for the intersection of a line with a sinusoidal wave surface -template -struct IntersectionResidual { - Line line_d; - SinusoidalWaveSurface surface_d; - - IntersectionResidual(const Line& l, const SinusoidalWaveSurface& s) - :line_d(l), surface_d(s) {} - - template - bool operator()(const J* const xy, J* residuals) const { - J x = xy[0]; - J y = xy[1]; - - // Convert stored double-typed objects to Jet-typed - Line line = line_d.template cast(); - SinusoidalWaveSurface surface = surface_d.template cast(); - - // Calculate surface point (Evaluate the wave surface at (x, y)) - Vector3 p = surface.evaluate(x, y); - Vector3 o = line.pt; // Point on line - Vector3 d = line.dir; // Direction of line - - // Project p onto the line direction to find the closest point - J t = (p - o).dot(d); - Vector3 proj = o + t * d; - - // Calculate difference between point on the surface and the projection - Vector3 diff = p - proj; - - // Set residuals (difference in 3D space) - residuals[0] = diff.x(); - residuals[1] = diff.y(); - residuals[2] = diff.z(); - - return true; - } -}; - /** * @brief Computes the intersection point of a line with a sinusoidal wave surface. * * @param line The line object represented as a parameterized line in 3D space. * @param surface The sinusoidal wave surface object to intersect with. * @param isec Output parameter to store the computed intersection point in 3D space. - * @return true If the intersection is successfully computed and within a reasonable distance. - * @return false If the intersection is too far away or the computation fails. */ template -bool intersectWithSinPlane(Line line, SinusoidalWaveSurface surface, Vector3& isec) { - - // since only solution with double: - const SinusoidalWaveSurface tempplane(surface.jet_param()); - const Line templine(line.getLineJet()); +Vector3 intersectWithSinPlane(const Line &line, const SinusoidalWaveSurface &surface) { // Initial guess for (x, y) parameters to evaluate on the surface - double xy[2] = {0.0, 0.0}; - - ceres::Problem problem; - - // Add the residual block - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction, 3, 2>( - new IntersectionResidual{templine, tempplane}, - ceres::Ownership::TAKE_OWNERSHIP - ), - nullptr, // no loss function - xy // the (x, y) on the surface - ); + // TODO choose initial guess as intersection of flat plane with ray + T x {0}, y {0}, + limit { 1e-3 }, distance { DBL_MAX }; + constexpr size_t STEPS = 10000; - // TODO: see if better parameter can be chosen - ceres::Solver::Options options; - options.minimizer_type = ceres::MinimizerType::TRUST_REGION; - options.linear_solver_type = ceres::LinearSolverType::DENSE_QR; - options.minimizer_progress_to_stdout = true; - options.logging_type = ceres::SILENT; - options.use_explicit_schur_complement = true; - options.update_state_every_iteration = true; - options.function_tolerance = 1e-30; - options.gradient_tolerance = 1e-30; - options.parameter_tolerance = 1e-30; - options.max_num_iterations = 100; - options.num_threads = sysconf(_SC_NPROCESSORS_ONLN); + Vector3 proj; + size_t i; + for (i = 0; i < STEPS && distance > limit; ++i) { + Vector3 p = surface.evaluate(x, y); + Vector3 o = line.pt; // Point on line + Vector3 d = line.dir; // Direction of line - ceres::Solver::Summary summary; - ceres::Solve(options, &problem, &summary); - - // print("isec sum: {}\n", summary.FullReport()); + // Project p onto the line direction to find the closest point + T t = (p - o).dot(d); + proj = o + t * d; - // Get the resulting point on the surface - Vector3 intersection = tempplane.evaluate(xy[0], xy[1]); + // Calculate difference between point on the surface and the projection + distance = (p - proj).norm(); - isec = intersection.template cast(); + x = proj.x(); + y = proj.y(); + } - return true; + return proj; } template @@ -743,15 +683,12 @@ bool forward_refract_estimate_sin( Vector3& lestimate, Vector3& restimate, Vector3& lisect, - Vector3& risect) - { - + Vector3& risect +) { Line lline(pt - T0, T0), rline(pt - T1, T1); - if (!intersectWithSinPlane(lline, plane, lisect)) - return false; - if (!intersectWithSinPlane(rline, plane, risect)) - return false; + lisect = intersectWithSinPlane(lline, plane); + risect = intersectWithSinPlane(rline, plane); // print("lisect: {}\n", lisect); // print("lline dir: {}\n", lline.dir); diff --git a/viswrap.py b/viswrap.py index 0ebda61..9d3ffa0 100755 --- a/viswrap.py +++ b/viswrap.py @@ -15,27 +15,7 @@ datadir = os.environ["HOME"] + '/Documents/schnell/Daten/wasserkiste/foureyes/april' -def parser(): - - sin = False - - if len(sys.argv) < 2: - - sin = False - - else: - - # get the arguments to the wrigth variabales - for elements in sys.argv[1:]: - - print(elements) - - if(elements == "--sin" ): - sin = True - - return sin - -SIN = parser() +SIN = '--sin' in sys.argv[1:] schnell = subprocess.Popen( ['builddir/schnell', datadir] + sys.argv, From 46edecccac41af1ffa934c66f7a9f643c5152c87 Mon Sep 17 00:00:00 2001 From: Pascal Meyer Date: Thu, 1 May 2025 12:44:06 +0200 Subject: [PATCH 4/7] working sinusoidal wave fit for a flat surface -> not tested for waves --- .gitignore | 1 + README.md | 32 +++++++++ schnell.cpp | 136 ++++++++++++++++++++++++++++---------- sinwave.py | 144 +++++++++++++++++++++++++++++++++++++++++ subprojects/.gitignore | 1 + viswrap.py | 8 +-- 6 files changed, 283 insertions(+), 39 deletions(-) create mode 100644 .gitignore create mode 100644 README.md create mode 100644 sinwave.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..313c9b1 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +Daten/* \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..4fd03cf --- /dev/null +++ b/README.md @@ -0,0 +1,32 @@ +# Run four cameras + +to compile use: + +```sh +meson setup builddir +meson compile -C builddir +``` + +then run the program with: + +```sh +python3 viswrap.py +``` + +each argument needs to be passed with ```--sift``` as example + +# example + +Example for a flat water surface with 4 cameras: + +```sh +python3 viswrap.py --solve --sift --lone 0.4 +``` + +# arguments + +| Argument | Description | Type | +|------------|---------------------------------|--------| +| `--sin` | Enables sine processing | Flag | +| `--lone` | Sets the lone threshold value | Float | +| `--solve` | Enable the ceres solver | Flag | \ No newline at end of file diff --git a/schnell.cpp b/schnell.cpp index a780bf9..d4cd1b5 100644 --- a/schnell.cpp +++ b/schnell.cpp @@ -499,42 +499,108 @@ struct SinusoidalWaveSurface { } }; +// methode befor using Newtown gauss iteration -> this one is not correct little offset for test point +// template +// Vector3 intersectWithSinPlane(const Line &line, const SinusoidalWaveSurface &surface) { + +// // Initial guess for (x, y) parameters to evaluate on the surface +// // choose initial guess as intersection of flat plane with ray + +// T x {0}, y {0}, limit { 1e-4 }, distance { DBL_MAX }; +// constexpr size_t STEPS = 10000; + +// Vector3 proj; + +// size_t i; +// for (i = 0; i < STEPS && distance > limit; ++i) { + +// Vector3 p = surface.evaluate(x, y); +// Vector3 o = line.pt; // Point on line +// Vector3 d = line.dir; // Direction of line + +// // Project p onto the line direction to find the closest point +// T t = (p - o).dot(d); +// proj = o + t * d; + +// // Calculate difference between point on the surface and the projection +// distance = (p - proj).norm(); + +// x = proj.x(); +// y = proj.y(); +// } + +// return proj; +// } + /** * @brief Computes the intersection point of a line with a sinusoidal wave surface. * * @param line The line object represented as a parameterized line in 3D space. * @param surface The sinusoidal wave surface object to intersect with. - * @param isec Output parameter to store the computed intersection point in 3D space. */ template -Vector3 intersectWithSinPlane(const Line &line, const SinusoidalWaveSurface &surface) { +Vector3 intersectWithSinPlane(const Line& line, const SinusoidalWaveSurface& surface) { + + // Iteration of gauss newton algo see: https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm - // Initial guess for (x, y) parameters to evaluate on the surface - // TODO choose initial guess as intersection of flat plane with ray - T x {0}, y {0}, - limit { 1e-3 }, distance { DBL_MAX }; + T x {0}, y {0}, limit { 1e-4 }; constexpr size_t STEPS = 10000; - Vector3 proj; - - size_t i; - for (i = 0; i < STEPS && distance > limit; ++i) { + for (size_t iter = 0; iter < STEPS; ++iter) { + // Surface point Vector3 p = surface.evaluate(x, y); - Vector3 o = line.pt; // Point on line - Vector3 d = line.dir; // Direction of line - // Project p onto the line direction to find the closest point + // Line direction and origin + Vector3 d = line.dir.normalized(); + Vector3 o = line.pt; + + // Closest point on line to surface point T t = (p - o).dot(d); - proj = o + t * d; + Vector3 q = o + t * d; + + // Residual + // r(x, y) = surface(x, y) - project_onto_line(surface(x, y)) + Vector3 r = p - q; + + if (r.norm() < limit) break; + + // Compute df/dx and df/dy (Jacobian of p(x, y)) + Vector3 du = surface.u; + Vector3 dv = surface.v; + T k = surface.frequency; + T phase = surface.phase; + T A = surface.amplitude; + Vector3 n = surface.normal; + + T arg = k * x + k * y + phase; + T d_sin = A * k * ceres::cos(arg); + + Vector3 dpdx = du + d_sin * n; + Vector3 dpdy = dv + d_sin * n; - // Calculate difference between point on the surface and the projection - distance = (p - proj).norm(); + // Jacobian J = [dpdx - d*(d⋅dpdx), dpdy - d*(d⋅dpdy)] + // because q(x, y) = o + ((p - o)⋅d) * d, and we need df = dp - dq + Vector3 dqdx = d * dpdx.dot(d); + Vector3 dqdy = d * dpdy.dot(d); - x = proj.x(); - y = proj.y(); + Vector3 drdx = dpdx - dqdx; + Vector3 drdy = dpdy - dqdy; + + // Build system: J * delta = -r + Eigen::Matrix J; + J.col(0) = drdx; + J.col(1) = drdy; + + // Solve least-squares (since J isn't square) + Eigen::JacobiSVD> svd(J, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix delta = svd.solve(-r); + + + x += delta(0); + y += delta(1); } - return proj; + return surface.evaluate(x, y); } template @@ -1068,10 +1134,10 @@ int main(int argc, const char** argv) { DetectionType detection_type = args.count("sift") ? DetectionType::SIFT : DetectionType::APRIL; std::string datapath = args["datapath"].as(); - fmt::print(stderr, "data path : {}", datapath); - fmt::print(stderr, "type of detection: {}", args.count("sift") ? "sift" : "apriltag"); - fmt::print(stderr, "lone: {}", args["lone"].as()); - fmt::print(stderr, "sin: {}", args.count("sin") ? "optimizing sin plane" : "optimizing flat plane"); + fmt::println(stderr, "data path : {}", datapath); + fmt::println(stderr, "type of detection: {}", args.count("sift") ? "sift" : "apriltag"); + fmt::println(stderr, "lone: {}", args["lone"].as()); + fmt::println(stderr, "sin: {}", args.count("sin") ? "optimizing sin plane" : "optimizing flat plane"); StereoMap combos; for (size_t i = 0; i < camidxs.size(); ++i) { @@ -1174,18 +1240,18 @@ int main(int argc, const char** argv) { fmt::println("fist sin guess: {}", somesinplane.wave_param()); // //NOTE: HERE ----------------------------------- - // Vector3 intersections; - // SinusoidalWaveSurface wave_surface = { - // {0.1522135796926206, 0.07386223835536908, 0.2721447693021134}, // origin - // {-0.8779893174123757, 0.11769968478070637, 0.46398442076461244}, // u -> propagation is in this direction - // {-0.007492214882922366, 0.9658229358896685, -0.25909442916745534}, // v - // {0.47911459271736, 0.23195390399035876, 0.8465498174761541}, // normal - // 0, 0, 0 // amplitude, frequency, phase - // }; - // Line line({0, 0, 1}, {0,1,1}); // vec , origin - // // print("sin wave: {}\n", wave_surface.wave_param()); - // bool res = intersectWithSinPlane(line, wave_surface, intersections); - // print("intersection: {}\n", intersections); + Vector3 intersections; + SinusoidalWaveSurface wave_surface = { + {0.1522135796926206, 0.07386223835536908, 0.2721447693021134}, // origin + {-0.8779893174123757, 0.11769968478070637, 0.46398442076461244}, // u -> propagation is in this direction + {-0.007492214882922366, 0.9658229358896685, -0.25909442916745534}, // v + {0.47911459271736, 0.23195390399035876, 0.8465498174761541}, // normal + 0, 0, 0 // amplitude, frequency, phase + }; + Line line({0, 1, 1}, {0,0,1}); // vec , origin + // fmt::println("sin wave: {}\n", wave_surface.wave_param()); + intersections = intersectWithSinPlane(line, wave_surface); + fmt::println("intersection: {}\n", intersections); // //NOTE: HERE ----------------------------------- std::vector> steps; diff --git a/sinwave.py b/sinwave.py new file mode 100644 index 0000000..e614efd --- /dev/null +++ b/sinwave.py @@ -0,0 +1,144 @@ +import numpy as np +from vispy import scene, app +from vispy.geometry import MeshData +from scipy.optimize import least_squares + +# === Define the sinusoidal surface parameters === + +# origin = np.array([0.0, 0.0, 1.0]) +# u = np.array([1.0, 0.0, 0.0]) +# v = np.array([0.0, 1.0, 0.0]) +# normal = np.array([0.0, 0.0, 1.0]) +# amplitude = 0.5 +# frequency = 5 +# phase = np.pi / 2 + +plane_param = np.array([0.1522135796926206, 0.07386223835536908, 0.2721447693021134, -0.8779893174123757, 0.11769968478070637, 0.46398442076461244, -0.007492214882922366, 0.9658229358896685, -0.25909442916745534, 0.47911459271736, 0.23195390399035876, 0.8465498174761541, 0, 0, 0]) +xy_guess = np.array([0.17852059308677284, 0.-0.6040395325557387]) + + +origin = np.array(plane_param[:3]) +u = np.array(plane_param[3:6]) +v = np.array(plane_param[6:9]) +normal = np.array(plane_param[9:12]) +amplitude = plane_param[12] +frequency = plane_param[13] +phase = plane_param[14] + +line_pt = np.array([0,0,1]) +line_vec = np.array([0,1,1]) + +# === Function to evaluate the surface at (x, y) === +def evaluate(x, y): + base = origin + x * u + y * v + wave = amplitude * np.sin(frequency * x + phase) + return base + wave * normal + +def intersect_line_with_surface(initial_guess=(0.0, 0.0)): + + global line_pt, line_vec + + line_pt = np.array(line_pt) + line_vec = np.array(line_vec) + line_vec = line_vec / np.linalg.norm(line_vec) # normalize + + def residual(xy): + x, y = xy + surface_point = evaluate(x, y) + o = line_pt + d = line_vec + + # Project surface point onto line + t = np.dot(surface_point - o, d) + closest_point = o + t * d + + diff = surface_point - closest_point + + # print(f"Surface point: {surface_point}, Closest point: {closest_point}, Diff: {diff}") + + return diff # 3 residuals: (dx, dy, dz) + + result = least_squares(residual, initial_guess) + + if result.success: + x_opt, y_opt = result.x + intersection_point = evaluate(x_opt, y_opt) + return intersection_point + else: + raise RuntimeError("Intersection optimization failed!") + +# === Generate a grid of points === +res = 100 +x_vals = np.linspace(-1, 1, res) +y_vals = np.linspace(-1, 1, res) +X, Y = np.meshgrid(x_vals, y_vals) +Z = np.zeros_like(X) + +# Compute 3D surface points +points = np.zeros((res, res, 3), dtype=np.float32) +for i in range(res): + for j in range(res): + points[i, j] = evaluate(X[i, j], Y[i, j]) + +# Flatten the grid for VisPy +vertices = points.reshape(-1, 3) + +# Build face indices +faces = [] +for i in range(res - 1): + for j in range(res - 1): + idx = i * res + j + faces.append([idx, idx + 1, idx + res]) + faces.append([idx + 1, idx + res + 1, idx + res]) +faces = np.array(faces) + +# === Setup VisPy canvas === +canvas = scene.SceneCanvas(keys='interactive', bgcolor='white', show=True) +view = canvas.central_widget.add_view() +view.camera = 'turntable' + +isec = np.array(intersect_line_with_surface()) +print("isec = ", isec) +marker = scene.visuals.Markers(parent=view.scene) +marker.set_data( + pos=np.array([isec]), # Marker position + face_color='orange', + size=10 +) + +# Your line data (two points stacked into shape (2, 3)) +dist = np.vstack([line_pt, line_pt + line_vec]) +# Create and configure the line +line = scene.visuals.Line( + pos=dist, + color='red', + width=3, + parent=view.scene, + # method='gl' # Optional: 'gl' for smooth GPU rendering +) + +# print("guess: ", evaluate(xy_guess[0], xy_guess[1]), "diff =", evaluate(xy_guess[0], xy_guess[1]) - isec) +# marker = scene.visuals.Markers(parent=view.scene) +# marker.set_data( +# pos=np.array([evaluate(xy_guess[0], xy_guess[1])]), # Marker position +# face_color='green', +# size=10 +# ) + +# print("guess: ", evaluate(xy_guess[0], xy_guess[1]), "diff =", evaluate(xy_guess[0], xy_guess[1]) - isec) +marker = scene.visuals.Markers(parent=view.scene) +marker.set_data( + pos=np.array([[0, 1, 0.013050340134658]]), # Marker position + face_color='green', + size=10 +) + +# === Create mesh visual === +mesh = scene.visuals.Mesh(vertices=vertices, faces=faces, color=(0.5, 0.7, 1, 1), shading='smooth') +view.add(mesh) + +# Add axis for orientation +axis = scene.visuals.XYZAxis(parent=view.scene) + +# Run the app +app.run() diff --git a/subprojects/.gitignore b/subprojects/.gitignore index 10ee1c5..c675574 100644 --- a/subprojects/.gitignore +++ b/subprojects/.gitignore @@ -1,2 +1,3 @@ packagecache CCTag-1.0.4 +fmt-11.1.4 diff --git a/viswrap.py b/viswrap.py index 9d3ffa0..54fc7f8 100755 --- a/viswrap.py +++ b/viswrap.py @@ -13,7 +13,7 @@ os.environ['QT_QPA_PLATFORM'] = 'xcb' -datadir = os.environ["HOME"] + '/Documents/schnell/Daten/wasserkiste/foureyes/april' +datadir = os.environ["HOME"] + '/Documents/schnell-git/Daten/wasserkiste/foureyes/april' SIN = '--sin' in sys.argv[1:] @@ -32,9 +32,9 @@ print(stdout) -output_path ="output.json" -with open(output_path, "w") as f: - f.write(jsondump) +# output_path ="output.json" +# with open(output_path, "w") as f: +# f.write(jsondump) data = json.loads(jsondump) steps = data["steps"] From 0dbcbf773d4f12d8b086791dab4fd3699e9f15b8 Mon Sep 17 00:00:00 2001 From: Pascal Meyer Date: Tue, 13 May 2025 17:19:00 +0200 Subject: [PATCH 5/7] added data --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 313c9b1..0146420 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ -Daten/* \ No newline at end of file +Daten/* +daten/* \ No newline at end of file From 174fbbd28ec937a4b3f05b2ea4f961b13d28e85e Mon Sep 17 00:00:00 2001 From: Pascal Meyer Date: Tue, 13 May 2025 17:19:27 +0200 Subject: [PATCH 6/7] added data --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 0146420..135ad25 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,2 @@ Daten/* -daten/* \ No newline at end of file +data/* \ No newline at end of file From 188c8eca3beffa281108b1604735b0a987baf7e3 Mon Sep 17 00:00:00 2001 From: Pascal Meyer Date: Tue, 13 May 2025 17:27:51 +0200 Subject: [PATCH 7/7] added arduino code --- .../basicStepperDriver/basicStepperDriver.ino | 183 +++++++++ .../Arduino/libraries/StepperDriver/LICENSE | 22 + .../Arduino/libraries/StepperDriver/Makefile | 57 +++ .../Arduino/libraries/StepperDriver/README.md | 124 ++++++ .../libraries/StepperDriver/_config.yml | 1 + .../libraries/StepperDriver/arduino-cli.yaml | 11 + .../examples/AccelTest/AccelTest.ino | 94 +++++ .../BasicStepperDriver/BasicStepperDriver.ino | 60 +++ .../examples/ClockStepper/ClockStepper.ino | 70 ++++ .../examples/MicroStepping/MicroStepping.ino | 101 +++++ .../examples/MultiAxis/MultiAxis.ino | 67 ++++ .../examples/NonBlocking/NonBlocking.ino | 106 +++++ .../examples/SpeedProfile/SpeedProfile.ino | 76 ++++ .../examples/UnitTest/UnitTest.ino | 163 ++++++++ .../examples/UnitTest/adafruit_feather_m0.txt | 56 +++ .../examples/UnitTest/esp8266_nodemcu.txt | 58 +++ .../libraries/StepperDriver/keywords.txt | 28 ++ .../StepperDriver/library.properties | 9 + .../libraries/StepperDriver/platformio.ini | 49 +++ .../libraries/StepperDriver/src/A4988.cpp | 95 +++++ .../libraries/StepperDriver/src/A4988.h | 56 +++ .../StepperDriver/src/BasicStepperDriver.cpp | 379 ++++++++++++++++++ .../StepperDriver/src/BasicStepperDriver.h | 251 ++++++++++++ .../libraries/StepperDriver/src/DRV8825.cpp | 49 +++ .../libraries/StepperDriver/src/DRV8825.h | 43 ++ .../libraries/StepperDriver/src/DRV8834.cpp | 85 ++++ .../libraries/StepperDriver/src/DRV8834.h | 48 +++ .../libraries/StepperDriver/src/DRV8880.cpp | 121 ++++++ .../libraries/StepperDriver/src/DRV8880.h | 63 +++ .../StepperDriver/src/MultiDriver.cpp | 149 +++++++ .../libraries/StepperDriver/src/MultiDriver.h | 121 ++++++ .../StepperDriver/src/SyncDriver.cpp | 43 ++ .../libraries/StepperDriver/src/SyncDriver.h | 26 ++ .../libraries/StepperDriver/test/README | 11 + WaveGen/README.md | 41 ++ WaveGen/WaveGen.py | 86 ++++ 36 files changed, 3002 insertions(+) create mode 100644 WaveGen/Arduino/basicStepperDriver/basicStepperDriver.ino create mode 100644 WaveGen/Arduino/libraries/StepperDriver/LICENSE create mode 100644 WaveGen/Arduino/libraries/StepperDriver/Makefile create mode 100644 WaveGen/Arduino/libraries/StepperDriver/README.md create mode 100644 WaveGen/Arduino/libraries/StepperDriver/_config.yml create mode 100644 WaveGen/Arduino/libraries/StepperDriver/arduino-cli.yaml create mode 100644 WaveGen/Arduino/libraries/StepperDriver/examples/AccelTest/AccelTest.ino create mode 100644 WaveGen/Arduino/libraries/StepperDriver/examples/BasicStepperDriver/BasicStepperDriver.ino create mode 100644 WaveGen/Arduino/libraries/StepperDriver/examples/ClockStepper/ClockStepper.ino create mode 100644 WaveGen/Arduino/libraries/StepperDriver/examples/MicroStepping/MicroStepping.ino create mode 100644 WaveGen/Arduino/libraries/StepperDriver/examples/MultiAxis/MultiAxis.ino create mode 100644 WaveGen/Arduino/libraries/StepperDriver/examples/NonBlocking/NonBlocking.ino create mode 100644 WaveGen/Arduino/libraries/StepperDriver/examples/SpeedProfile/SpeedProfile.ino create mode 100644 WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/UnitTest.ino create mode 100644 WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/adafruit_feather_m0.txt create mode 100644 WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/esp8266_nodemcu.txt create mode 100644 WaveGen/Arduino/libraries/StepperDriver/keywords.txt create mode 100644 WaveGen/Arduino/libraries/StepperDriver/library.properties create mode 100644 WaveGen/Arduino/libraries/StepperDriver/platformio.ini create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/A4988.cpp create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/A4988.h create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/BasicStepperDriver.cpp create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/BasicStepperDriver.h create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/DRV8825.cpp create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/DRV8825.h create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/DRV8834.cpp create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/DRV8834.h create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/DRV8880.cpp create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/DRV8880.h create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/MultiDriver.cpp create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/MultiDriver.h create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/SyncDriver.cpp create mode 100644 WaveGen/Arduino/libraries/StepperDriver/src/SyncDriver.h create mode 100644 WaveGen/Arduino/libraries/StepperDriver/test/README create mode 100644 WaveGen/README.md create mode 100644 WaveGen/WaveGen.py diff --git a/WaveGen/Arduino/basicStepperDriver/basicStepperDriver.ino b/WaveGen/Arduino/basicStepperDriver/basicStepperDriver.ino new file mode 100644 index 0000000..3fad2c8 --- /dev/null +++ b/WaveGen/Arduino/basicStepperDriver/basicStepperDriver.ino @@ -0,0 +1,183 @@ +#include +#include "BasicStepperDriver.h" + +// motor : https://www.omc-stepperonline.com/de/p-series-ip67-wasserdicht-nema-23-schrittmotor-5-0a-1-8nm-254-95oz-in-23ip67-20 +// stepper controller: https://www.omc-stepperonline.com/de/digitaler-schrittmotortreiber-1-0-4-2a-20-50vdc-fuer-nema-17-23-24-schrittmotor-dm542t + +// the motor has a 1.8 deg step so 200 steps +#define MOTOR_STEPS 200 + +// set a safty RPM +#define SAFTY_RPM 600 + +// step resolution on stepper controller set to 800 step/rev +#define MICROSTEPS 4 + +// All the wires needed for full functionality +#define DIR 4 +#define STEP 3 +#define ENABLE 2 + +//Uncomment line to use enable/disable functionality +//#define SLEEP 13 + +// set all value to default +float rpmVal = 60.0; +float timeVal = 0.0; +int angleVal = 0; +int start_delay = 0; + +unsigned long start_time = 0; + +// start bsic driver +BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP, ENABLE); + +void setup() { + + // enable serial + Serial.begin(9600); + while (!Serial) {;} // Wait for serial port to connect + Serial.println("Found serial connection of wavegen"); + + stepper.begin(rpmVal, MICROSTEPS); + stepper.setEnableActiveState(LOW); + + stepper.disable(); + +} + +void loop(){ + + // parse the message from python + if (Serial.available() > 0) { + + // read in line + String input = Serial.readStringUntil('\n'); + input.trim(); + + // Split command and value + int spaceIndex = input.indexOf(' '); + String command = input; + int value = 0; + + // Split string into tokens + const int maxTokens = 10; + String tokens[maxTokens]; + int tokenCount = 0; + + // sort the messages + while (input.length() > 0 && tokenCount < maxTokens) { + int spaceIndex = input.indexOf(' '); + if (spaceIndex == -1) { + + tokens[tokenCount++] = input; + break; + + } else { + + tokens[tokenCount++] = input.substring(0, spaceIndex); + input = input.substring(spaceIndex + 1); + input.trim(); + + } + } + + // Parse name/value pairs + for (int i = 0; i < tokenCount - 1; i += 2) { + String name = tokens[i]; + float value = tokens[i + 1].toFloat(); + + if (name == "a") { + angleVal = tokens[i + 1].toInt(); + Serial.print("Set angle to "); + Serial.println(angleVal); + + } else if (name == "t") { + + // time in ms + timeVal = value; + Serial.print("Set time to "); + Serial.println(timeVal); + + } else if (name == "rpm") { + + if(value > SAFTY_RPM){ + rpmVal = SAFTY_RPM; + } + else if(value != 0.0){ + rpmVal = value; + } + + // set the new rpm value + stepper.begin(rpmVal, MICROSTEPS); + + Serial.print("Set rpm to "); + Serial.println(rpmVal); + + } else if(name == "delay"){ + + start_delay = value; + Serial.print("Delay before start: "); + Serial.println(start_delay); + + } else { + + Serial.print("Unknown parameter: "); + Serial.println(name); + } + } + + delay(start_delay * 1000); // in sec + Serial.println("Starting"); + + } + + + if(angleVal != 0){ + Serial.print("start rotation with angle and rpm: "); + Serial.print(angleVal); + Serial.print(" "); + Serial.println(rpmVal); + + stepper.enable(); + // if no rpm given the rotation speed is set to default + stepper.rotate(angleVal); + + stepper.disable(); + angleVal = 0; + rpmVal = 60.0; + + Serial.println("Finished"); + + } + + // only set in rpm if the angle is 0 -> else do angular rotation + if(rpmVal != 60.0 && angleVal == 0){ + + Serial.print("rotating at rpm "); + Serial.println(rpmVal); + + // set the new rpm value + stepper.setRPM(rpmVal); + + if(start_time = 0){ + start_time = millis(); + } + + stepper.rotate(360); + } + + if(timeVal != 0.0){ + //Serial.println(millis() - start_time); + if(millis() - start_time > timeVal){ + // set RPM and set time to 0 + timeVal = 0.0; + rpmVal = 60.0; + stepper.disable(); + + Serial.println("Stopped motor due to time"); + Serial.println("Finished"); + } + } + +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/LICENSE b/WaveGen/Arduino/libraries/StepperDriver/LICENSE new file mode 100644 index 0000000..32af83b --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/LICENSE @@ -0,0 +1,22 @@ +The MIT License (MIT) + +Copyright (c) 2015 Laurentiu Badea + +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. + diff --git a/WaveGen/Arduino/libraries/StepperDriver/Makefile b/WaveGen/Arduino/libraries/StepperDriver/Makefile new file mode 100644 index 0000000..da78b41 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/Makefile @@ -0,0 +1,57 @@ +# Default build architecture and board +TARGET ?= arduino:avr:uno +CORE = $(shell echo $(TARGET) | cut -d: -f1,2) + +# Where to save the Arduino support files, this should match what is in arduino-cli.yaml +ARDUINO_DIR ?= .arduino + +default: + ################################################################################################# + # Initial setup: make .arduino/arduino-cli setup + # + # Build all the examples: make all TARGET=adafruit:samd:adafruit_feather_m0 + # + # Install more cores: make core TARGET=adafruit:samd:adafruit_feather_m0 + # (edit arduino-cli.yaml and add repository if needed) + ################################################################################################# + +# See https://arduino.github.io/arduino-cli/installation/ +ARDUINO_CLI_URL = https://downloads.arduino.cc/arduino-cli/arduino-cli_latest_Linux_64bit.tar.gz +ARDUINO_CLI ?= $(ARDUINO_DIR)/arduino-cli --config-file arduino-cli.yaml +EXAMPLES := $(shell ls examples) + +COMPILE = $(ARDUINO_CLI) compile --warnings all --fqbn $(TARGET) + +all: # Build all example sketches +all: $(EXAMPLES:%=%.hex) + ls -l build + +%.hex: # Generic rule for compiling sketch to uploadable hex file +%.hex: examples/% core + $(ARDUINO_CLI) compile --warnings all --fqbn $(TARGET) --output-dir build $< + +# Remove built objects +clean: + rm -rfv build + +core: $(ARDUINO_DIR)/arduino-cli + $(ARDUINO_CLI) core install $(CORE) + +$(ARDUINO_DIR)/arduino-cli: # Download and install arduino-cli +$(ARDUINO_DIR)/arduino-cli: + mkdir -p $(ARDUINO_DIR) + cd $(ARDUINO_DIR) + curl -L -s $(ARDUINO_CLI_URL) \ + | tar xfz - -C $(ARDUINO_DIR) arduino-cli + chmod 755 $@ + $(ARDUINO_CLI) version + +setup: # Configure cores and libraries for arduino-cli (which it will download if missing) +setup: $(ARDUINO_DIR)/arduino-cli + mkdir -p $(ARDUINO_DIR)/libraries + ln -sf $(CURDIR) $(ARDUINO_DIR)/libraries/ + $(ARDUINO_CLI) config dump + $(ARDUINO_CLI) core update-index + $(ARDUINO_CLI) core list + +.PHONY: clean %.hex all setup diff --git a/WaveGen/Arduino/libraries/StepperDriver/README.md b/WaveGen/Arduino/libraries/StepperDriver/README.md new file mode 100644 index 0000000..9f26d22 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/README.md @@ -0,0 +1,124 @@ +[![arduino-library-badge](https://www.ardu-badge.com/badge/StepperDriver.svg?)](https://www.ardu-badge.com/StepperDriver) +[![Actions Status](https://github.com/laurb9/StepperDriver/workflows/PlatformIO/badge.svg)](https://github.com/laurb9/StepperDriver/actions) +[![Actions Status](https://github.com/laurb9/StepperDriver/workflows/Arduino/badge.svg)](https://github.com/laurb9/StepperDriver/actions) + +StepperDriver +============= + +A4988, DRV8825, DRV8834, DRV8880 and generic two-pin stepper motor driver library. +Features: + - Constant speed mode (low rpms) + - Linear (accelerated) speed mode, with separate acceleration and deceleration settings. + - Non-blocking mode (yields back to caller after each pulse) + - Early brake / increase runtime in non-blocking mode + +Hardware currently supported: + - DRV8834 Low-Voltage Stepper Motor Driver + up to 1:32 + - A4988 Stepper Motor Driver up to 1:16 + - DRV8825 up to 1:32 + - DRV8880 up to 1:16, with current/torque control + - any other 2-pin stepper via DIR and STEP pins, microstepping up to 1:128 externally set + +Microstepping +============= + +The library can set microstepping and generate the signals for each of the support driver boards. + +High RPM plus high microstep combinations may not work correctly on slower MCUs, there is a maximum speed +achieveable for each board, especially with acceleration on multiple motors at the same time. + +Motors +====== + +- 4-wire bipolar stepper motor or +- some 6-wire unipolar in 4-wire configuration (leaving centers out) or +- 28BYJ-48 (commonly available) with a small modification (search for "convert 28byj-48 to 4-wire"). + +Connections +=========== + +Minimal configuration from Pololu DRV8834 page: + + + +Wiring +====== + +This is suggested wiring for running the examples unmodified. All the pins below can be changed. + +- Arduino to driver board: + - DIR - D8 + - STEP - D9 + - GND - Arduino GND + - GND - Motor power GND + - VMOT - Motor power (check driver-specific voltage range) + - A4988/DRV8825 microstep control + - MS1/MODE0 - D10 + - MS2/MODE1 - D11 + - MS3/MODE2 - D12 + - DRV8834/DRV8880 microstep control + - M0 - D10 + - M1 - D11 + - ~SLEEP (optional) D13 + +- driver board to motor (this varies from motor to motor, check motor coils schematic). +- 100uF capacitor between GND - VMOT +- Make sure to set the max current on the driver board to the motor limit (see below). +- Have a motor power supply that can deliver that current. +- Make sure the motor power supply voltage is within the range supported by the driver board. + +Set Max Current +=============== + +The max current is set via the potentiometer on board. +Turn it while measuring voltage at the passthrough next to it. +The formula is V = I*5*R where I=max current, R=current sense resistor installed onboard + +- DRV8834 or DRV8825 Pololu boards, R=0.1 and V = 0.5 * max current(A). + For example, for 1A you will set it to 0.5V. + +For latest info, see the Pololu board information pages. + +Code +==== + +See the BasicStepperDriver example for a generic driver that should work with any board +supporting the DIR/STEP indexing mode. + +The Microstepping example works with a DRV8834 board. + +For example, to show what is possible, here is the ClockStepper example that moves a +stepper motor like the seconds hand of a watch: + +```C++ +#include +#include "A4988.h" + +// using a 200-step motor (most common) +#define MOTOR_STEPS 200 +// configure the pins connected +#define DIR 8 +#define STEP 9 +#define MS1 10 +#define MS2 11 +#define MS3 12 +A4988 stepper(MOTOR_STEPS, DIR, STEP, MS1, MS2, MS3); + +void setup() { + // Set target motor RPM to 1RPM and microstepping to 1 (full step mode) + stepper.begin(1, 1); +} + +void loop() { + // Tell motor to rotate 360 degrees. That's it. + stepper.rotate(360); +} +``` + +Hardware +======== +- Arduino-compatible board +- A stepper motor driver, for example DRV8834, DRV8825, DRV8824, A4988. +- A Stepper Motor. +- 1 x 100uF capacitor diff --git a/WaveGen/Arduino/libraries/StepperDriver/_config.yml b/WaveGen/Arduino/libraries/StepperDriver/_config.yml new file mode 100644 index 0000000..cc35c1d --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/_config.yml @@ -0,0 +1 @@ +theme: jekyll-theme-modernist \ No newline at end of file diff --git a/WaveGen/Arduino/libraries/StepperDriver/arduino-cli.yaml b/WaveGen/Arduino/libraries/StepperDriver/arduino-cli.yaml new file mode 100644 index 0000000..93f1b2b --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/arduino-cli.yaml @@ -0,0 +1,11 @@ +directories: + data: .arduino + downloads: .arduino/staging + user: .arduino +board_manager: + additional_urls: + - http://arduino.esp8266.com/stable/package_esp8266com_index.json + - https://dl.espressif.com/dl/package_esp32_index.json + - https://adafruit.github.io/arduino-board-index/package_adafruit_index.json +telemetry: + enabled: false diff --git a/WaveGen/Arduino/libraries/StepperDriver/examples/AccelTest/AccelTest.ino b/WaveGen/Arduino/libraries/StepperDriver/examples/AccelTest/AccelTest.ino new file mode 100644 index 0000000..1ec4833 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/examples/AccelTest/AccelTest.ino @@ -0,0 +1,94 @@ +/* + * Using accelerated motion ("linear speed") in nonblocking mode + * + * Copyright (C)2015-2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include + +// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step +#define MOTOR_STEPS 200 +// Target RPM for cruise speed +#define RPM 120 +// Acceleration and deceleration values are always in FULL steps / s^2 +#define MOTOR_ACCEL 2000 +#define MOTOR_DECEL 1000 + +// Microstepping mode. If you hardwired it to save pins, set to the same value here. +#define MICROSTEPS 16 + +#define DIR 8 +#define STEP 9 +#define SLEEP 13 // optional (just delete SLEEP from everywhere if not used) + +/* + * Choose one of the sections below that match your board + */ + +#include "DRV8834.h" +#define M0 10 +#define M1 11 +DRV8834 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, M0, M1); + +// #include "A4988.h" +// #define MS1 10 +// #define MS2 11 +// #define MS3 12 +// A4988 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MS1, MS2, MS3); + +// #include "DRV8825.h" +// #define MODE0 10 +// #define MODE1 11 +// #define MODE2 12 +// DRV8825 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MODE0, MODE1, MODE2); + +// #include "DRV8880.h" +// #define M0 10 +// #define M1 11 +// #define TRQ0 6 +// #define TRQ1 7 +// DRV8880 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, M0, M1, TRQ0, TRQ1); + +// #include "BasicStepperDriver.h" // generic +// BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP); + +void setup() { + Serial.begin(115200); + + stepper.begin(RPM, MICROSTEPS); + // if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line + // stepper.setEnableActiveState(LOW); + stepper.enable(); + // set current level (for DRV8880 only). Valid percent values are 25, 50, 75 or 100. + // stepper.setCurrent(100); + + /* + * Set LINEAR_SPEED (accelerated) profile. + */ + stepper.setSpeedProfile(stepper.LINEAR_SPEED, MOTOR_ACCEL, MOTOR_DECEL); + + Serial.println("START"); + /* + * Using non-blocking mode to print out the step intervals. + * We could have just as easily replace everything below this line with + * stepper.rotate(360); + */ + stepper.startRotate(360); +} + +void loop() { + static int step = 0; + unsigned wait_time = stepper.nextAction(); + if (wait_time){ + Serial.print(" step="); Serial.print(step++); + Serial.print(" dt="); Serial.print(wait_time); + Serial.print(" rpm="); Serial.print(stepper.getCurrentRPM()); + Serial.println(); + } else { + stepper.disable(); + Serial.println("END"); + delay(3600000); + } +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/examples/BasicStepperDriver/BasicStepperDriver.ino b/WaveGen/Arduino/libraries/StepperDriver/examples/BasicStepperDriver/BasicStepperDriver.ino new file mode 100644 index 0000000..46b8bc3 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/examples/BasicStepperDriver/BasicStepperDriver.ino @@ -0,0 +1,60 @@ +/* + * Simple demo, should work with any driver board + * + * Connect STEP, DIR as indicated + * + * Copyright (C)2015-2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include +#include "BasicStepperDriver.h" + +// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step +#define MOTOR_STEPS 200 +#define RPM 120 + +// Since microstepping is set externally, make sure this matches the selected mode +// If it doesn't, the motor will move at a different RPM than chosen +// 1=full step, 2=half step etc. +#define MICROSTEPS 1 + +// All the wires needed for full functionality +#define DIR 8 +#define STEP 9 +//Uncomment line to use enable/disable functionality +//#define SLEEP 13 + +// 2-wire basic config, microstepping is hardwired on the driver +BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP); + +//Uncomment line to use enable/disable functionality +//BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP, SLEEP); + +void setup() { + stepper.begin(RPM, MICROSTEPS); + // if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line + // stepper.setEnableActiveState(LOW); +} + +void loop() { + + // energize coils - the motor will hold position + // stepper.enable(); + + /* + * Moving motor one full revolution using the degree notation + */ + stepper.rotate(360); + + /* + * Moving motor to original position using steps + */ + stepper.move(-MOTOR_STEPS*MICROSTEPS); + + // pause and allow the motor to be moved by hand + // stepper.disable(); + + delay(5000); +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/examples/ClockStepper/ClockStepper.ino b/WaveGen/Arduino/libraries/StepperDriver/examples/ClockStepper/ClockStepper.ino new file mode 100644 index 0000000..fd8a66b --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/examples/ClockStepper/ClockStepper.ino @@ -0,0 +1,70 @@ +/* + * Clock Microstepping demo + * + * Moves the stepper motor like the seconds hand of a watch. + * + * Copyright (C)2015-2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include + +// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step +#define MOTOR_STEPS 200 + +// Microstepping mode. If you hardwired it to save pins, set to the same value here. +#define MICROSTEPS 1 + +#define DIR 8 +#define STEP 9 +#define SLEEP 13 // optional (just delete SLEEP from everywhere if not used) + +/* + * Choose one of the sections below that match your board + */ + +#include "DRV8834.h" +#define M0 10 +#define M1 11 +DRV8834 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, M0, M1); + +// #include "A4988.h" +// #define MS1 10 +// #define MS2 11 +// #define MS3 12 +// A4988 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MS1, MS2, MS3); + +// #include "DRV8825.h" +// #define MODE0 10 +// #define MODE1 11 +// #define MODE2 12 +// DRV8825 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MODE0, MODE1, MODE2); + +// #include "DRV8880.h" +// #define M0 10 +// #define M1 11 +// #define TRQ0 6 +// #define TRQ1 7 +// DRV8880 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, M0, M1, TRQ0, TRQ1); + +// #include "BasicStepperDriver.h" // generic +// BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP); + +void setup() { + /* + * Set target motor RPM=1 + */ + stepper.begin(1, MICROSTEPS); + + // if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line + // stepper.setEnableActiveState(LOW); + stepper.enable(); +} + +void loop() { + /* + * The easy way is just tell the motor to rotate 360 degrees at 1rpm + */ + stepper.rotate(360); +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/examples/MicroStepping/MicroStepping.ino b/WaveGen/Arduino/libraries/StepperDriver/examples/MicroStepping/MicroStepping.ino new file mode 100644 index 0000000..aed5393 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/examples/MicroStepping/MicroStepping.ino @@ -0,0 +1,101 @@ +/* + * Microstepping demo + * + * This requires that microstep control pins be connected in addition to STEP,DIR + * + * Copyright (C)2015 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include + +// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step +#define MOTOR_STEPS 200 +#define RPM 120 + +#define DIR 8 +#define STEP 9 +#define SLEEP 13 // optional (just delete SLEEP from everywhere if not used) + +/* + * Choose one of the sections below that match your board + */ + +#include "DRV8834.h" +#define M0 10 +#define M1 11 +DRV8834 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, M0, M1); + +// #include "A4988.h" +// #define MS1 10 +// #define MS2 11 +// #define MS3 12 +// A4988 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MS1, MS2, MS3); + +// #include "DRV8825.h" +// #define MODE0 10 +// #define MODE1 11 +// #define MODE2 12 +// DRV8825 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MODE0, MODE1, MODE2); + +// #include "DRV8880.h" +// #define M0 10 +// #define M1 11 +// #define TRQ0 6 +// #define TRQ1 7 +// DRV8880 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, M0, M1, TRQ0, TRQ1); + +// #include "BasicStepperDriver.h" // generic +// BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP); + +void setup() { + /* + * Set target motor RPM. + */ + stepper.begin(RPM); + // if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line + // stepper.setEnableActiveState(LOW); + stepper.enable(); + + // set current level (for DRV8880 only). + // Valid percent values are 25, 50, 75 or 100. + // stepper.setCurrent(100); +} + +void loop() { + delay(1000); + + /* + * Moving motor in full step mode is simple: + */ + stepper.setMicrostep(1); // Set microstep mode to 1:1 + + // One complete revolution is 360° + stepper.rotate(360); // forward revolution + stepper.rotate(-360); // reverse revolution + + // One complete revolution is also MOTOR_STEPS steps in full step mode + stepper.move(MOTOR_STEPS); // forward revolution + stepper.move(-MOTOR_STEPS); // reverse revolution + + /* + * Microstepping mode: 1, 2, 4, 8, 16 or 32 (where supported by driver) + * Mode 1 is full speed. + * Mode 32 is 32 microsteps per step. + * The motor should rotate just as fast (at the set RPM), + * but movement precision is increased, which may become visually apparent at lower RPMs. + */ + stepper.setMicrostep(8); // Set microstep mode to 1:8 + + // In 1:8 microstepping mode, one revolution takes 8 times as many microsteps + stepper.move(8 * MOTOR_STEPS); // forward revolution + stepper.move(-8 * MOTOR_STEPS); // reverse revolution + + // One complete revolution is still 360° regardless of microstepping mode + // rotate() is easier to use than move() when no need to land on precise microstep position + stepper.rotate(360); + stepper.rotate(-360); + + delay(5000); +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/examples/MultiAxis/MultiAxis.ino b/WaveGen/Arduino/libraries/StepperDriver/examples/MultiAxis/MultiAxis.ino new file mode 100644 index 0000000..6528012 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/examples/MultiAxis/MultiAxis.ino @@ -0,0 +1,67 @@ +/* + * Multi-motor control (experimental) + * + * Move two or three motors at the same time. + * This module is still work in progress and may not work well or at all. + * + * Copyright (C)2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include +#include "BasicStepperDriver.h" +#include "MultiDriver.h" +#include "SyncDriver.h" + +// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step +#define MOTOR_STEPS 200 +// Target RPM for X axis motor +#define MOTOR_X_RPM 30 +// Target RPM for Y axis motor +#define MOTOR_Y_RPM 90 + +// X motor +#define DIR_X 8 +#define STEP_X 9 + +// Y motor +#define DIR_Y 6 +#define STEP_Y 7 + +// If microstepping is set externally, make sure this matches the selected mode +// 1=full step, 2=half step etc. +#define MICROSTEPS 32 + +// 2-wire basic config, microstepping is hardwired on the driver +// Other drivers can be mixed and matched but must be configured individually +BasicStepperDriver stepperX(MOTOR_STEPS, DIR_X, STEP_X); +BasicStepperDriver stepperY(MOTOR_STEPS, DIR_Y, STEP_Y); + +// Pick one of the two controllers below +// each motor moves independently, trajectory is a hockey stick +// MultiDriver controller(stepperX, stepperY); +// OR +// synchronized move, trajectory is a straight line +SyncDriver controller(stepperX, stepperY); + +void setup() { + /* + * Set target motors RPM. + */ + stepperX.begin(MOTOR_X_RPM, MICROSTEPS); + stepperY.begin(MOTOR_Y_RPM, MICROSTEPS); + // if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next two lines + // stepperX.setEnableActiveState(LOW); + // stepperY.setEnableActiveState(LOW); +} + +void loop() { + + controller.rotate(90*5, 60*15); + delay(1000); + controller.rotate(-90*5, -30*15); + delay(1000); + controller.rotate(0, -30*15); + delay(30000); +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/examples/NonBlocking/NonBlocking.ino b/WaveGen/Arduino/libraries/StepperDriver/examples/NonBlocking/NonBlocking.ino new file mode 100644 index 0000000..2c9d465 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/examples/NonBlocking/NonBlocking.ino @@ -0,0 +1,106 @@ +/* + * Example using non-blocking mode to move until a switch is triggered. + * + * Copyright (C)2015-2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include + +// this pin should connect to Ground when want to stop the motor +#define STOPPER_PIN 4 + +// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step +#define MOTOR_STEPS 200 +#define RPM 120 +// Microstepping mode. If you hardwired it to save pins, set to the same value here. +#define MICROSTEPS 16 + +#define DIR 8 +#define STEP 9 +#define SLEEP 13 // optional (just delete SLEEP from everywhere if not used) + +/* + * Choose one of the sections below that match your board + */ + +#include "DRV8834.h" +#define M0 10 +#define M1 11 +DRV8834 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, M0, M1); + +// #include "A4988.h" +// #define MS1 10 +// #define MS2 11 +// #define MS3 12 +// A4988 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MS1, MS2, MS3); + +// #include "DRV8825.h" +// #define MODE0 10 +// #define MODE1 11 +// #define MODE2 12 +// DRV8825 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MODE0, MODE1, MODE2); + +// #include "DRV8880.h" +// #define M0 10 +// #define M1 11 +// #define TRQ0 6 +// #define TRQ1 7 +// DRV8880 stepper(MOTORS_STEPS, DIR, STEP, SLEEP, M0, M1, TRQ0, TRQ1); + +// #include "BasicStepperDriver.h" // generic +// BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP); + +void setup() { + Serial.begin(115200); + + // Configure stopper pin to read HIGH unless grounded + pinMode(STOPPER_PIN, INPUT_PULLUP); + + stepper.begin(RPM, MICROSTEPS); + // if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line + // stepper.setEnableActiveState(LOW); + stepper.enable(); + + // set current level (for DRV8880 only). Valid percent values are 25, 50, 75 or 100. + // stepper.setCurrent(100); + + Serial.println("START"); + + // set the motor to move continuously for a reasonable time to hit the stopper + // let's say 100 complete revolutions (arbitrary number) + stepper.startMove(100 * MOTOR_STEPS * MICROSTEPS); // in microsteps + // stepper.startRotate(100 * 360); // or in degrees +} + +void loop() { + // first, check if stopper was hit + if (digitalRead(STOPPER_PIN) == LOW){ + Serial.println("STOPPER REACHED"); + + /* + * Choosing stop() vs startBrake(): + * + * constant speed mode, they are the same (stop immediately) + * linear (accelerated) mode with brake, the motor will go past the stopper a bit + */ + + stepper.stop(); + // stepper.startBrake(); + } + + // motor control loop - send pulse and return how long to wait until next pulse + unsigned wait_time_micros = stepper.nextAction(); + + // 0 wait time indicates the motor has stopped + if (wait_time_micros <= 0) { + stepper.disable(); // comment out to keep motor powered + delay(3600000); + } + + // (optional) execute other code if we have enough time + if (wait_time_micros > 100){ + // other code here + } +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/examples/SpeedProfile/SpeedProfile.ino b/WaveGen/Arduino/libraries/StepperDriver/examples/SpeedProfile/SpeedProfile.ino new file mode 100644 index 0000000..6461d38 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/examples/SpeedProfile/SpeedProfile.ino @@ -0,0 +1,76 @@ +/* + * This is not an example sketch, it is used to visualize the motor speed. + * + * Usage: upload and start Tool -> Serial Plotter + * + * All driver tests are done with microstep 1. Increasing microstep halves max rpm with each level. + * The maximum usable RPM can be determined from the output. + * The max RPM at a different microstep can be calculated with formula "max rpm / microstep" + * + * Copyright (C)2020 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include + +#include "BasicStepperDriver.h" +#include "MultiDriver.h" +#include "SyncDriver.h" + +#define RPM 150 +#define MICROSTEP 1 + +#define MOTOR_STEPS 200 + +// Do not use with a real motor, the step timing is very delayed due to serial printing. +BasicStepperDriver s1(MOTOR_STEPS, 12, 13); +BasicStepperDriver s2(MOTOR_STEPS, 12, 13); +BasicStepperDriver s3(MOTOR_STEPS, 12, 13); + +void setup() { + Serial.begin(115200); + delay(4000); + + s1.setSpeedProfile(BasicStepperDriver::LINEAR_SPEED, 2000, 2000); + s2.setSpeedProfile(BasicStepperDriver::LINEAR_SPEED, 500, 500); + s3.setSpeedProfile(BasicStepperDriver::CONSTANT_SPEED); + + s1.begin(RPM, MICROSTEP); + s2.begin(RPM, MICROSTEP); + s3.begin(RPM, MICROSTEP); + + s1.startMove(500); + s2.startMove(500); + s3.startMove(500); +} + +void loop() { + unsigned w1, w2, w3; + + w1 = s1.nextAction(); + w2 = s2.nextAction(); + w3 = s3.nextAction(); + + if (w1 > 0 || w2 > 0 || w3 > 0){ + // uncomment to see step delays instead of speed + /* + Serial.print(w1); + Serial.print("\t"); + Serial.print(w2); + Serial.print("\t"); + Serial.print(w2); + Serial.println(); + */ + + // graph current rpm + Serial.print(s1.getCurrentRPM()); + Serial.print("\t"); + Serial.print(s2.getCurrentRPM()); + Serial.print("\t"); + Serial.print(s3.getCurrentRPM()); + Serial.println(); + } else { + delay(100000); + } +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/UnitTest.ino b/WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/UnitTest.ino new file mode 100644 index 0000000..bd7a0a9 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/UnitTest.ino @@ -0,0 +1,163 @@ +/* + * This is not an example sketch, it is used to validate code changes + * and determine maximum workable RPM/microsteps parameters for a given board. + * + * Usage: run with serial terminal open + * + * All driver tests are done with microstep 1. Increasing microstep halves max rpm with each level. + * The maximum usable RPM can be determined from the output. + * The max RPM at a different microstep can be calculated with formula "max rpm / microstep" + * + * Copyright (C)2020 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include + +#include "BasicStepperDriver.h" +#include "MultiDriver.h" +#include "SyncDriver.h" + +// RPMS contains the list of RPMS to test at, assuming microstep=1 +const float RPMS[] = {6000, 600, 60, 6}; +const int RPMS_COUNT = sizeof(RPMS)/sizeof(*RPMS); +const long DURATION_CONSTANT[] = {10000, 100000, 1000000, 10000000}; +const long DURATION_LINEAR[] = {365148, 365148, 1033246, 10000000}; +// STEPS is how many steps for each test. More has better accuracy but slower +#define STEPS 200 +// ALLOWED_DEVIATION is the error tolerance. 0.10 considers 90% - 110% range aceptable +#define ALLOWED_DEVIATION 0.10 + +/* + * Verify that the expected time calculation is correct at different rpms and two microstep levels + */ +bool test_calculations(BasicStepperDriver stepper, const long duration[]){ + bool pass = true; + char t[128]; + for (int i = 0; i < RPMS_COUNT; i++){ + float rpm = RPMS[i]; + for (int microstep = 1; microstep <= 16; microstep <<= 4){ + long expected_micros = duration[i]; + stepper.begin(rpm, microstep); + long estimated_micros = stepper.getTimeForMove(STEPS*microstep); + sprintf(t, " rpm=%-4d microstep=%-2d expected=%10luµs estimated %10luµs", + int(rpm), microstep, expected_micros, estimated_micros); + Serial.print(t); + float ratio = float(estimated_micros) / float(expected_micros); + if (ratio > 1.01 or ratio < 0.99) { + Serial.print(" FAIL"); + pass = false; + } + Serial.println(); + } + } + return pass; +} + +/* + * Pass/fail the result and print it out in a one-line format + */ +bool result(float rpm, int microstep, int steps, long elapsed_micros, long expected_micros){ + bool pass = true; + char t[128]; + float error = float(elapsed_micros) / float(expected_micros); + unsigned step_micros = expected_micros / steps; + unsigned error_micros = labs(elapsed_micros - expected_micros) / steps; + sprintf(t, " rpm=%-4d expected=%10luµs elapsed=%10luµs step_err=%6uµs avgstep=%6uµs", + int(rpm), expected_micros, elapsed_micros, error_micros, step_micros); + Serial.print(t); + if (error >= 1.0f + ALLOWED_DEVIATION || error <= 1.0f - ALLOWED_DEVIATION) { + pass = false; + Serial.print(" FAIL"); + } + Serial.println(); + return pass; +} + +/* + * Run the tests for BasicStepperDriver + */ +bool test_basic(BasicStepperDriver stepper){ + bool pass = true; + for (int i = 0; i < RPMS_COUNT; i++){ + float rpm = RPMS[i]; + stepper.begin(rpm, 1); + unsigned long start_time_micros = micros(); + stepper.move(STEPS); + long elapsed_micros = micros() - start_time_micros; + pass &= result(rpm, 1, STEPS, elapsed_micros, stepper.getTimeForMove(STEPS)); + } + return pass; +} + +/* + * Run the tests for MultiDriver with 3 motors + */ +bool test_multi(BasicStepperDriver s1, BasicStepperDriver s2, BasicStepperDriver s3){ + MultiDriver controller(s1, s2, s3); + bool pass = true; + for (int i = 0; i < RPMS_COUNT; i++){ + float rpm = RPMS[i]; + s1.begin(rpm, 1); + s2.begin(rpm, 1); + s3.begin(rpm, 1); + unsigned long start_time_micros = micros(); + controller.move(STEPS, 2*STEPS/3, -STEPS/2); + long elapsed_micros = micros() - start_time_micros; + pass &= result(rpm, 1, STEPS, elapsed_micros, s1.getTimeForMove(STEPS)); + } + return pass; +} + +/* + * Run the tests for SyncDriver with 3 motors + */ +bool test_sync(BasicStepperDriver s1, BasicStepperDriver s2, BasicStepperDriver s3){ + SyncDriver controller(s1, s2, s3); + bool pass = true; + for (int i = 0; i < RPMS_COUNT; i++){ + float rpm = RPMS[i]; + s1.begin(rpm, 1); + s2.begin(rpm, 1); + s3.begin(rpm, 1); + unsigned long start_time_micros = micros(); + controller.move(STEPS, 2*STEPS/3, -STEPS/2); + long elapsed_micros = micros() - start_time_micros; + pass &= result(rpm, 1, STEPS, elapsed_micros, s1.getTimeForMove(STEPS)); + } + return pass; +} + +#define TEST_RESULT(result, func, ...) #func "(" #__VA_ARGS__ "): " result +#define RUN_TEST(desc, func, ...) Serial.println(desc); Serial.println(func(__VA_ARGS__) ? TEST_RESULT("OK", func, __VA_ARGS__) : TEST_RESULT("FAIL", func, __VA_ARGS__)) + +void setup() { + + BasicStepperDriver s1(200, 12, 13); + BasicStepperDriver s2(200, 12, 13); + BasicStepperDriver s3(200, 12, 13); + + Serial.begin(115200); + delay(2000); +#ifdef ARDUINO_BOARD + Serial.println(ARDUINO_BOARD); +#endif + RUN_TEST("Timing Calculation test, constant speed", test_calculations, s1, DURATION_CONSTANT); + RUN_TEST("BasicStepperDriver test, constant speed", test_basic, s1); + RUN_TEST("MultiDriver test, constant speed", test_multi, s1, s2, s3); + RUN_TEST("SyncDriver test, constant speed", test_sync, s1, s2, s3); + + s1.setSpeedProfile(s1.LINEAR_SPEED, 6000, 6000); + s2.setSpeedProfile(s2.LINEAR_SPEED, 6000, 6000); + s3.setSpeedProfile(s3.LINEAR_SPEED, 6000, 6000); + + RUN_TEST("Timing Calculation test, linear speed", test_calculations, s1, DURATION_LINEAR); + RUN_TEST("BasicStepperDriver test, linear speed", test_basic, s1); + RUN_TEST("MultiDriver test, linear speed", test_multi, s1, s2, s3); + RUN_TEST("SyncDriver test, linear speed", test_sync, s1, s2, s3); +} + +void loop() { + delay(1); +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/adafruit_feather_m0.txt b/WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/adafruit_feather_m0.txt new file mode 100644 index 0000000..43de6dc --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/adafruit_feather_m0.txt @@ -0,0 +1,56 @@ +Timing Calculation test, constant speed + rpm=6000 microstep=1 expected= 10000µs estimated 10000µs + rpm=6000 microstep=16 expected= 10000µs estimated 10000µs + rpm=600 microstep=1 expected= 100000µs estimated 100000µs + rpm=600 microstep=16 expected= 100000µs estimated 100000µs + rpm=60 microstep=1 expected= 1000000µs estimated 1000000µs + rpm=60 microstep=16 expected= 1000000µs estimated 1000000µs + rpm=6 microstep=1 expected= 10000000µs estimated 10000000µs + rpm=6 microstep=16 expected= 10000000µs estimated 10000000µs +test_calculations(s1, DURATION_CONSTANT): OK +BasicStepperDriver test, constant speed + rpm=6000 expected= 10000µs elapsed= 11336µs step_err= 6µs avgstep= 50µs FAIL + rpm=600 expected= 100000µs elapsed= 100728µs step_err= 3µs avgstep= 500µs + rpm=60 expected= 1000000µs elapsed= 996284µs step_err= 18µs avgstep= 5000µs + rpm=6 expected= 10000000µs elapsed= 9951257µs step_err= 243µs avgstep= 50000µs +test_basic(s1): FAIL +MultiDriver test, constant speed + rpm=6000 expected= 10000µs elapsed= 18210µs step_err= 41µs avgstep= 50µs FAIL + rpm=600 expected= 100000µs elapsed= 108452µs step_err= 42µs avgstep= 500µs + rpm=60 expected= 1000000µs elapsed= 1008295µs step_err= 41µs avgstep= 5000µs + rpm=6 expected= 10000000µs elapsed= 10008300µs step_err= 41µs avgstep= 50000µs +test_multi(s1, s2, s3): FAIL +SyncDriver test, constant speed + rpm=6000 expected= 10000µs elapsed= 19284µs step_err= 46µs avgstep= 50µs FAIL + rpm=600 expected= 100000µs elapsed= 109244µs step_err= 46µs avgstep= 500µs + rpm=60 expected= 1000000µs elapsed= 1009259µs step_err= 46µs avgstep= 5000µs + rpm=6 expected= 10000000µs elapsed= 10009274µs step_err= 46µs avgstep= 50000µs +test_sync(s1, s2, s3): FAIL +Timing Calculation test, linear speed + rpm=6000 microstep=1 expected= 365148µs estimated 365148µs + rpm=6000 microstep=16 expected= 365148µs estimated 365148µs + rpm=600 microstep=1 expected= 365148µs estimated 365148µs + rpm=600 microstep=16 expected= 365148µs estimated 365148µs + rpm=60 microstep=1 expected= 1033246µs estimated 1033246µs + rpm=60 microstep=16 expected= 1033246µs estimated 1033333µs + rpm=6 microstep=1 expected= 10000000µs estimated 10000000µs + rpm=6 microstep=16 expected= 10000000µs estimated 10000000µs +test_calculations(s1, DURATION_LINEAR): OK +BasicStepperDriver test, linear speed + rpm=6000 expected= 365148µs elapsed= 343357µs step_err= 108µs avgstep= 1825µs + rpm=600 expected= 365148µs elapsed= 343361µs step_err= 108µs avgstep= 1825µs + rpm=60 expected= 1033246µs elapsed= 1010367µs step_err= 114µs avgstep= 5166µs + rpm=6 expected= 10000000µs elapsed= 2457426µs step_err= 37712µs avgstep= 50000µs FAIL +test_basic(s1): FAIL +MultiDriver test, linear speed + rpm=6000 expected= 365148µs elapsed= 364538µs step_err= 3µs avgstep= 1825µs + rpm=600 expected= 365148µs elapsed= 364552µs step_err= 2µs avgstep= 1825µs + rpm=60 expected= 1033246µs elapsed= 1030380µs step_err= 14µs avgstep= 5166µs + rpm=6 expected= 10000000µs elapsed= 2477104µs step_err= 37614µs avgstep= 50000µs FAIL +test_multi(s1, s2, s3): FAIL +SyncDriver test, linear speed + rpm=6000 expected= 365148µs elapsed= 365655µs step_err= 2µs avgstep= 1825µs + rpm=600 expected= 365148µs elapsed= 365591µs step_err= 2µs avgstep= 1825µs + rpm=60 expected= 1033246µs elapsed= 1245906µs step_err= 1063µs avgstep= 5166µs FAIL + rpm=6 expected= 10000000µs elapsed= 2478551µs step_err= 37607µs avgstep= 50000µs FAIL +test_sync(s1, s2, s3): FAIL diff --git a/WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/esp8266_nodemcu.txt b/WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/esp8266_nodemcu.txt new file mode 100644 index 0000000..088eb5e --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/examples/UnitTest/esp8266_nodemcu.txt @@ -0,0 +1,58 @@ + +ESP8266_NODEMCU +Timing Calculation test, constant speed + rpm=6000 microstep=1 expected= 10000µs estimated 10000µs + rpm=6000 microstep=16 expected= 10000µs estimated 10000µs + rpm=600 microstep=1 expected= 100000µs estimated 100000µs + rpm=600 microstep=16 expected= 100000µs estimated 100000µs + rpm=60 microstep=1 expected= 1000000µs estimated 1000000µs + rpm=60 microstep=16 expected= 1000000µs estimated 1000000µs + rpm=6 microstep=1 expected= 10000000µs estimated 10000000µs + rpm=6 microstep=16 expected= 10000000µs estimated 10000000µs +test_calculations(s1, DURATION_CONSTANT): OK +BasicStepperDriver test, constant speed + rpm=6000 expected= 10000µs elapsed= 10346µs step_err= 1µs avgstep= 50µs + rpm=600 expected= 100000µs elapsed= 99760µs step_err= 1µs avgstep= 500µs + rpm=60 expected= 1000000µs elapsed= 995291µs step_err= 23µs avgstep= 5000µs + rpm=6 expected= 10000000µs elapsed= 9950300µs step_err= 248µs avgstep= 50000µs +test_basic(s1): OK +MultiDriver test, constant speed + rpm=6000 expected= 10000µs elapsed= 12420µs step_err= 12µs avgstep= 50µs FAIL + rpm=600 expected= 100000µs elapsed= 105190µs step_err= 25µs avgstep= 500µs + rpm=60 expected= 1000000µs elapsed= 1006547µs step_err= 32µs avgstep= 5000µs + rpm=6 expected= 10000000µs elapsed= 10017375µs step_err= 86µs avgstep= 50000µs +test_multi(s1, s2, s3): FAIL +SyncDriver test, constant speed + rpm=6000 expected= 10000µs elapsed= 14511µs step_err= 22µs avgstep= 50µs FAIL + rpm=600 expected= 100000µs elapsed= 105689µs step_err= 28µs avgstep= 500µs + rpm=60 expected= 1000000µs elapsed= 1006867µs step_err= 34µs avgstep= 5000µs + rpm=6 expected= 10000000µs elapsed= 10018707µs step_err= 93µs avgstep= 50000µs +test_sync(s1, s2, s3): FAIL +Timing Calculation test, linear speed + rpm=6000 microstep=1 expected= 365148µs estimated 365148µs + rpm=6000 microstep=16 expected= 365148µs estimated 365148µs + rpm=600 microstep=1 expected= 365148µs estimated 365148µs + rpm=600 microstep=16 expected= 365148µs estimated 365148µs + rpm=60 microstep=1 expected= 1033246µs estimated 1033246µs + rpm=60 microstep=16 expected= 1033246µs estimated 1033333µs + rpm=6 microstep=1 expected= 10000000µs estimated 10000000µs + rpm=6 microstep=16 expected= 10000000µs estimated 10000000µs +test_calculations(s1, DURATION_LINEAR): OK +BasicStepperDriver test, linear speed + rpm=6000 expected= 365148µs elapsed= 342337µs step_err= 114µs avgstep= 1825µs + rpm=600 expected= 365148µs elapsed= 342328µs step_err= 114µs avgstep= 1825µs + rpm=60 expected= 1033246µs elapsed= 1009343µs step_err= 119µs avgstep= 5166µs + rpm=6 expected= 10000000µs elapsed= 2456407µs step_err= 37717µs avgstep= 50000µs FAIL +test_basic(s1): FAIL +MultiDriver test, linear speed + rpm=6000 expected= 365148µs elapsed= 360765µs step_err= 21µs avgstep= 1825µs + rpm=600 expected= 365148µs elapsed= 360705µs step_err= 22µs avgstep= 1825µs + rpm=60 expected= 1033246µs elapsed= 1028486µs step_err= 23µs avgstep= 5166µs + rpm=6 expected= 10000000µs elapsed= 2477224µs step_err= 37613µs avgstep= 50000µs FAIL +test_multi(s1, s2, s3): FAIL +SyncDriver test, linear speed + rpm=6000 expected= 365148µs elapsed= 361367µs step_err= 18µs avgstep= 1825µs + rpm=600 expected= 365148µs elapsed= 361408µs step_err= 18µs avgstep= 1825µs + rpm=60 expected= 1033246µs elapsed= 1242580µs step_err= 1046µs avgstep= 5166µs FAIL + rpm=6 expected= 10000000µs elapsed= 2477912µs step_err= 37610µs avgstep= 50000µs FAIL +test_sync(s1, s2, s3): FAIL diff --git a/WaveGen/Arduino/libraries/StepperDriver/keywords.txt b/WaveGen/Arduino/libraries/StepperDriver/keywords.txt new file mode 100644 index 0000000..4247a05 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/keywords.txt @@ -0,0 +1,28 @@ +StepperDriver KEYWORD1 + +BasicStepperDriver KEYWORD1 +DRV8880 KEYWORD1 +DRV8834 KEYWORD1 +DRV8824 KEYWORD1 +DRV8825 KEYWORD1 +A4988 KEYWORD1 +MultiDriver KEYWORD1 +SyncDriver KEYWORD1 + +setMicrostep KEYWORD2 +setSpeedProfile KEYWORD2 +move KEYWORD2 +rotate KEYWORD2 +setRPM KEYWORD2 +getRPM KEYWORD2 +setCurrent KEYWORD2 +enable KEYWORD2 +disable KEYWORD2 +startMove KEYWORD2 +startRotate KEYWORD2 +nextAction KEYWORD2 +stop KEYWORD2 +startBrake KEYWORD2 + +CONSTANT_SPEED LITERAL1 +LINEAR_SPEED LITERAL1 diff --git a/WaveGen/Arduino/libraries/StepperDriver/library.properties b/WaveGen/Arduino/libraries/StepperDriver/library.properties new file mode 100644 index 0000000..368abe5 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/library.properties @@ -0,0 +1,9 @@ +name=StepperDriver +version=1.4.1 +author=Laurentiu Badea +maintainer=Laurentiu Badea +sentence=A4988, DRV8825 and generic two-pin stepper motor driver library. +paragraph=Control steppers via a driver board providing STEP+DIR like the ones from Pololu. Microstepping is supported. Acceleration is supported. Supported drivers are A4988, DRV8824, DRV8825, DRV8834, DRV8880. +category=Device Control +url=https://github.com/laurb9/StepperDriver +architectures=* diff --git a/WaveGen/Arduino/libraries/StepperDriver/platformio.ini b/WaveGen/Arduino/libraries/StepperDriver/platformio.ini new file mode 100644 index 0000000..9db153d --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/platformio.ini @@ -0,0 +1,49 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[platformio] +src_dir = examples/UnitTest +lib_dir = . +default_envs = + nodemcuv2 + adafruit_feather_m0 + esp32dev + teensylc + +[env] +framework = arduino +monitor_filters = + colorize + send_on_enter +monitor_speed = 115200 + +[env:nodemcuv2] +platform = espressif8266 +board = nodemcuv2 +upload_speed = 1000000 + +[env:adafruit_feather_m0] +platform = atmelsam +board = adafruit_feather_m0 + +[env:esp32dev] +board = esp32dev +platform = espressif32 + +[env:teensylc] +platform = teensy +board = teensylc + +[env:uno] +board = uno +platform = atmelavr + +[env:native] +platform = native diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/A4988.cpp b/WaveGen/Arduino/libraries/StepperDriver/src/A4988.cpp new file mode 100644 index 0000000..088ff6e --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/A4988.cpp @@ -0,0 +1,95 @@ +/* + * A4988 - Stepper Motor Driver Driver + * Indexer mode only. + + * Copyright (C)2015 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include "A4988.h" + +/* + * Microstepping resolution truth table (Page 6 of A4988 pdf) + * 0bMS3,MS2,MS1 for 1,2,4,8,16 microsteps + */ +const uint8_t A4988::MS_TABLE[] = {0b000, 0b001, 0b010, 0b011, 0b111}; + +/* + * Basic connection: only DIR, STEP are connected. + * Microstepping controls should be hardwired. + */ +A4988::A4988(short steps, short dir_pin, short step_pin) +:BasicStepperDriver(steps, dir_pin, step_pin) +{} + +A4988::A4988(short steps, short dir_pin, short step_pin, short enable_pin) +:BasicStepperDriver(steps, dir_pin, step_pin, enable_pin) +{} + +/* + * Fully wired. + * All the necessary control pins for A4988 are connected. + */ +A4988::A4988(short steps, short dir_pin, short step_pin, short ms1_pin, short ms2_pin, short ms3_pin) +:BasicStepperDriver(steps, dir_pin, step_pin), + ms1_pin(ms1_pin), ms2_pin(ms2_pin), ms3_pin(ms3_pin) +{} + +A4988::A4988(short steps, short dir_pin, short step_pin, short enable_pin, short ms1_pin, short ms2_pin, short ms3_pin) +:BasicStepperDriver(steps, dir_pin, step_pin, enable_pin), +ms1_pin(ms1_pin), ms2_pin(ms2_pin), ms3_pin(ms3_pin) +{} + +void A4988::begin(float rpm, short microsteps){ + BasicStepperDriver::begin(rpm, microsteps); + + if (!IS_CONNECTED(ms1_pin) || !IS_CONNECTED(ms2_pin) || !IS_CONNECTED(ms3_pin)){ + return; + } + + pinMode(ms1_pin, OUTPUT); + pinMode(ms2_pin, OUTPUT); + pinMode(ms3_pin, OUTPUT); +} + +/* + * Set microstepping mode (1:divisor) + * Allowed ranges for A4988 are 1:1 to 1:16 + * If the control pins are not connected, we recalculate the timing only + */ +short A4988::setMicrostep(short microsteps){ + BasicStepperDriver::setMicrostep(microsteps); + + if (!IS_CONNECTED(ms1_pin) || !IS_CONNECTED(ms2_pin) || !IS_CONNECTED(ms3_pin)){ + return this->microsteps; + } + + const uint8_t* ms_table = getMicrostepTable(); + size_t ms_table_size = getMicrostepTableSize(); + + unsigned short i = 0; + while (i < ms_table_size){ + if (this->microsteps & (1<microsteps; +} + +const uint8_t* A4988::getMicrostepTable(){ + return A4988::MS_TABLE; +} + +size_t A4988::getMicrostepTableSize(){ + return sizeof(A4988::MS_TABLE); +} + +short A4988::getMaxMicrostep(){ + return A4988::MAX_MICROSTEP; +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/A4988.h b/WaveGen/Arduino/libraries/StepperDriver/src/A4988.h new file mode 100644 index 0000000..454e287 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/A4988.h @@ -0,0 +1,56 @@ +/* + * A4988 - Stepper Motor Driver Driver + * Indexer mode only. + * + * Copyright (C)2015 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#ifndef A4988_H +#define A4988_H +#include +#include "BasicStepperDriver.h" + +class A4988 : public BasicStepperDriver { +protected: + static const uint8_t MS_TABLE[]; + short ms1_pin = PIN_UNCONNECTED; + short ms2_pin = PIN_UNCONNECTED; + short ms3_pin = PIN_UNCONNECTED; + // tA STEP minimum, HIGH pulse width (1us) + static const int step_high_min = 1; + // tB STEP minimum, LOW pulse width (1us) + static const int step_low_min = 1; + // wakeup time, nSLEEP inactive to STEP (1000us) + static const int wakeup_time = 1000; + // also 200ns between ENBL/DIR/MSx changes and STEP HIGH + + // Get the microstep table + virtual const uint8_t* getMicrostepTable(); + virtual size_t getMicrostepTableSize(); + + // Get max microsteps supported by the device + short getMaxMicrostep() override; + +private: + // microstep range (1, 16, 32 etc) + static const short MAX_MICROSTEP = 16; + +public: + /* + * Basic connection: only DIR, STEP are connected. + * Microstepping controls should be hardwired. + */ + A4988(short steps, short dir_pin, short step_pin); + A4988(short steps, short dir_pin, short step_pin, short enable_pin); + + void begin(float rpm=60, short microsteps=1); + /* + * Fully wired. All the necessary control pins for A4988 are connected. + */ + A4988(short steps, short dir_pin, short step_pin, short ms1_pin, short ms2_pin, short ms3_pin); + A4988(short steps, short dir_pin, short step_pin, short enable_pin, short ms1_pin, short ms2_pin, short ms3_pin); + short setMicrostep(short microsteps) override; +}; +#endif // A4988_H diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/BasicStepperDriver.cpp b/WaveGen/Arduino/libraries/StepperDriver/src/BasicStepperDriver.cpp new file mode 100644 index 0000000..562385b --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/BasicStepperDriver.cpp @@ -0,0 +1,379 @@ +/* + * Generic Stepper Motor Driver Driver + * Indexer mode only. + + * Copyright (C)2015-2019 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + * + * Linear speed profile calculations based on + * - Generating stepper-motor speed profiles in real time - David Austin, 2004 + * - Atmel AVR446: Linear speed control of stepper motor, 2006 + */ + +#include "BasicStepperDriver.h" + + +/* + * Min/Max functions which avoid evaluating the arguments multiple times. + * See also https://github.com/arduino/Arduino/issues/2069 + */ +template +constexpr const T& stepperMin(const T& a, const T& b) +{ + return b < a ? b : a; +} + +template +constexpr const T& stepperMax(const T& a, const T& b) +{ + return a < b ? b : a; +} + +/* + * Basic connection: only DIR, STEP are connected. + * Microstepping controls should be hardwired. + */ +BasicStepperDriver::BasicStepperDriver(short steps, short dir_pin, short step_pin) +:BasicStepperDriver(steps, dir_pin, step_pin, PIN_UNCONNECTED) +{ +} + +BasicStepperDriver::BasicStepperDriver(short steps, short dir_pin, short step_pin, short enable_pin) +:motor_steps(steps), dir_pin(dir_pin), step_pin(step_pin), enable_pin(enable_pin) +{ + steps_to_cruise = 0; + steps_remaining = 0; + dir_state = 0; + steps_to_brake = 0; + step_pulse = 0; + cruise_step_pulse = 0; + rest = 0; + step_count = 0; +} + +/* + * Initialize pins, calculate timings etc + */ +void BasicStepperDriver::begin(float rpm, short microsteps){ + pinMode(dir_pin, OUTPUT); + digitalWrite(dir_pin, LOW); + + pinMode(step_pin, OUTPUT); + digitalWrite(step_pin, LOW); + + if IS_CONNECTED(enable_pin){ + pinMode(enable_pin, OUTPUT); + disable(); + } + + this->rpm = rpm; + setMicrostep(microsteps); + + enable(); +} + +/* + * Set target motor RPM (1-200 is a reasonable range) + */ +void BasicStepperDriver::setRPM(float rpm){ + if (this->rpm == 0){ // begin() has not been called (old 1.0 code) + begin(rpm, microsteps); + } + this->rpm = rpm; +} + +/* + * Set stepping mode (1:microsteps) + * Allowed ranges for BasicStepperDriver are 1:1 to 1:128 + */ +short BasicStepperDriver::setMicrostep(short microsteps){ + for (short ms=1; ms <= getMaxMicrostep(); ms<<=1){ + if (microsteps == ms){ + this->microsteps = microsteps; + break; + } + } + return this->microsteps; +} + +/* + * Set speed profile - CONSTANT_SPEED, LINEAR_SPEED (accelerated) + * accel and decel are given in [full steps/s^2] + */ +void BasicStepperDriver::setSpeedProfile(Mode mode, short accel, short decel){ + profile.mode = mode; + profile.accel = accel; + profile.decel = decel; +} +void BasicStepperDriver::setSpeedProfile(struct Profile profile){ + this->profile = profile; +} + +/* + * Move the motor a given number of steps. + * positive to move forward, negative to reverse + */ +void BasicStepperDriver::move(long steps){ + startMove(steps); + while (nextAction()); +} +/* + * Move the motor a given number of degrees (1-360) + */ +void BasicStepperDriver::rotate(long deg){ + move(calcStepsForRotation(deg)); +} +/* + * Move the motor with sub-degree precision. + * Note that using this function even once will add 1K to your program size + * due to inclusion of float support. + */ +void BasicStepperDriver::rotate(double deg){ + move(calcStepsForRotation(deg)); +} + +/* + * Set up a new move (calculate and save the parameters) + */ +void BasicStepperDriver::startMove(long steps, long time){ + float speed; + // set up new move + dir_state = (steps >= 0) ? HIGH : LOW; + last_action_end = 0; + steps_remaining = labs(steps); + step_count = 0; + rest = 0; + switch (profile.mode){ + case LINEAR_SPEED: + // speed is in [steps/s] + speed = rpm * motor_steps / 60; + if (time > 0){ + // Calculate a new speed to finish in the time requested + float t = time / (1e+6); // convert to seconds + float d = steps_remaining / microsteps; // convert to full steps + float a2 = 1.0 / profile.accel + 1.0 / profile.decel; + float sqrt_candidate = t*t - 2 * a2 * d; // in √b^2-4ac + if (sqrt_candidate >= 0){ + speed = stepperMin(speed, (t - (float)sqrt(sqrt_candidate)) / a2); + }; + } + // how many microsteps from 0 to target speed + steps_to_cruise = microsteps * (speed * speed / (2 * profile.accel)); + // how many microsteps are needed from cruise speed to a full stop + steps_to_brake = steps_to_cruise * profile.accel / profile.decel; + if (steps_remaining < steps_to_cruise + steps_to_brake){ + // cannot reach max speed, will need to brake early + steps_to_cruise = steps_remaining * profile.decel / (profile.accel + profile.decel); + steps_to_brake = steps_remaining - steps_to_cruise; + } + // Initial pulse (c0) including error correction factor 0.676 [us] + step_pulse = (1e+6)*0.676*sqrt(2.0f/profile.accel/microsteps); + // Save cruise timing since we will no longer have the calculated target speed later + cruise_step_pulse = 1e+6 / speed / microsteps; + break; + + case CONSTANT_SPEED: + default: + steps_to_cruise = 0; + steps_to_brake = 0; + step_pulse = cruise_step_pulse = STEP_PULSE(motor_steps, microsteps, rpm); + if (time > steps_remaining * step_pulse){ + step_pulse = (float)time / steps_remaining; + } + } +} +/* + * Alter a running move by adding/removing steps + * FIXME: This is a naive implementation and it only works well in CRUISING state + */ +void BasicStepperDriver::alterMove(long steps){ + switch (getCurrentState()){ + case ACCELERATING: // this also works but will keep the original speed target + case CRUISING: + if (steps >= 0){ + steps_remaining += steps; + } else { + steps_remaining = stepperMax(steps_to_brake, steps_remaining+steps); + }; + break; + case DECELERATING: + // would need to start accelerating again -- NOT IMPLEMENTED + break; + case STOPPED: + startMove(steps); + break; + } +} +/* + * Brake early. + */ +void BasicStepperDriver::startBrake(void){ + switch (getCurrentState()){ + case CRUISING: // this applies to both CONSTANT_SPEED and LINEAR_SPEED modes + steps_remaining = steps_to_brake; + break; + + case ACCELERATING: + steps_remaining = step_count * profile.accel / profile.decel; + break; + + default: + break; // nothing to do if already stopped or braking + } +} +/* + * Stop movement immediately and return remaining steps. + */ +long BasicStepperDriver::stop(void){ + long retval = steps_remaining; + steps_remaining = 0; + return retval; +} +/* + * Return calculated time to complete the given move + */ +long BasicStepperDriver::getTimeForMove(long steps){ + float t; + long cruise_steps; + float speed; + if (steps == 0){ + return 0; + } + switch (profile.mode){ + case LINEAR_SPEED: + startMove(steps); + cruise_steps = steps_remaining - steps_to_cruise - steps_to_brake; + speed = rpm * motor_steps / 60; // full steps/s + t = (cruise_steps / (microsteps * speed)) + + sqrt(2.0 * steps_to_cruise / profile.accel / microsteps) + + sqrt(2.0 * steps_to_brake / profile.decel / microsteps); + t *= (1e+6); // seconds -> micros + break; + case CONSTANT_SPEED: + default: + t = steps * STEP_PULSE(motor_steps, microsteps, rpm); + } + return round(t); +} +/* + * Move the motor an integer number of degrees (360 = full rotation) + * This has poor precision for small amounts, since step is usually 1.8deg + */ +void BasicStepperDriver::startRotate(long deg){ + startMove(calcStepsForRotation(deg)); +} +/* + * Move the motor with sub-degree precision. + * Note that calling this function will increase program size substantially + * due to inclusion of float support. + */ +void BasicStepperDriver::startRotate(double deg){ + startMove(calcStepsForRotation(deg)); +} + +/* + * calculate the interval til the next pulse + */ +void BasicStepperDriver::calcStepPulse(void){ + if (steps_remaining <= 0){ // this should not happen, but avoids strange calculations + return; + } + steps_remaining--; + step_count++; + + if (profile.mode == LINEAR_SPEED){ + switch (getCurrentState()){ + case ACCELERATING: + if (step_count < steps_to_cruise){ + step_pulse = step_pulse - (2*step_pulse+rest)/(4*step_count+1); + rest = (step_count < steps_to_cruise) ? (2*step_pulse+rest) % (4*step_count+1) : 0; + } else { + // The series approximates target, set the final value to what it should be instead + step_pulse = cruise_step_pulse; + } + break; + + case DECELERATING: + step_pulse = step_pulse - (2*step_pulse+rest)/(-4*steps_remaining+1); + rest = (2*step_pulse+rest) % (-4*steps_remaining+1); + break; + + default: + break; // no speed changes + } + } +} +/* + * Yield to step control + * Toggle step and return time until next change is needed (micros) + */ +long BasicStepperDriver::nextAction(void){ + if (steps_remaining > 0){ + delayMicros(next_action_interval, last_action_end); + /* + * DIR pin is sampled on rising STEP edge, so it is set first + */ + digitalWrite(dir_pin, dir_state); + digitalWrite(step_pin, HIGH); + unsigned m = micros(); + unsigned long pulse = step_pulse; // save value because calcStepPulse() will overwrite it + calcStepPulse(); + // We should pull HIGH for at least 1-2us (step_high_min) + delayMicros(step_high_min); + digitalWrite(step_pin, LOW); + // account for calcStepPulse() execution time; sets ceiling for max rpm on slower MCUs + last_action_end = micros(); + m = last_action_end - m; + next_action_interval = (pulse > m) ? pulse - m : 1; + } else { + // end of move + last_action_end = 0; + next_action_interval = 0; + } + return next_action_interval; +} + +enum BasicStepperDriver::State BasicStepperDriver::getCurrentState(void){ + enum State state; + if (steps_remaining <= 0){ + state = STOPPED; + } else { + if (steps_remaining <= steps_to_brake){ + state = DECELERATING; + } else if (step_count <= steps_to_cruise){ + state = ACCELERATING; + } else { + state = CRUISING; + } + } + return state; +} +/* + * Configure which logic state on ENABLE pin means active + * when using SLEEP (default) this is active HIGH + */ +void BasicStepperDriver::setEnableActiveState(short state){ + enable_active_state = state; +} +/* + * Enable/Disable the motor by setting a digital flag + */ +void BasicStepperDriver::enable(void){ + if IS_CONNECTED(enable_pin){ + digitalWrite(enable_pin, enable_active_state); + }; + delayMicros(2); +} + +void BasicStepperDriver::disable(void){ + if IS_CONNECTED(enable_pin){ + digitalWrite(enable_pin, (enable_active_state == HIGH) ? LOW : HIGH); + // variable = (condition) ? expressionTrue : expressionFalse; + } +} + +short BasicStepperDriver::getMaxMicrostep(){ + return BasicStepperDriver::MAX_MICROSTEP; +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/BasicStepperDriver.h b/WaveGen/Arduino/libraries/StepperDriver/src/BasicStepperDriver.h new file mode 100644 index 0000000..029e899 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/BasicStepperDriver.h @@ -0,0 +1,251 @@ +/* + * Generic Stepper Motor Driver Driver + * Indexer mode only. + * + * Copyright (C)2015-2018 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#ifndef STEPPER_DRIVER_BASE_H +#define STEPPER_DRIVER_BASE_H +#include + +// used internally by the library to mark unconnected pins +#define PIN_UNCONNECTED -1 +#define IS_CONNECTED(pin) (pin != PIN_UNCONNECTED) + +/* + * calculate the step pulse in microseconds for a given rpm value. + * 60[s/min] * 1000000[us/s] / microsteps / steps / rpm + */ +#define STEP_PULSE(steps, microsteps, rpm) (60.0*1000000L/steps/microsteps/rpm) + +// don't call yield if we have a wait shorter than this +#define MIN_YIELD_MICROS 50 + +/* + * Basic Stepper Driver class. + * Microstepping level should be externally controlled or hardwired. + */ +class BasicStepperDriver { +public: + enum Mode {CONSTANT_SPEED, LINEAR_SPEED}; + enum State {STOPPED, ACCELERATING, CRUISING, DECELERATING}; + struct Profile { + Mode mode = CONSTANT_SPEED; + short accel = 1000; // acceleration [steps/s^2] + short decel = 1000; // deceleration [steps/s^2] + }; + static inline void delayMicros(unsigned long delay_us, unsigned long start_us = 0){ + if (delay_us){ + if (!start_us){ + start_us = micros(); + } + if (delay_us > MIN_YIELD_MICROS){ + yield(); + } + // See https://www.gammon.com.au/millis + while (micros() - start_us < delay_us); + } + } + +private: + // calculation remainder to be fed into successive steps to increase accuracy (Atmel DOC8017) + long rest; + unsigned long last_action_end = 0; + unsigned long next_action_interval = 0; + +protected: + /* + * Motor Configuration + */ + short motor_steps; // motor steps per revolution (usually 200) + + /* + * Driver Configuration + */ + short dir_pin; + short step_pin; + short enable_pin = PIN_UNCONNECTED; + short enable_active_state = HIGH; + // Get max microsteps supported by the device + virtual short getMaxMicrostep(); + // current microstep level (1,2,4,8,...), must be < getMaxMicrostep() + short microsteps = 1; + // tWH(STEP) pulse duration, STEP high, min value (us) + static const int step_high_min = 1; + // tWL(STEP) pulse duration, STEP low, min value (us) + static const int step_low_min = 1; + // tWAKE wakeup time, nSLEEP inactive to STEP (us) + static const int wakeup_time = 0; + + float rpm = 0; + + /* + * Movement state + */ + struct Profile profile; + + long step_count; // current position + long steps_remaining; // to complete the current move (absolute value) + long steps_to_cruise; // steps to reach cruising (max) rpm + long steps_to_brake; // steps needed to come to a full stop + long step_pulse; // step pulse duration (microseconds) + long cruise_step_pulse; // step pulse duration for constant speed section (max rpm) + + // DIR pin state + short dir_state; + + void calcStepPulse(void); + + // this is internal because one can call the start methods while CRUISING to get here + void alterMove(long steps); + +private: + // microstep range (1, 16, 32 etc) + static const short MAX_MICROSTEP = 128; + +public: + /* + * Basic connection: DIR, STEP are connected. + */ + BasicStepperDriver(short steps, short dir_pin, short step_pin); + BasicStepperDriver(short steps, short dir_pin, short step_pin, short enable_pin); + /* + * Initialize pins, calculate timings etc + */ + void begin(float rpm=60, short microsteps=1); + /* + * Set current microstep level, 1=full speed, 32=fine microstepping + * Returns new level or previous level if value out of range + */ + virtual short setMicrostep(short microsteps); + short getMicrostep(void){ + return microsteps; + } + short getSteps(void){ + return motor_steps; + } + /* + * Set target motor RPM (1-200 is a reasonable range) + */ + void setRPM(float rpm); + float getRPM(void){ + return rpm; + }; + float getCurrentRPM(void){ + return (60.0*1000000L / step_pulse / microsteps / motor_steps); + } + /* + * Set speed profile - CONSTANT_SPEED, LINEAR_SPEED (accelerated) + * accel and decel are given in [full steps/s^2] + */ + void setSpeedProfile(Mode mode, short accel=1000, short decel=1000); + void setSpeedProfile(struct Profile profile); + struct Profile getSpeedProfile(void){ + return profile; + } + short getAcceleration(void){ + return profile.accel; + } + short getDeceleration(void){ + return profile.decel; + } + /* + * Move the motor a given number of steps. + * positive to move forward, negative to reverse + */ + void move(long steps); + /* + * Rotate the motor a given number of degrees (1-360) + */ + void rotate(long deg); + inline void rotate(int deg){ + rotate((long)deg); + }; + /* + * Rotate using a float or double for increased movement precision. + */ + void rotate(double deg); + /* + * Configure which logic state on ENABLE pin means active + * when using SLEEP (default) this is active HIGH + */ + void setEnableActiveState(short state); + /* + * Turn off/on motor to allow the motor to be moved by hand/hold the position in place + */ + virtual void enable(void); + virtual void disable(void); + /* + * Methods for non-blocking mode. + * They use more code but allow doing other operations between impulses. + * The flow has two parts - start/initiate followed by looping with nextAction. + * See NonBlocking example. + */ + /* + * Initiate a move over known distance (calculate and save the parameters) + * Pick just one based on move type and distance type. + * If time (microseconds) is given, the driver will attempt to execute the move in exactly that time + * by altering rpm for this move only (up to preset rpm). + */ + void startMove(long steps, long time=0); + inline void startRotate(int deg){ + startRotate((long)deg); + }; + void startRotate(long deg); + void startRotate(double deg); + /* + * Toggle step at the right time and return time until next change is needed (micros) + */ + long nextAction(void); + /* + * Optionally, call this to begin braking (and then stop) early + * For constant speed, this is the same as stop() + */ + void startBrake(void); + /* + * Immediate stop + * Returns the number of steps remaining. + */ + long stop(void); + /* + * State querying + */ + enum State getCurrentState(void); + /* + * Get the number of completed steps so far. + * This is always a positive number + */ + long getStepsCompleted(void){ + return step_count; + } + /* + * Get the number of steps remaining to complete the move + * This is always a positive number + */ + long getStepsRemaining(void){ + return steps_remaining; + } + /* + * Get movement direction: forward +1, back -1 + */ + int getDirection(void){ + return (dir_state == HIGH) ? 1 : -1; + } + /* + * Return calculated time to complete the given move + */ + long getTimeForMove(long steps); + /* + * Calculate steps needed to rotate requested angle, given in degrees + */ + long calcStepsForRotation(long deg){ + return deg * motor_steps * (long)microsteps * 4 / 360; // times 4 because of 1:4 gear ratio + } + long calcStepsForRotation(double deg){ + return deg * motor_steps * microsteps * 4 / 360; // times 4 because of 1:4 gear ratio + } +}; +#endif // STEPPER_DRIVER_BASE_H diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/DRV8825.cpp b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8825.cpp new file mode 100644 index 0000000..d0800b7 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8825.cpp @@ -0,0 +1,49 @@ +/* + * DRV8825 - Stepper Motor Driver Driver + * Indexer mode only. + + * Copyright (C)2015 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include "DRV8825.h" + +/* + * Microstepping resolution truth table (Page 13 of DRV8825 pdf) + * 0bMODE2,MODE1,MODE0 for 1,2,4,8,16,32 microsteps + */ +const uint8_t DRV8825::MS_TABLE[] = {0b000, 0b001, 0b010, 0b011, 0b100, 0b111}; + +DRV8825::DRV8825(short steps, short dir_pin, short step_pin) +:A4988(steps, dir_pin, step_pin) +{} + +DRV8825::DRV8825(short steps, short dir_pin, short step_pin, short enable_pin) +:A4988(steps, dir_pin, step_pin, enable_pin) +{} + +/* + * A4988-DRV8825 Compatibility map: MS1-MODE0, MS2-MODE1, MS3-MODE2 + */ +DRV8825::DRV8825(short steps, short dir_pin, short step_pin, short mode0_pin, short mode1_pin, short mode2_pin) +:A4988(steps, dir_pin, step_pin, mode0_pin, mode1_pin, mode2_pin) +{} + +DRV8825::DRV8825(short steps, short dir_pin, short step_pin, short enable_pin, short mode0_pin, short mode1_pin, short mode2_pin) +:A4988(steps, dir_pin, step_pin, enable_pin, mode0_pin, mode1_pin, mode2_pin) +{} + +const uint8_t* DRV8825::getMicrostepTable() +{ + return (uint8_t*)DRV8825::MS_TABLE; +} + +size_t DRV8825::getMicrostepTableSize() +{ + return sizeof(DRV8825::MS_TABLE); +} + +short DRV8825::getMaxMicrostep(){ + return DRV8825::MAX_MICROSTEP; +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/DRV8825.h b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8825.h new file mode 100644 index 0000000..68afe8f --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8825.h @@ -0,0 +1,43 @@ +/* + * DRV8825 - Stepper Motor Driver Driver (A4988-compatible) + * Indexer mode only. + * + * Copyright (C)2015 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#ifndef DRV8825_H +#define DRV8825_H +#include +#include "A4988.h" + +class DRV8825 : public A4988 { +protected: + static const uint8_t MS_TABLE[]; + // tWH(STEP) pulse duration, STEP high, min value (1.9us) + static const int step_high_min = 2; + // tWL(STEP) pulse duration, STEP low, min value (1.9us) + static const int step_low_min = 2; + // tWAKE wakeup time, nSLEEP inactive to STEP (1000us) + static const int wakeup_time = 1700; + // also 650ns between ENBL/DIR/MODEx changes and STEP HIGH + + // Get the microstep table + const uint8_t* getMicrostepTable() override; + size_t getMicrostepTableSize() override; + + // Get max microsteps supported by the device + short getMaxMicrostep() override; + +private: + // microstep range (1, 16, 32 etc) + static const short MAX_MICROSTEP = 32; + +public: + DRV8825(short steps, short dir_pin, short step_pin); + DRV8825(short steps, short dir_pin, short step_pin, short enable_pin); + DRV8825(short steps, short dir_pin, short step_pin, short mode0_pin, short mode1_pin, short mode2_pin); + DRV8825(short steps, short dir_pin, short step_pin, short enable_pin, short mode0_pin, short mode1_pin, short mode2_pin); +}; +#endif // DRV8825_H diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/DRV8834.cpp b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8834.cpp new file mode 100644 index 0000000..6a048c3 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8834.cpp @@ -0,0 +1,85 @@ +/* + * DRV8834 - LV Stepper Motor Driver Driver (A4988-compatible - mostly) + * Indexer mode only. + + * Copyright (C)2015 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include "DRV8834.h" + +/* + * Basic connection: only DIR, STEP are connected. + * Microstepping controls should be hardwired. + */ +DRV8834::DRV8834(short steps, short dir_pin, short step_pin) +:BasicStepperDriver(steps, dir_pin, step_pin) +{} + +DRV8834::DRV8834(short steps, short dir_pin, short step_pin, short enable_pin) +:BasicStepperDriver(steps, dir_pin, step_pin, enable_pin) +{} + +/* + * Fully wired. All the necessary control pins for DRV8834 are connected. + */ +DRV8834::DRV8834(short steps, short dir_pin, short step_pin, short m0_pin, short m1_pin) +:BasicStepperDriver(steps, dir_pin, step_pin), m0_pin(m0_pin), m1_pin(m1_pin) +{} + +DRV8834::DRV8834(short steps, short dir_pin, short step_pin, short enable_pin, short m0_pin, short m1_pin) +:BasicStepperDriver(steps, dir_pin, step_pin, enable_pin), m0_pin(m0_pin), m1_pin(m1_pin) +{} + +/* + * Set microstepping mode (1:divisor) + * Allowed ranges for DRV8834 are 1:1 to 1:32 + * If the control pins are not connected, we recalculate the timing only + * + */ +short DRV8834::setMicrostep(short microsteps){ + BasicStepperDriver::setMicrostep(microsteps); + + if (!IS_CONNECTED(m0_pin) || !IS_CONNECTED(m1_pin)){ + return this->microsteps; + } + + /* + * Step mode truth table + * M1 M0 step mode + * 0 0 1 + * 0 1 2 + * 0 Z 4 + * 1 0 8 + * 1 1 16 + * 1 Z 32 + * + * Z = high impedance mode (M0 is tri-state) + */ + + pinMode(m1_pin, OUTPUT); + digitalWrite(m1_pin, (this->microsteps < 8) ? LOW : HIGH); + + switch(this->microsteps){ + case 1: + case 8: + pinMode(m0_pin, OUTPUT); + digitalWrite(m0_pin, LOW); + break; + case 2: + case 16: + pinMode(m0_pin, OUTPUT); + digitalWrite(m0_pin, HIGH); + break; + case 4: + case 32: + pinMode(m0_pin, INPUT); // Z - high impedance + break; + } + return this->microsteps; +} + +short DRV8834::getMaxMicrostep(){ + return DRV8834::MAX_MICROSTEP; +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/DRV8834.h b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8834.h new file mode 100644 index 0000000..7aeeec6 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8834.h @@ -0,0 +1,48 @@ +/* + * DRV8834 - LV Stepper Motor Driver Driver (A4988-compatible - mostly) + * Indexer mode only. + * + * Copyright (C)2015 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#ifndef DRV8834_H +#define DRV8834_H +#include +#include "BasicStepperDriver.h" + +class DRV8834 : public BasicStepperDriver { +protected: + short m0_pin = PIN_UNCONNECTED; + short m1_pin = PIN_UNCONNECTED; + // tWH(STEP) pulse duration, STEP high, min value (1.9us) + static const int step_high_min = 2; + // tWL(STEP) pulse duration, STEP low, min value (1.9us) + static const int step_low_min = 2; + // tWAKE wakeup time, nSLEEP inactive to STEP (1000us) + static const int wakeup_time = 1000; + // also 200ns between ENBL/DIR/Mx changes and STEP HIGH + + // Get max microsteps supported by the device + short getMaxMicrostep() override; + +private: + // microstep range (1, 16, 32 etc) + static const short MAX_MICROSTEP = 32; + +public: + /* + * Basic connection: only DIR, STEP are connected. + * Microstepping controls should be hardwired. + */ + DRV8834(short steps, short dir_pin, short step_pin); + DRV8834(short steps, short dir_pin, short step_pin, short enable_pin); + /* + * Fully wired. All the necessary control pins for DRV8834 are connected. + */ + DRV8834(short steps, short dir_pin, short step_pin, short m0_pin, short m1_pin); + DRV8834(short steps, short dir_pin, short step_pin, short enable_pin, short m0_pin, short m1_pin); + short setMicrostep(short microsteps) override; +}; +#endif // DRV8834_H diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/DRV8880.cpp b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8880.cpp new file mode 100644 index 0000000..5bae6a8 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8880.cpp @@ -0,0 +1,121 @@ +/* + * DRV8880 - 2A Stepper Motor Driver with AutoTune and Torque Control + * + * Copyright (C)2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include "DRV8880.h" + +/* + * Basic connection: only DIR, STEP are connected. + * Microstepping controls should be hardwired. + */ +DRV8880::DRV8880(short steps, short dir_pin, short step_pin) +:BasicStepperDriver(steps, dir_pin, step_pin) +{} + +DRV8880::DRV8880(short steps, short dir_pin, short step_pin, short enable_pin) +:BasicStepperDriver(steps, dir_pin, step_pin, enable_pin) +{} + +/* + * Fully wired. All the necessary control pins for DRV8880 are connected. + */ +DRV8880::DRV8880(short steps, short dir_pin, short step_pin, short m0, short m1) +:BasicStepperDriver(steps, dir_pin, step_pin), m0(m0), m1(m1) +{} + +DRV8880::DRV8880(short steps, short dir_pin, short step_pin, short enable_pin, short m0, short m1) +:BasicStepperDriver(steps, dir_pin, step_pin, enable_pin), m0(m0), m1(m1) +{} + +DRV8880::DRV8880(short steps, short dir_pin, short step_pin, short m0, short m1, short trq0, short trq1) +:BasicStepperDriver(steps, dir_pin, step_pin), m0(m0), m1(m1), trq0(trq0), trq1(trq1) +{} + +DRV8880::DRV8880(short steps, short dir_pin, short step_pin, short enable_pin, short m0, short m1, short trq0, short trq1) +:BasicStepperDriver(steps, dir_pin, step_pin, enable_pin), m0(m0), m1(m1), trq0(trq0), trq1(trq1) +{} + +void DRV8880::begin(float rpm, short microsteps){ + BasicStepperDriver::begin(rpm, microsteps); + setCurrent(100); +} + +short DRV8880::getMaxMicrostep(){ + return DRV8880::MAX_MICROSTEP; +} + +/* + * Set microstepping mode (1:divisor) + * Allowed ranges for DRV8880 are 1:1 to 1:16 + * If the control pins are not connected, we recalculate the timing only + */ +short DRV8880::setMicrostep(short microsteps){ + BasicStepperDriver::setMicrostep(microsteps); + + if (!IS_CONNECTED(m0) || !IS_CONNECTED(m1)){ + return this->microsteps; + } + + /* + * Step mode truth table + * M1 M0 step mode + * 0 0 1 + * 1 0 2 + * 1 1 4 + * 0 Z 8 + * 1 Z 16 + * + * 0 1 2 (non-circular, not implemented) + * Z = high impedance mode (M0 is tri-state) + */ + + pinMode(m1, OUTPUT); + pinMode(m0, OUTPUT); + switch(this->microsteps){ + case 1: + digitalWrite(m1, LOW); + digitalWrite(m0, LOW); + break; + case 2: + digitalWrite(m1, HIGH); + digitalWrite(m0, LOW); + break; + case 4: + digitalWrite(m1, HIGH); + digitalWrite(m0, HIGH); + break; + case 8: + digitalWrite(m1, LOW); + pinMode(m0, INPUT); // Z - high impedance + break; + case 16: + digitalWrite(m1, HIGH); + pinMode(m0, INPUT); // Z - high impedance + break; + } + return this->microsteps; +} + +void DRV8880::setCurrent(short percent){ + /* + * Torque DAC Settings table + * TRQ1 TRQ0 Current scalar + * 1 1 25% + * 1 0 50% + * 0 1 75% + * 0 0 100% + */ + if (!IS_CONNECTED(trq1) || !IS_CONNECTED(trq0)){ + return; + } + pinMode(trq1, OUTPUT); + pinMode(trq0, OUTPUT); + percent = (100-percent)/25; + digitalWrite(trq1, percent & 2); + digitalWrite(trq0, percent & 1); +} + diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/DRV8880.h b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8880.h new file mode 100644 index 0000000..15c7722 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/DRV8880.h @@ -0,0 +1,63 @@ +/* + * DRV8880 - 2A Stepper Motor Driver with AutoTune and Torque Control + * + * Copyright (C)2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#ifndef DRV8880_H +#define DRV8880_H +#include +#include "BasicStepperDriver.h" + +class DRV8880 : public BasicStepperDriver { +protected: + short m0 = PIN_UNCONNECTED; + short m1 = PIN_UNCONNECTED; + short trq0 = PIN_UNCONNECTED; + short trq1 = PIN_UNCONNECTED; + // tWH(STEP) pulse duration, STEP high, min value + static const int step_high_min = 0; // 0.47us + // tWL(STEP) pulse duration, STEP low, min value + static const int step_low_min = 0; // 0.47us + // tWAKE wakeup time, nSLEEP inactive to STEP + static const int wakeup_time = 1500; + // also 200ns between ENBL/DIR/Mx changes and STEP HIGH + + // Get max microsteps supported by the device + short getMaxMicrostep() override; + +private: + // microstep range (1, 16, 32 etc) + static const short MAX_MICROSTEP = 16; + +public: + /* + * Basic connection: only DIR, STEP are connected. + * Microstepping controls should be hardwired. + */ + DRV8880(short steps, short dir_pin, short step_pin); + DRV8880(short steps, short dir_pin, short step_pin, short enable_pin); + /* + * DIR, STEP and microstep control M0, M1 + */ + DRV8880(short steps, short dir_pin, short step_pin, short m0, short m1); + DRV8880(short steps, short dir_pin, short step_pin, short enable_pin, short m0, short m1); + /* + * Fully Wired - DIR, STEP, microstep and current control + */ + DRV8880(short steps, short dir_pin, short step_pin, short m0, short m1, short trq0, short trq1); + DRV8880(short steps, short dir_pin, short step_pin, short enable_pin, short m0, short m1, short trq0, short trq1); + + void begin(float rpm=60, short microsteps=1); + + short setMicrostep(short microsteps) override; + + /* + * Torque DAC Control + * current percent value must be 25, 50, 75 or 100. + */ + void setCurrent(short percent=100); +}; +#endif // DRV8880_H diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/MultiDriver.cpp b/WaveGen/Arduino/libraries/StepperDriver/src/MultiDriver.cpp new file mode 100644 index 0000000..e05cbe1 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/MultiDriver.cpp @@ -0,0 +1,149 @@ +/* + * Multi-motor group driver + * + * Copyright (C)2017-2019 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include "MultiDriver.h" + +#define FOREACH_MOTOR(action) for (short i=count-1; i >= 0; i--){action;} + +/* + * Initialize motor parameters + */ +void MultiDriver::startMove(long steps1, long steps2, long steps3){ + long steps[3] = {steps1, steps2, steps3}; + /* + * Initialize state for all active motors + */ + FOREACH_MOTOR( + if (steps[i]){ + motors[i]->startMove(steps[i]); + event_timers[i] = 1; + } else { + event_timers[i] = 0; + } + ); + ready = false; + last_action_end = 0; + next_action_interval = 1; +} +/* + * Trigger next step action + */ +long MultiDriver::nextAction(void){ + Motor::delayMicros(next_action_interval, last_action_end); + + // TODO: unroll these loops + // Trigger all the motors that need it + FOREACH_MOTOR( + if (event_timers[i] <= next_action_interval){ + event_timers[i] = motors[i]->nextAction(); + } else { + event_timers[i] -= next_action_interval; + } + ); + last_action_end = micros(); + + next_action_interval = 0; + // Find the time when the next pulse needs to fire + // this is the smallest non-zero timer value from all active motors + FOREACH_MOTOR( + if (event_timers[i] > 0 && (event_timers[i] < next_action_interval || next_action_interval == 0)){ + next_action_interval = event_timers[i]; + } + ); + ready = (next_action_interval == 0); + + return next_action_interval; +} +/* + * Optionally, call this to begin braking to stop early + */ +void MultiDriver::startBrake(void){ + FOREACH_MOTOR( + if (event_timers[i] > 0){ + motors[i]->startBrake(); + } + ) +} +/* + * Immediate stop + * Returns the number of steps remaining. + */ +MultiDriver::Steps MultiDriver::stop(void){ + Steps retval = Steps(); + FOREACH_MOTOR( + if (event_timers[i] > 0){ + retval.steps[i] = motors[i]->stop(); + } + ) + return retval; +} +/* + * State querying + */ +bool MultiDriver::isRunning(void){ + bool running = false; + FOREACH_MOTOR( + if (motors[i]->getCurrentState() != Motor::STOPPED){ + running = true; + break; + } + ) + return running; +} + +/* + * Initialize pins, calculate timings etc + */ +void MultiDriver::begin(float rpm, short microsteps){ + FOREACH_MOTOR( + motors[i]->begin(rpm, microsteps); + ) +} + +/* + * Move each motor the requested number of steps, in parallel + * positive to move forward, negative to reverse, 0 to remain still + */ +void MultiDriver::move(long steps1, long steps2, long steps3){ + startMove(steps1, steps2, steps3); + while (!ready){ + nextAction(); + } +} + +#define CALC_STEPS(i, deg) ((motors[i] && deg) ? motors[i]->calcStepsForRotation(deg) : 0) +void MultiDriver::rotate(long deg1, long deg2, long deg3){ + move(CALC_STEPS(0, deg1), CALC_STEPS(1, deg2), CALC_STEPS(2, deg3)); +} + +void MultiDriver::rotate(double deg1, double deg2, double deg3){ + move(CALC_STEPS(0, deg1), CALC_STEPS(1, deg2), CALC_STEPS(2, deg3)); +} + +void MultiDriver::startRotate(long deg1, long deg2, long deg3){ + startMove(CALC_STEPS(0, deg1), CALC_STEPS(1, deg2), CALC_STEPS(2, deg3)); +} + +void MultiDriver::startRotate(double deg1, double deg2, double deg3){ + startMove(CALC_STEPS(0, deg1), CALC_STEPS(1, deg2), CALC_STEPS(2, deg3)); +} + +void MultiDriver::setMicrostep(unsigned microsteps){ + FOREACH_MOTOR(motors[i]->setMicrostep(microsteps)); +} + +void MultiDriver::setRPM(float rpm){ + FOREACH_MOTOR(motors[i]->setRPM(rpm)); +} + +void MultiDriver::enable(void){ + FOREACH_MOTOR(motors[i]->enable()); +} +void MultiDriver::disable(void){ + FOREACH_MOTOR(motors[i]->disable()); +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/MultiDriver.h b/WaveGen/Arduino/libraries/StepperDriver/src/MultiDriver.h new file mode 100644 index 0000000..b947adc --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/MultiDriver.h @@ -0,0 +1,121 @@ +/* + * Multi-motor group driver + * + * Copyright (C)2017 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#ifndef MULTI_DRIVER_H +#define MULTI_DRIVER_H +#include +#include "BasicStepperDriver.h" + +#define MAX_MOTORS 3 // a reasonable but arbitrary limit +#define Motor BasicStepperDriver +/* + * Multi-motor group driver class. + */ +class MultiDriver { +protected: + /* + * Configuration + */ + unsigned short count; + Motor* const *motors; + /* + * Generic initializer, will be called by the others + */ + MultiDriver(const unsigned short count, Motor* const *motors) + :count(count), motors(motors) + {}; + + /* + * Movement state + */ + // ready to start a new move + bool ready = true; + // when next state change is due for each motor + unsigned long event_timers[MAX_MOTORS]; + unsigned long next_action_interval = 0; + unsigned long last_action_end = 0; + +public: + struct Steps { + long steps[3]; + }; + /* + * Two-motor setup + */ + MultiDriver(Motor& motor1, Motor& motor2) + :MultiDriver(2, new Motor* const[2]{&motor1, &motor2}) + {}; + /* + * Three-motor setup (X, Y, Z for example) + */ + MultiDriver(Motor& motor1, Motor& motor2, Motor& motor3) + :MultiDriver(3, new Motor* const[3]{&motor1, &motor2, &motor3}) + {}; + unsigned short getCount(void){ + return count; + } + Motor& getMotor(short index){ + return *motors[index]; + } + /* + * Initialize pins, calculate timings etc + */ + void begin(float rpm=60, short microsteps=1); + /* + * Move the motors a given number of steps. + * positive to move forward, negative to reverse + */ + void move(long steps1, long steps2, long steps3=0); + void rotate(int deg1, int deg2, int deg3=0){ + rotate((long)deg1, (long)deg2, (long)deg3); + }; + void rotate(long deg1, long deg2, long deg3=0); + void rotate(double deg1, double deg2, double deg3=0); + + /* + * Motor movement with external control of timing + */ + virtual void startMove(long steps1, long steps2, long steps3=0); + void startRotate(int deg1, int deg2, int deg3=0){ + startRotate((long)deg1, (long)deg2, (long)deg3); + }; + void startRotate(long deg1, long deg2, long deg3=0); + void startRotate(double deg1, double deg2, double deg3=0); + /* + * Toggle step and return time until next change is needed (micros) + */ + virtual long nextAction(void); + /* + * Optionally, call this to begin braking to stop early + */ + void startBrake(void); + /* + * Immediate stop + * Returns the number of steps remaining. + */ + Steps stop(void); + /* + * State querying + */ + bool isRunning(void); + + /* + * Set the same microstepping level on all motors + */ + void setMicrostep(unsigned microsteps); + /* + * Set all motors RPM (1-200 is a reasonable range) + */ + void setRPM(float rpm); + /* + * Turn all motors on or off + */ + void enable(void); + void disable(void); +}; +#endif // MULTI_DRIVER_H diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/SyncDriver.cpp b/WaveGen/Arduino/libraries/StepperDriver/src/SyncDriver.cpp new file mode 100644 index 0000000..80280a7 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/SyncDriver.cpp @@ -0,0 +1,43 @@ +/* + * Synchronous Multi-motor group driver + * All motors reach their target at the same time. + * + * Copyright (C)2017-2019 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include "SyncDriver.h" + +#define FOREACH_MOTOR(action) for (short i=count-1; i >= 0; i--){action;} + +/* + * Initialize motor parameters + */ +void SyncDriver::startMove(long steps1, long steps2, long steps3){ + long steps[3] = {steps1, steps2, steps3}; + /* + * find which motor would take the longest to finish, + */ + long move_time = 0; + FOREACH_MOTOR( + long m = motors[i]->getTimeForMove(labs(steps[i])); + if (m > move_time){ + move_time = m; + } + ); + /* + * Initialize state for all active motors to complete with micros + */ + FOREACH_MOTOR( + if (steps[i]){ + motors[i]->startMove(steps[i], move_time); + event_timers[i] = 1; + } else { + event_timers[i] = 0; + } + ); + ready = false; + last_action_end = 0; + next_action_interval = 1; +} diff --git a/WaveGen/Arduino/libraries/StepperDriver/src/SyncDriver.h b/WaveGen/Arduino/libraries/StepperDriver/src/SyncDriver.h new file mode 100644 index 0000000..dc74e0f --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/src/SyncDriver.h @@ -0,0 +1,26 @@ +/* + * Synchronous Multi-motor group driver + * All motors reach their target at the same time. + * + * Copyright (C)2017-2019 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#ifndef SYNC_DRIVER_H +#define SYNC_DRIVER_H +#include +#include "MultiDriver.h" + +/* + * Synchronous Multi-motor group driver class. + * This driver sets up timing so all motors reach their target at the same time. + */ +class SyncDriver : public MultiDriver { + using MultiDriver::MultiDriver; + +public: + + void startMove(long steps1, long steps2, long steps3=0) override; +}; +#endif // SYNC_DRIVER_H diff --git a/WaveGen/Arduino/libraries/StepperDriver/test/README b/WaveGen/Arduino/libraries/StepperDriver/test/README new file mode 100644 index 0000000..b94d089 --- /dev/null +++ b/WaveGen/Arduino/libraries/StepperDriver/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Unit Testing and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/page/plus/unit-testing.html diff --git a/WaveGen/README.md b/WaveGen/README.md new file mode 100644 index 0000000..b4a2a03 --- /dev/null +++ b/WaveGen/README.md @@ -0,0 +1,41 @@ +# Wave genereator control code + +The wave generator has a [motor](https://www.omc-stepperonline.com/de/p-series-ip67-wasserdicht-nema-23-schrittmotor-5-0a-1-8nm-254-95oz-in-23ip67-20) and a [stepper driver](https://www.omc-stepperonline.com/digital-stepper-driver-1-0-4-2a-20-50vdc-for-nema-17-23-24-stepper-motor-dm542t). + +# Arduino lib + +The stepper driver is controlled by a arduino nano. The code is running a stepper [library](https://docs.arduino.cc/libraries/stepperdriver/). +For information on installing libraries, [here](http://www.arduino.cc/en/Guide/Libraries). + +# Python code + +| Argument | Description | Type | +|------------|-----------------------------------|--------| +| `angle` | turns around a certain angle [deg]| int | +| `rpm` | Sets the rpm (default 60) | Float | +| `t` | Limits the rotation time [s] | Float | +| `delay` | Add a delayy before startin [s] | Float | + +## rpm mode + +If only passing rpm the motor will turn until t is reached with the given rpm. + +```sh +pyhton3 WaveGen.py rpm 60 t 5 delay 0 +``` + +## andgle mode + +Sets the angle to turn and the rpm. The motor will turn until the angle is reached. if no rpm given then default 60 is used. + +```sh +pyhton3 WaveGen.py angle 720 rpm 100 delay 0 +``` + +# connection to pi on the WaveGen + +connect to ```tank@tank.local``` after seting up a network conection + + + + diff --git a/WaveGen/WaveGen.py b/WaveGen/WaveGen.py new file mode 100644 index 0000000..2f6a49c --- /dev/null +++ b/WaveGen/WaveGen.py @@ -0,0 +1,86 @@ +import serial +import time +import sys + +# Replace with the correct port and baud rate (e.g., 'COM3' for Windows or '/dev/ttyACM0' for Linux) +PORT = '/dev/ttyUSB0' +BAUD_RATE = 9600 + +rpm = 0.0 +run_time = 0.0 +angle = 0 +delay = 0 + +def send_command(command): + with serial.Serial(PORT, BAUD_RATE, timeout=1) as ser: + time.sleep(2) # Give time for Arduino to reset + + ser.write((command + '\n').encode()) + print(f"Sent: {command}") + while True: + response = ser.readline().decode().strip() + if(response != ""): + print(f"Received: {response}") + if(response == "Finished"): + break + +def parser(): + + global rpm, run_time, angle, delay + + if len(sys.argv) < 2: + print("No inputs given, please use 't', 'rpm' or 'angle'") + return False + + else: + + # get the arguments to the wrigth variabales + for i in range(1, len(sys.argv)): + + if sys.argv[i] == 't': + run_time = sys.argv[i + 1] + print("run_time set to:", run_time) + continue + + elif sys.argv[i] == 'rpm': + rpm = sys.argv[i + 1] + print("rpm set to:", rpm) + continue + + elif sys.argv[i] == 'angle': + angle = sys.argv[i + 1] + print("angle set to:", angle) + continue + elif sys.argv[i] == 'delay': + delay = sys.argv[i + 1] + print("start delay set to: ", delay) + continue + + else: + if sys.argv[i].isdigit(): + continue + + else: + print("wrong input option, please use 't', 'rpm' or 'angle', %s is not valid" % sys.argv[i]) + + return True + + + + +if __name__ == '__main__': + + is_cmd = parser() + + if not is_cmd: + sys.exit(1) + + run_time = float(run_time) * 1000 # Convert to milliseconds + + # time in ms + cmd = str("t " + str(run_time) + " rpm " + str(rpm) + " a " + str(angle)) + " delay " + str(delay) + + print("Command to be sent: ", cmd) + + # Send the command to the Arduino + send_command(cmd)