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)
112 lines
No EOL
4.2 KiB
C++
112 lines
No EOL
4.2 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 "hardware/MT6835_encoder.h"
|
|
#include "hardware/TB6612_motor_driver.h"
|
|
#include "utilities/lookup_table.h"
|
|
#include "pid.h"
|
|
|
|
//*** CLASS *****************************************************************************
|
|
|
|
class ServoController {
|
|
public:
|
|
// use defines instead of virtual functions for speed
|
|
// TODO: check if this makes any difference and change accordingly
|
|
typedef TB6612MotorDriver MOTOR_DRIVER_TYPE;
|
|
typedef MT6835Encoder ENCODER_TYPE;
|
|
|
|
public:
|
|
ServoController(MOTOR_DRIVER_TYPE& motor_driver, ENCODER_TYPE& encoder, int32_t motor_pole_pairs);
|
|
|
|
// initialize the servo controller hardware
|
|
void init(float max_motor_amplitude);
|
|
|
|
// set the encoder raw angle to motor position lookup table
|
|
void set_enc_to_pos_lut(LookupTable& lut);
|
|
// get the encoder raw angle to motor position lookup table
|
|
const LookupTable& get_enc_to_pos_lut() const;
|
|
|
|
// set the motor position to field angle lookup table
|
|
void set_pos_to_field_lut(LookupTable& lut);
|
|
// get the motor position to field angle lookup table
|
|
const LookupTable& get_pos_to_field_lut() const;
|
|
|
|
// Updates the servo loop.
|
|
void update(float target_motor_pos,
|
|
float dt,
|
|
float one_over_dt);
|
|
|
|
// Checks if the motor is at position (uses values from previous update() call).
|
|
bool at_position(float motor_pos_eps);
|
|
|
|
// Reads the current motor position from the encoder.
|
|
float read_position();
|
|
|
|
// Returns the motor position.
|
|
float get_position();
|
|
|
|
// Returns the current position error from the last servo loop update.
|
|
float get_position_error();
|
|
|
|
// Moves to a new motor position using closed loop control (blocking).
|
|
bool move_to(float target_motor_angle,
|
|
float at_pos_motor_angle_eps,
|
|
float settle_time_ms,
|
|
float timeout_us);
|
|
|
|
// Moves to a new motor position using open loop controll (blocking).
|
|
// Motor updates must be disabled if servo loop is running in background.
|
|
void move_to_open_loop(float target_motor_angle,
|
|
float angular_velocity);
|
|
|
|
// Returns the encoder object.
|
|
ENCODER_TYPE& get_encoder();
|
|
|
|
// Returns the motor driver object.
|
|
MOTOR_DRIVER_TYPE& get_motor_driver();
|
|
|
|
// Returns number of motor pole pairs.
|
|
// Can be used to approximate conversion of motor position to field angle.
|
|
float get_pole_pair_count();
|
|
|
|
// enable or disable motor
|
|
void set_motor_enabled(bool enable, bool synchronize_field_angle);
|
|
|
|
// enable or disable motor updates
|
|
void set_motor_update_enabled(bool enable);
|
|
|
|
// enable or disable encoder reads
|
|
void set_encoder_update_enabled(bool enable);
|
|
|
|
public:
|
|
float encoder_angle_to_motor_pos(int32_t encoder_angle_raw);
|
|
float motor_pos_to_field_angle(float motor_pos);
|
|
float motor_pos_to_field_angle_derivative(float motor_pos);
|
|
|
|
public:
|
|
LowpassFilter velocity_lowpass;
|
|
PIDController pos_controller;
|
|
PIDController velocity_controller;
|
|
|
|
private:
|
|
ENCODER_TYPE& encoder;
|
|
MOTOR_DRIVER_TYPE& motor_driver;
|
|
LookupTable encoder_raw_to_motor_pos_lut;
|
|
LookupTable motor_pos_to_field_angle_lut;
|
|
|
|
float motor_pole_pair_count = 0.0f; // number as motor pole pairs (as float to avoid repeated conversion)
|
|
float motor_current_amplitude = 0.5f; // motor current in range [0..1]
|
|
float motor_pos = 0; // current motor position
|
|
float motor_pos_prev = 0; // previous motor position
|
|
float pos_error = 0; // current position error as computed by upate()
|
|
float velocity = 0; // current velocity estimate
|
|
float output = 0.0f; // servo loop output (field angle offset)
|
|
bool motor_update_enabled = false; // enables mootor field updates
|
|
bool encoder_update_enabled = true; // enables encoder reads
|
|
}; |