Koduino
|
Base single-motor class. More...
#include <Motor.h>
Base single-motor class.
Implements enable/disable, setting open-loop commands, PD position control, gear ratio. This class is an abstract base class, and derived classes need to implement the low-level communication with the motor driver.
Public Member Functions | |
virtual void | enable (bool flag)=0 |
Enable the motor. More... | |
virtual float | getRawPosition ()=0 |
Raw position read from the motor driver. More... | |
virtual void | sendOpenLoop (float val)=0 |
Communicate a raw PWM to the motor controller. More... | |
void | init (float zero, int8_t direction, float gearRatio) |
Initialize motor properties. More... | |
float | getPosition () |
Get position after direction and zero are taken into account. More... | |
float | getVelocity () |
Get motor velocity after direction and zero are taken into account. More... | |
float | getOpenLoop () |
Get the PWM duty cycle commanded to the motor. More... | |
void | setTorqueEstParams (float Kt, float res, float Vsource, float curLim) |
Details for torque estimate in getTorque(). Not used elsewhere. More... | |
float | getTorque () |
Returns an estimate of torque assuming quasistatic. More... | |
void | setOpenLoop (float val) |
Set an open loop PWM command for the motor. More... | |
void | setGain (float Kp, float Kd) |
Set gain for position control. More... | |
void | setGain (float Kp) |
Set P-gain for position control (D-gain is set to 0) More... | |
void | setPosition (float setpoint) |
Set a desired position for the motor in radians. More... | |
float | update () |
Function that communicates with the motor controller (including getting position, commanding PWM) More... | |
void | resetOffset () |
Run this after you are sure there is non-garbage rawPosition data. | |
Static Public Attributes | |
static int | updateRate = 1000 |
Set expected rate (Hz) at which update() will be called. More... | |
static float | velSmooth = 0.8 |
Set smoothing factor for getVelocity() (0 is no smoothing, 1 never updates) | |
Inherited by BlCon34.
|
pure virtual |
Enable the motor.
Must be implemented in derived classes
flag | true to enable, false to disable |
Implemented in BlCon34.
|
inline |
Get the PWM duty cycle commanded to the motor.
Useful to record input when getPosition() is used
float Motor::getPosition | ( | ) |
Get position after direction and zero are taken into account.
|
pure virtual |
Raw position read from the motor driver.
This is before the direction/zero are applied. The user should call getPosition()
Implemented in BlCon34.
|
inline |
Returns an estimate of torque assuming quasistatic.
Need to call setTorqueEstParams() beforhand
|
inline |
Get motor velocity after direction and zero are taken into account.
void Motor::init | ( | float | zero, |
int8_t | direction, | ||
float | gearRatio | ||
) |
Initialize motor properties.
zero | The zero angle (in terms of getRawPosition()) |
direction | One of +1 and -1 (reverses in which direction positions increase) |
gearRatio | Default is 1.0 (direct drive) |
|
pure virtual |
Communicate a raw PWM to the motor controller.
Must be implemented in derived classes. The user should call setOpenLoop()
val | Raw PWM in [-1, 1] |
Implemented in BlCon34.
void Motor::setGain | ( | float | Kp, |
float | Kd | ||
) |
Set gain for position control.
This must be called when setPosition() is used
Kp | P-gain (in units of PWM / rad) |
Kd | D-gain (in units of PWM / (rad/s)) |
|
inline |
Set P-gain for position control (D-gain is set to 0)
This must be called when setPosition() is used
Kp | P-gain (in units of PWM / rad) |
void Motor::setOpenLoop | ( | float | val | ) |
Set an open loop PWM command for the motor.
These commands are only sent by update()
val | Commanded PWM in [-1, 1] |
void Motor::setPosition | ( | float | setpoint | ) |
Set a desired position for the motor in radians.
setGain() needs to be called as well
setpoint | Desired position in radians (relative to direction and zero set by Motor::init()) |
void Motor::setTorqueEstParams | ( | float | Kt, |
float | res, | ||
float | Vsource, | ||
float | curLim | ||
) |
Details for torque estimate in getTorque(). Not used elsewhere.
Kt | in Nm/A |
res | in Ohms |
Vsource | in Volts |
float Motor::update | ( | ) |
Function that communicates with the motor controller (including getting position, commanding PWM)
This should be called at a more or less fixed rate (once per iteration). Change Motor::updateRate to the expected frequency that update() will be called.
|
static |