d9888ef369
* Improved homing (parallel homing support, better repeatability, better geometric reference point) * Improved joint calibration procedure * Calibration data can now be stored persistently on the flash memory (no repeated calibration required) * Improved logging * added PythonAPI to control device easily New G-Code commands: * Enable/Disable motors command, including pose recovery from current position on motor enable * Dedicated joint calibration command with save to flash option * Set pose command to directly set a target pose for the servo loops, bypassing the motion controller (good for real-time control)
76 lines
2.4 KiB
C++
76 lines
2.4 KiB
C++
// --------------------------------------------------------------------------------------
|
|
// Project: MicroManipulatorStepper
|
|
// License: MIT (see LICENSE file for full description)
|
|
// All text in here must be included in any redistribution.
|
|
// Author: M. S. (diffraction limited)
|
|
// --------------------------------------------------------------------------------------
|
|
|
|
#pragma once
|
|
|
|
#include "servo_controller.h"
|
|
|
|
//*** CLASS *****************************************************************************
|
|
|
|
/**
|
|
* This class implements the homing procedure for a single actuator. To allow for parallel
|
|
* homing the class is stateful and has an update() function, that can be called togeather
|
|
* with the updates of other homing controllers inside a loop.
|
|
*
|
|
* Homing procedure:
|
|
* 1. move axis in negative direction until a physical hard stop is reached
|
|
* 2. reset encoder period
|
|
* 3. back off slightly from the hard stop
|
|
*/
|
|
class HomingController {
|
|
public:
|
|
HomingController();
|
|
|
|
// Starts the homing cycle, motor_velocity can be negative and defines the homing direction.
|
|
// WARNING: Servo loop updates (including encoder reads) must be completely disabled during homing.
|
|
bool run_blocking(ServoController* servo_controller, float motor_velocity, float search_range, float current);
|
|
void start(ServoController* servo_controller, float motor_velocity, float search_range, float current);
|
|
void update();
|
|
void finalize();
|
|
|
|
bool is_finished() const;
|
|
bool is_successful() const;
|
|
float get_home_encoder_angle() const;
|
|
|
|
private:
|
|
void on_endstop_detected();
|
|
float compute_eval_pos_delta(float pos, float field_angle_delta);
|
|
|
|
private:
|
|
enum class State {
|
|
Idle,
|
|
Initializing,
|
|
Homing,
|
|
Done
|
|
};
|
|
|
|
ServoController* servo_ctrl;
|
|
|
|
// Configuration params
|
|
float field_velocity = 0.0f; // defines homing direction
|
|
float field_angle_search_range = 0.0f;
|
|
float homing_current = 0.0f;
|
|
float initial_current = 0.0f;
|
|
float retract_field_velocity = 0.0f;
|
|
|
|
// State machine
|
|
State state = State::Idle;
|
|
bool search_failed = false;
|
|
|
|
// Timing
|
|
uint64_t last_time = 0;
|
|
|
|
// Offsets and tracking
|
|
float start_field_angle = 0.0f;
|
|
float field_angle_offset = 0.0f;
|
|
float last_eval_field_angle_offset = 0.0f;
|
|
float last_eval_encoder_angle = 0.0f;
|
|
float eval_field_angle_delta = 0.0f;
|
|
float expected_encoder_delta = 0.0f;
|
|
|
|
float home_encoder_angle = 0.0f;
|
|
};
|