Added additional PWM/ServoCluster constructors

drivers/pwm/pwm_cluster.cpp

@@ -77,14 +77,81 @@ interrupt is fired, and the handler reconfigures channel A so that it is ready f
      * */
     
 
-PWMCluster::PWMCluster(PIO pio, uint sm, uint channel_mask) : pio(pio), sm(sm), channel_mask(channel_mask) {
+PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask) : pio(pio), sm(sm), pin_mask(pin_mask) {
+  channel_polarities = 0x00000000;
+  wrap_level = 0;
+
+  // Initialise all the channels this PWM will control
+  for(uint channel = 0; channel < NUM_BANK0_GPIOS; channel++) {
+    channel_levels[channel] = 0u;
+    channel_offsets[channel] = 0u;
+  }
+}
+
+
+PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count) : pio(pio), sm(sm) {
+  pin_mask = 0x00000000;
+  channel_polarities = 0x00000000;
+  wrap_level = 0;
+
+  // Initialise all the channels this PWM will control
+  uint pin_end = MIN(pin_count, NUM_BANK0_GPIOS);
+  for(uint channel = pin_base; channel < pin_end; channel++) {
+    pin_mask |= (1u << channel);
+  }
+
+  // Initialise all the channels this PWM will control
+  for(uint channel = 0; channel < NUM_BANK0_GPIOS; channel++) {
+    channel_levels[channel] = 0u;
+    channel_offsets[channel] = 0u;
+  }
+}
+
+PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins) : pio(pio), sm(sm) {
+  pin_mask = 0x00000000;
+  channel_polarities = 0x00000000;
+  wrap_level = 0;
+
+  for(auto pin : pins) {
+    if(pin < NUM_BANK0_GPIOS) {
+      pin_mask |= (1u << pin);
+    }
+  }
+
+  // Initialise all the channels this PWM will control
+  for(uint channel = 0; channel < NUM_BANK0_GPIOS; channel++) {
+    channel_levels[channel] = 0u;
+    channel_offsets[channel] = 0u;
+  }
+}
+
+PWMCluster::~PWMCluster() {
+  dma_channel_unclaim(data_dma_channel);
+  dma_channel_unclaim(ctrl_dma_channel);
+  pio_sm_set_enabled(pio, sm, false);
+#ifdef DEBUG_MULTI_PWM
+  pio_remove_program(pio, &debug_pwm_cluster_program, pio_program_offset);
+#else
+  pio_remove_program(pio, &pwm_cluster_program, pio_program_offset);
+#endif
+#ifndef MICROPY_BUILD_TYPE
+  // pio_sm_unclaim seems to hardfault in MicroPython
+  pio_sm_unclaim(pio, sm);
+#endif
+  // Reset all the pins this PWM will control back to an unused state
+  for(uint pin = 0; pin < 32; pin++) { // 32 is number of bits
+    if((1u << pin) != 0) {
+      gpio_set_function(pin, GPIO_FUNC_NULL);
+    }
+  }
+}
+
+bool PWMCluster::init() {
 #ifdef DEBUG_MULTI_PWM
   pio_program_offset = pio_add_program(pio, &debug_pwm_cluster_program);
 #else
   pio_program_offset = pio_add_program(pio, &pwm_cluster_program);
 #endif
-  channel_polarities = 0x00000000;
-  wrap_level = 0;
 
   gpio_init(irq_gpio);
   gpio_set_dir(irq_gpio, GPIO_OUT);
@@ -93,16 +160,14 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint channel_mask) : pio(pio), sm(sm),
 
   // Initialise all the channels this PWM will control
   for(uint channel = 0; channel < NUM_BANK0_GPIOS; channel++) {
-    if(bit_in_mask(channel, channel_mask)) {
+    if(bit_in_mask(channel, pin_mask)) {
       pio_gpio_init(pio, channel);
-      channel_levels[channel] = 0u;
-      channel_offsets[channel] = 0u;
     }
   }
 
   // Set their default state and direction
-  pio_sm_set_pins_with_mask(pio, sm, 0x00, channel_mask);
+  pio_sm_set_pins_with_mask(pio, sm, 0x00, pin_mask);
-  pio_sm_set_pindirs_with_mask(pio, sm, channel_mask, channel_mask);
+  pio_sm_set_pindirs_with_mask(pio, sm, pin_mask, pin_mask);
 
 #ifdef DEBUG_MULTI_PWM
   pio_gpio_init(pio, DEBUG_SIDESET);
@@ -189,31 +254,11 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint channel_mask) : pio(pio), sm(sm),
   pwm_dma_handler();
 
   //dma_start_channel_mask(1u << ctrl_dma_channel);
+  return true;
 }
 
