kopia lustrzana https://github.com/pimoroni/pimoroni-pico
Added support for servo phase adjustment
ZodiusInfuser
rodzic
7f90d3c529
commit
72516b2ecc
|
|
@ -86,6 +86,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask)
|
|||
: pio(pio)
|
||||
, sm(sm)
|
||||
, pin_mask(pin_mask & ((1u << NUM_BANK0_GPIOS) - 1))
|
||||
, channel_count(0)
|
||||
, channel_polarities(0x00000000)
|
||||
, wrap_level(0) {
|
||||
|
||||
|
|
@ -95,6 +96,10 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask)
|
|||
channel_offsets[channel] = 0u;
|
||||
channel_overruns[channel] = 0u;
|
||||
next_channel_overruns[channel] = 0u;
|
||||
|
||||
if(bit_in_mask(channel, pin_mask)) {
|
||||
channel_count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -103,6 +108,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count)
|
|||
: pio(pio)
|
||||
, sm(sm)
|
||||
, pin_mask(0x00000000)
|
||||
, channel_count(0)
|
||||
, channel_polarities(0x00000000)
|
||||
, wrap_level(0) {
|
||||
|
||||
|
|
@ -110,6 +116,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count)
|
|||
uint pin_end = MIN(pin_count + pin_base, NUM_BANK0_GPIOS);
|
||||
for(uint channel = pin_base; channel < pin_end; channel++) {
|
||||
pin_mask |= (1u << channel);
|
||||
channel_count++;
|
||||
}
|
||||
|
||||
// Initialise all the channels this PWM will control
|
||||
|
|
@ -125,6 +132,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins)
|
|||
: pio(pio)
|
||||
, sm(sm)
|
||||
, pin_mask(0x00000000)
|
||||
, channel_count(0)
|
||||
, channel_polarities(0x00000000)
|
||||
, wrap_level(0) {
|
||||
|
||||
|
|
@ -132,6 +140,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins)
|
|||
for(auto pin : pins) {
|
||||
if(pin < NUM_BANK0_GPIOS) {
|
||||
pin_mask |= (1u << pin);
|
||||
channel_count++;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -281,6 +290,35 @@ uint PWMCluster::get_pin_mask() const {
|
|||
return pin_mask;
|
||||
}
|
||||
|
||||
uint8_t PWMCluster::get_chan_count() const {
|
||||
return channel_count;
|
||||
}
|
||||
|
||||
uint32_t PWMCluster::get_wrap() const {
|
||||
return wrap_level;
|
||||
}
|
||||
|
||||
uint32_t PWMCluster::get_chan_level(uint8_t channel) const {
|
||||
if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, pin_mask))
|
||||
return channel_levels[channel];
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t PWMCluster::get_chan_offset(uint8_t channel) const {
|
||||
if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, pin_mask))
|
||||
return channel_offsets[channel];
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool PWMCluster::get_chan_polarity(uint8_t channel) const {
|
||||
if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, pin_mask))
|
||||
return bit_in_mask(channel, channel_polarities);
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
void PWMCluster::set_wrap(uint32_t wrap, bool load) {
|
||||
wrap_level = MAX(wrap, 1); // Cannot have a wrap of zero!
|
||||
if(load)
|
||||
|
|
|
|||
|
|
@ -33,6 +33,13 @@ namespace pimoroni {
|
|||
bool init();
|
||||
|
||||
uint get_pin_mask() const;
|
||||
uint8_t get_chan_count() const;
|
||||
|
||||
uint32_t get_wrap() const;
|
||||
uint32_t get_chan_level(uint8_t channel) const;
|
||||
uint32_t get_chan_offset(uint8_t channel) const;
|
||||
bool get_chan_polarity(uint8_t channel) const;
|
||||
|
||||
void set_wrap(uint32_t wrap, bool load = true);
|
||||
void set_chan_level(uint8_t channel, uint32_t level, bool load = true);
|
||||
void set_chan_offset(uint8_t channel, uint32_t offset, bool load = true);
|
||||
|
|
@ -50,6 +57,7 @@ namespace pimoroni {
|
|||
uint sm;
|
||||
uint pio_program_offset;
|
||||
uint pin_mask;
|
||||
uint8_t channel_count;
|
||||
uint channel_levels[NUM_BANK0_GPIOS];
|
||||
uint channel_offsets[NUM_BANK0_GPIOS];
|
||||
uint channel_polarities;
|
||||
|
|
|
|||
|
|
@ -3,35 +3,44 @@
|
|||
#include <cstdio>
|
||||
|
||||
namespace servo {
|
||||
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type, float freq)
|
||||
: pwms(pio, sm, pin_mask), pwm_frequency(freq) {
|
||||
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type, float freq, bool auto_phase)
|
||||
: pwms(pio, sm, pin_mask), pwm_frequency(freq) {
|
||||
|
||||
for(uint i = 0; i < NUM_BANK0_GPIOS; i++) {
|
||||
if(pimoroni::PWMCluster::bit_in_mask(i, pin_mask)) {
|
||||
servos[i] = new ServoState(default_type);
|
||||
}
|
||||
servos[i] = (pimoroni::PWMCluster::bit_in_mask(i, pin_mask)) ?
|
||||
new ServoState(default_type) : nullptr;
|
||||
}
|
||||
|
||||
if(auto_phase) {
|
||||
apply_uniform_phases();
|
||||
}
|
||||
}
|
||||
|
||||
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type, float freq)
|
||||
: pwms(pio, sm, pin_base, pin_count), pwm_frequency(freq) {
|
||||
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type, float freq, bool auto_phase)
|
||||
: pwms(pio, sm, pin_base, pin_count), pwm_frequency(freq) {
|
||||
uint pin_mask = pwms.get_pin_mask();
|
||||
|
||||
for(uint i = 0; i < NUM_BANK0_GPIOS; i++) {
|
||||
if(pimoroni::PWMCluster::bit_in_mask(i, pin_mask)) {
|
||||
servos[i] = new ServoState(default_type);
|
||||
}
|
||||
servos[i] = (pimoroni::PWMCluster::bit_in_mask(i, pin_mask)) ?
|
||||
new ServoState(default_type) : nullptr;
|
||||
}
|
||||
|
||||
if(auto_phase) {
|
||||
apply_uniform_phases();
|
||||
}
|
||||
}
|
||||
|
||||
ServoCluster::ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type, float freq)
|
||||
: pwms(pio, sm, pins), pwm_frequency(freq) {
|
||||
ServoCluster::ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type, float freq, bool auto_phase)
|
||||
: pwms(pio, sm, pins), pwm_frequency(freq) {
|
||||
uint pin_mask = pwms.get_pin_mask();
|
||||
|
||||
for(uint i = 0; i < NUM_BANK0_GPIOS; i++) {
|
||||
if(pimoroni::PWMCluster::bit_in_mask(i, pin_mask)) {
|
||||
servos[i] = new ServoState(default_type);
|
||||
}
|
||||
servos[i] = (pimoroni::PWMCluster::bit_in_mask(i, pin_mask)) ?
|
||||
new ServoState(default_type) : nullptr;
|
||||
}
|
||||
|
||||
if(auto_phase) {
|
||||
apply_uniform_phases();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -55,10 +64,11 @@ namespace servo {
|
|||
// Update the pwm before setting the new wrap
|
||||
for(uint servo = 0; servo < NUM_BANK0_GPIOS; servo++) {
|
||||
pwms.set_chan_level(servo, 0, false);
|
||||
pwms.set_chan_offset(servo, (uint32_t)(servo_phases[servo] * (float)pwm_period), false);
|
||||
}
|
||||
|
||||
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
|
||||
pwms.set_wrap(pwm_period); // NOTE Minus 1 not needed here. Maybe should change Wrap behaviour so it is needed, for consistency with hardware pwm?
|
||||
pwms.set_wrap(pwm_period, true); // NOTE Minus 1 not needed here. Maybe should change Wrap behaviour so it is needed, for consistency with hardware pwm?
|
||||
|
||||
// Apply the new divider
|
||||
// This is done after loading new PWM values to avoid a lockup condition
|
||||
|
|
@ -120,6 +130,17 @@ namespace servo {
|
|||
apply_pulse(servo, new_pulse, load);
|
||||
}
|
||||
|
||||
float ServoCluster::get_pulse_phase(uint servo) const {
|
||||
assert(is_assigned(servo));
|
||||
return servo_phases[servo];
|
||||
}
|
||||
|
||||
void ServoCluster::set_pulse_phase(uint servo, float phase, bool load) {
|
||||
assert(is_assigned(servo));
|
||||
servo_phases[servo] = MIN(MAX(phase, 0.0f), 1.0f);
|
||||
pwms.set_chan_offset(servo, (uint32_t)(servo_phases[servo] * (float)pwms.get_wrap()), load);
|
||||
}
|
||||
|
||||
float ServoCluster::get_frequency() const {
|
||||
return pwm_frequency;
|
||||
}
|
||||
|
|
@ -140,6 +161,7 @@ namespace servo {
|
|||
if(servos[servo] != nullptr) {
|
||||
float current_pulse = servos[servo]->get_pulse();
|
||||
apply_pulse(servo, current_pulse, false);
|
||||
pwms.set_chan_offset(servo, (uint32_t)(servo_phases[servo] * (float)pwm_period), false);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -216,4 +238,15 @@ namespace servo {
|
|||
void ServoCluster::apply_pulse(uint servo, float pulse, bool load) {
|
||||
pwms.set_chan_level(servo, ServoState::pulse_to_level(pulse, pwm_period, pwm_frequency), load);
|
||||
}
|
||||
|
||||
void ServoCluster::apply_uniform_phases() {
|
||||
uint i = 0;
|
||||
uint8_t servo_count = pwms.get_chan_count();
|
||||
for(uint servo = 0; servo < NUM_BANK0_GPIOS; servo++) {
|
||||
if(is_assigned(servo)) {
|
||||
servo_phases[servo] = (float)i / (float)servo_count;
|
||||
i++;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
|
@ -15,15 +15,16 @@ namespace servo {
|
|||
uint32_t pwm_period;
|
||||
float pwm_frequency;
|
||||
ServoState* servos[NUM_BANK0_GPIOS];
|
||||
float servo_phases[NUM_BANK0_GPIOS];
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
ServoCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY);
|
||||
ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY);
|
||||
ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY);
|
||||
ServoCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
|
||||
ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
|
||||
ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
|
||||
~ServoCluster();
|
||||
|
||||
//--------------------------------------------------
|
||||
|
|
@ -47,6 +48,9 @@ namespace servo {
|
|||
float get_pulse(uint servo) const;
|
||||
void set_pulse(uint servo, float pulse, bool load = true);
|
||||
|
||||
float get_pulse_phase(uint servo) const;
|
||||
void set_pulse_phase(uint servo, float phase, bool load = true);
|
||||
|
||||
float get_frequency() const;
|
||||
bool set_frequency(float freq);
|
||||
|
||||
|
|
@ -67,6 +71,7 @@ namespace servo {
|
|||
//--------------------------------------------------
|
||||
private:
|
||||
void apply_pulse(uint servo, float pulse, bool load);
|
||||
void apply_uniform_phases();
|
||||
};
|
||||
|
||||
}
|
||||
Ładowanie…
Reference in New Issue