pimoroni-pico/drivers/motor/motor_state.cpp

157 lines
4.8 KiB
C++
Raw Normal View History

#include "motor_state.hpp"
2022-04-05 16:53:36 +01:00
#include "common/pimoroni_common.hpp"
2022-04-04 20:00:03 +01:00
#include "float.h"
2022-04-09 01:41:42 +01:00
#include "math.h"
namespace motor {
2022-04-05 16:53:36 +01:00
MotorState::MotorState()
: motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false),
motor_direction(NORMAL_DIR), motor_scale(DEFAULT_SPEED_SCALE), motor_zeropoint(DEFAULT_ZEROPOINT), motor_deadzone(DEFAULT_DEADZONE){
}
MotorState::MotorState(Direction direction, float speed_scale, float zeropoint, float deadzone)
2022-04-05 16:53:36 +01:00
: motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false),
motor_direction(direction), motor_scale(MAX(speed_scale, FLT_EPSILON)), motor_zeropoint(CLAMP(zeropoint, 0.0f, 1.0f - FLT_EPSILON)), motor_deadzone(CLAMP(deadzone, 0.0f, 1.0f)) {
2022-04-04 20:00:03 +01:00
}
2022-04-04 20:00:03 +01:00
float MotorState::enable_with_return() {
enabled = true;
2022-04-09 01:41:42 +01:00
return get_deadzoned_duty();
}
2022-04-04 20:00:03 +01:00
float MotorState::disable_with_return() {
enabled = false;
2022-04-09 01:41:42 +01:00
return NAN;
}
bool MotorState::is_enabled() const {
return enabled;
}
float MotorState::get_duty() const {
return last_enabled_duty;
}
2022-04-09 01:41:42 +01:00
float MotorState::get_deadzoned_duty() const {
float duty = 0.0f;
if((last_enabled_duty <= 0.0f - motor_deadzone) || (last_enabled_duty >= motor_deadzone)) {
duty = last_enabled_duty;
}
2022-04-11 13:35:53 +01:00
if(enabled)
return duty;
else
return NAN;
2022-04-09 01:41:42 +01:00
}
float MotorState::set_duty_with_return(float duty) {
2022-04-04 20:00:03 +01:00
// Clamp the duty between the hard limits
2022-04-05 16:53:36 +01:00
last_enabled_duty = CLAMP(duty, -1.0f, 1.0f);
if(last_enabled_duty > motor_zeropoint)
motor_speed = map_float(last_enabled_duty, motor_zeropoint, 1.0f, 0.0f, motor_scale);
else if(last_enabled_duty < -motor_zeropoint)
motor_speed = map_float(last_enabled_duty, -motor_zeropoint, -1.0f, 0.0f, -motor_scale);
else
motor_speed = 0.0f;
//motor_speed = last_enabled_duty * motor_scale;
2022-04-04 20:00:03 +01:00
return enable_with_return();
}
float MotorState::get_speed() const {
return (motor_direction == NORMAL_DIR) ? motor_speed : 0.0f - motor_speed;
}
float MotorState::set_speed_with_return(float speed) {
2022-04-09 01:41:42 +01:00
// Invert provided speed if the motor direction is reversed
if(motor_direction == REVERSED_DIR)
2022-04-09 01:41:42 +01:00
speed = 0.0f - speed;
2022-04-04 20:00:03 +01:00
// Clamp the speed between the hard limits
2022-04-20 18:55:39 +01:00
motor_speed = CLAMP(speed, 0.0f - motor_scale, motor_scale);
if(motor_speed > 0.0f)
last_enabled_duty = map_float(motor_speed / motor_scale, 0.0f, 1.0f, motor_zeropoint, 1.0f);
else if(motor_speed < 0.0f)
last_enabled_duty = map_float(motor_speed / motor_scale, 0.0f, 1.0f, motor_zeropoint, 1.0f);
else
last_enabled_duty = 0.0f;
//last_enabled_duty = motor_speed / motor_scale;
2022-04-04 20:00:03 +01:00
return enable_with_return();
}
2022-04-04 20:00:03 +01:00
float MotorState::stop_with_return() {
return set_duty_with_return(0.0f);
}
2022-04-05 16:53:36 +01:00
float MotorState::full_negative_with_return() {
2022-04-04 20:00:03 +01:00
return set_duty_with_return(-1.0f);
}
2022-04-05 16:53:36 +01:00
float MotorState::full_positive_with_return() {
2022-04-04 20:00:03 +01:00
return set_duty_with_return(1.0f);
}
float MotorState::to_percent_with_return(float in, float in_min, float in_max) {
2022-04-20 18:55:39 +01:00
float speed = MotorState::map_float(in, in_min, in_max, 0.0f - motor_scale, motor_scale);
return set_speed_with_return(speed);
}
float MotorState::to_percent_with_return(float in, float in_min, float in_max, float speed_min, float speed_max) {
float speed = MotorState::map_float(in, in_min, in_max, speed_min, speed_max);
return set_speed_with_return(speed);
}
2022-04-09 01:41:42 +01:00
Direction MotorState::get_direction() const {
2022-04-05 16:53:36 +01:00
return motor_direction;
}
void MotorState::set_direction(Direction direction) {
2022-04-09 01:41:42 +01:00
motor_direction = direction;
2022-04-05 16:53:36 +01:00
}
float MotorState::get_speed_scale() const {
return motor_scale;
}
void MotorState::set_speed_scale(float speed_scale) {
motor_scale = MAX(speed_scale, FLT_EPSILON);
motor_speed = last_enabled_duty * motor_scale;
}
float MotorState::get_zeropoint() const {
return motor_scale;
}
void MotorState::set_zeropoint(float zeropoint) {
motor_zeropoint = CLAMP(zeropoint, 0.0f, 1.0f - FLT_EPSILON);
if(last_enabled_duty > motor_zeropoint)
motor_speed = map_float(last_enabled_duty, motor_zeropoint, 1.0f, 0.0f, motor_scale);
else if(last_enabled_duty < -motor_zeropoint)
motor_speed = map_float(last_enabled_duty, -motor_zeropoint, -1.0f, 0.0f, -motor_scale);
else
motor_speed = 0.0f;
}
2022-04-05 16:53:36 +01:00
2022-04-09 01:41:42 +01:00
float MotorState::get_deadzone() const {
return motor_deadzone;
2022-04-05 16:53:36 +01:00
}
2022-04-09 01:41:42 +01:00
float MotorState::set_deadzone_with_return(float deadzone) {
motor_deadzone = CLAMP(deadzone, 0.0f, 1.0f);
2022-04-11 13:35:53 +01:00
return get_deadzoned_duty();
2022-04-05 16:53:36 +01:00
}
int32_t MotorState::duty_to_level(float duty, uint32_t resolution) {
return (int32_t)(duty * (float)resolution);
}
float MotorState::map_float(float in, float in_min, float in_max, float out_min, float out_max) {
return (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min;
}
};