-PWMCluster::~PWMCluster() {
+uint PWMCluster::get_pin_mask() const {
-  dma_channel_unclaim(data_dma_channel);
+  return pin_mask;
-  dma_channel_unclaim(ctrl_dma_channel);
-  pio_sm_set_enabled(pio, sm, false);
-#ifdef DEBUG_MULTI_PWM
-  pio_remove_program(pio, &debug_pwm_cluster_program, pio_program_offset);
-#else
-  pio_remove_program(pio, &pwm_cluster_program, pio_program_offset);
-#endif
-#ifndef MICROPY_BUILD_TYPE
-  // pio_sm_unclaim seems to hardfault in MicroPython
-  pio_sm_unclaim(pio, sm);
-#endif
-  // Reset all the pins this PWM will control back to an unused state
-  for(uint pin = 0; pin < 32; pin++) { // 32 is number of bits
-    if((1u << pin) != 0) {
-      gpio_set_function(pin, GPIO_FUNC_NULL);
-    }
-  }
-}
-
-uint PWMCluster::get_chan_mask() const {
-  return channel_mask;
 }
 
 void PWMCluster::set_wrap(uint32_t wrap, bool load) {
@@ -223,7 +268,7 @@ void PWMCluster::set_wrap(uint32_t wrap, bool load) {
 }
 
 void PWMCluster::set_chan_level(uint8_t channel, uint32_t level, bool load) {
-  if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, channel_mask)) {
+  if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, pin_mask)) {
     channel_levels[channel] = level;
     if(load)
       load_pwm();
@@ -231,7 +276,7 @@ void PWMCluster::set_chan_level(uint8_t channel, uint32_t level, bool load) {
 }
 
 void PWMCluster::set_chan_offset(uint8_t channel, uint32_t offset, bool load) {
-  if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, channel_mask)) {
+  if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, pin_mask)) {
     channel_offsets[channel] = offset;
     if(load)
       load_pwm();
@@ -239,7 +284,7 @@ void PWMCluster::set_chan_offset(uint8_t channel, uint32_t offset, bool load) {
 }
 
 void PWMCluster::set_chan_polarity(uint8_t channel, bool polarity, bool load) {
-  if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, channel_mask)) {
+  if((channel < NUM_BANK0_GPIOS) && bit_in_mask(channel, pin_mask)) {
     if(polarity)
       channel_polarities |= (1u << channel);
     else
@@ -272,7 +317,7 @@ void PWMCluster::load_pwm() {
 
   // Go through each channel that we are assigned to
   for(uint channel = 0; channel < NUM_BANK0_GPIOS; channel++) {
-    if(bit_in_mask(channel, channel_mask)) {
+    if(bit_in_mask(channel, pin_mask)) {
       // Get the channel polarity, remembering that true means inverted
       bool polarity = bit_in_mask(channel, channel_polarities);
 

drivers/pwm/pwm_cluster.hpp

@@ -4,6 +4,7 @@
 #include "hardware/pio.h"
 #include "hardware/dma.h"
 #include "hardware/irq.h"
+#include <initializer_list>
 
 namespace pimoroni {
 
@@ -22,9 +23,14 @@ namespace pimoroni {
 
   class PWMCluster {
   public:
-    PWMCluster(PIO pio, uint sm, uint channel_mask);
+    PWMCluster(PIO pio, uint sm, uint pin_mask);
+    PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count);
+    PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins);
     ~PWMCluster();
-    uint get_chan_mask() const;
+
+    bool init();
+
+    uint get_pin_mask() 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);
@@ -34,14 +40,14 @@ namespace pimoroni {
     //void set_phase_correct(bool phase_correct);
     //void set_enabled(bool enabled);
     void load_pwm();
-  private:
     static bool bit_in_mask(uint bit, uint mask);
+  private:
     static void sorted_insert(TransitionData array[], uint &size, const TransitionData &data);
   private:
     PIO pio;
     uint sm;
     uint pio_program_offset;
-    uint channel_mask;
+    uint pin_mask;
     uint channel_levels[NUM_BANK0_GPIOS];
     uint channel_offsets[NUM_BANK0_GPIOS];
     uint channel_polarities;

drivers/servo/servo_cluster.cpp

@@ -3,50 +3,53 @@
 #include <cstdio>
 
 namespace servo {
-  ServoCluster::ServoCluster(PIO pio, uint sm, uint channel_mask)
+  ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_mask)
-    : pwms(pio, sm, channel_mask) {
+    : pwms(pio, sm, pin_mask) {
+  }
 
-    // Calculate a suitable pwm wrap period for this frequency
+  ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count)
-    uint32_t period; uint16_t div16;
+    : pwms(pio, sm, pin_base, pin_count) {
-    if(pimoroni::PWMCluster::calculate_pwm_factors(pwm_frequency, period, div16)) {
+  }
-      pwm_period = period;
 
-      // Update the pwm before setting the new wrap
+  ServoCluster::ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins)
-      for(uint servo = 0; servo < NUM_BANK0_GPIOS; servo++) {
+    : pwms(pio, sm, pins) {
-        pwms.set_chan_level(servo, 0, 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?
-
-      // Apply the new divider
-      // This is done after loading new PWM values to avoid a lockup condition
-      uint8_t div = div16 >> 4;
-      uint8_t mod = div16 % 16;
-      pwms.set_clkdiv_int_frac(div, mod);
-    }
   }
 
   ServoCluster::~ServoCluster() {
   }
 
   bool ServoCluster::init() {
-    // pwm_cfg = pwm_get_default_config();
+    bool success = false;
-    // pwm_config_set_wrap(&pwm_cfg, 20000 - 1);
 
-    // float div = clock_get_hz(clk_sys) / 1000000;
+    if(pwms.init()) {
-    // pwm_config_set_clkdiv(&pwm_cfg, div);
+      // Calculate a suitable pwm wrap period for this frequency
+      uint32_t period; uint16_t div16;
+      if(pimoroni::PWMCluster::calculate_pwm_factors(pwm_frequency, period, div16)) {
+        pwm_period = period;
 
-    // pwm_init(pwm_gpio_to_slice_num(pin), &pwm_cfg, true);
+        // Update the pwm before setting the new wrap
-    // gpio_set_function(pin, GPIO_FUNC_PWM);
+        for(uint servo = 0; servo < NUM_BANK0_GPIOS; servo++) {
+          pwms.set_chan_level(servo, 0, false);
+        }
 
-    // pwm_set_gpio_level(pin, 0);
+        // 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?
 
-    return true;
+        // Apply the new divider
+        // This is done after loading new PWM values to avoid a lockup condition
+        uint8_t div = div16 >> 4;
+        uint8_t mod = div16 % 16;
+        pwms.set_clkdiv_int_frac(div, mod);
+
+        success = true;
+      }
+    }
+
+    return success;
   }
 
   uint ServoCluster::get_pin_mask() const {
-    return pwms.get_chan_mask();
+    return pwms.get_pin_mask();
   }
 
   void ServoCluster::enable(uint servo, bool load) {

drivers/servo/servo_cluster.hpp

@@ -23,7 +23,9 @@ namespace servo {
     // Constructors/Destructor
     //--------------------------------------------------
   public:
-    ServoCluster(PIO pio, uint sm, uint channel_mask);
+    ServoCluster(PIO pio, uint sm, uint pin_mask);
+    ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count);
+    ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins);
     ~ServoCluster();
 
     //--------------------------------------------------

examples/servo2040/servo2040_functions.cpp

@@ -58,7 +58,9 @@ int main() {
 
   //PWMCluster pwms(pio1, 0, 0b111111111111111);
   //pwms.set_wrap(20000);
-  ServoCluster cluster(pio1, 0, 0b111100);
+  //ServoCluster cluster(pio1, 0, 0b111100);
+  ServoCluster cluster(pio1, 0, {2, 3, 4, 5});
+  cluster.init();
 
   int speed = DEFAULT_SPEED;
   float offset = 0.0f;

micropython/modules/servo/servo.cpp

@@ -819,14 +819,23 @@ typedef struct _ServoCluster_obj_t {
 /***** Print *****/
 void ServoCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
     (void)kind; //Unused input parameter
-    //_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
+    _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
     mp_print_str(print, "ServoCluster(");
 
-    // TODO
+    mp_print_str(print, "pins = {");
-    //mp_print_str(print, "num_leds = ");
+    uint pin_mask = self->cluster->get_pin_mask();
-    //mp_obj_print_helper(print, mp_obj_new_int(self->apa102->num_leds), PRINT_REPR);
+    bool first = true;
+    for(uint pin = 0; pin < NUM_BANK0_GPIOS; pin++) {
+        if(pimoroni::PWMCluster::bit_in_mask(pin, pin_mask)) {
+            if(!first) {
+                mp_print_str(print, ", ");
+            }
+            mp_obj_print_helper(print, mp_obj_new_int(pin), PRINT_REPR);
+            first = false;
+        }
+    }
 
-    mp_print_str(print, ")");
+    mp_print_str(print, "})");
 }
 
 /***** Destructor ******/
@@ -893,6 +902,7 @@ mp_obj_t ServoCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t
     self->base.type = &ServoCluster_type;
 
     self->cluster = new ServoCluster(pio1, 0, 0b11111100); //TODO Expose parameters
+    self->cluster->init();
 
     return MP_OBJ_FROM_PTR(self);
 }