Merge pull request #81 from matrix-io/bes/sGPIO

Easy to use PWM func

cpp/driver/gpio_control.cpp

@@ -163,4 +163,144 @@ void GPIOControl::Setup(MatrixIOBus *bus) {
   }
 }
 
+bool GPIOControl::Set9GServoAngle(float angle, uint16_t pin) {
+  if (!bus_) return false;
+  if (pin > 15) return false;
+
+  // Servo constants based on SG90 9g Micro Servo
+  const float ServoRatio = 37.7;
+  const int ServoOffset = 1800;
+
+  // We choose a prescaler of 32 to work with a lower frequency
+  // FPGA clock is being divided by 32
+  // 32 = (1 << 5)
+  const uint16_t GPIOPrescaler = 0x5;
+
+  // We need 50Hz for servo, so 1 / 50Hz = 0.02 sec
+  // from : https://en.wikipedia.org/wiki/Servo_(radio_control)
+  const float period_seconds = 0.02;
+
+  /* Getting period_counter to generate 50Hz:
+
+  FPGAClock = 150000000
+  FPGAClockAfterPrescaler = 150000000 / 32 = 4687500
+
+  Period counter required for 50Hz
+  period_counter = 0.02 / ( 1 / 4687500 ) = 93750
+
+  FPGA firmware need only half of the period counter
+  half_period_counter = period_counter / 2 = 46875
+
+  When all math is combined you get
+  final_period_counter =
+      (period_seconds * FPGAClock / ((1 << GPIOPrescaler) * 2);
+
+  */
+  uint32_t period_counter =
+      (period_seconds * bus_->FPGAClock()) / ((1 << GPIOPrescaler) * 2);
+
+  // Using servo parameters to get duty
+  uint16_t duty_counter = (ServoRatio * angle) + ServoOffset;
+
+  uint16_t bank = pin / 4;
+  uint16_t channel = pin % 4;
+
+  bool b1 = SetPrescaler(bank, GPIOPrescaler);
+  bool b2 = Bank(bank).SetPeriod(period_counter);
+  bool b3 = Bank(bank).SetDuty(channel, duty_counter);
+
+  return (b1 && b2 && b3);
+}
+
+bool GPIOControl::SetServoAngle(float angle, float min_pulse_ms, uint16_t pin) {
+  if (!bus_) return false;
+  if (pin > 15) return false;
+
+  // We choose a prescaler of 32 to work with a lower frequency
+  // FPGA clock is being divided by 32
+  // 32 = (1 << 5)
+  const uint16_t GPIOPrescaler = 0x5;
+
+  // We need 50Hz for servo, so 1 / 50Hz = 0.02 sec
+  // from : https://en.wikipedia.org/wiki/Servo_(radio_control)
+  const float period_seconds = 0.02;
+
+  /* Getting period_counter to generate 50Hz:
+
+  FPGAClock = 150000000
+  FPGAClockAfterPrescaler = 150000000 / 32 = 4687500
+
+  Period counter required for 50Hz
+  period_counter = 0.02 / ( 1 / 4687500 ) = 93750
+
+  FPGA firmware need only half of the period counter
+  half_period_counter = period_counter / 2 = 46875
+
+  When all math is combined you get
+  final_period_counter =
+      (period_seconds * FPGAClock / ((1 << GPIOPrescaler) * 2);
+
+  */
+  uint32_t period_counter =
+      (period_seconds * bus_->FPGAClock()) / ((1 << GPIOPrescaler) * 2);
+
+  // Servo pulse width is symmetrical, with 1.5ms as neutral position
+  // 1.5ms / 20ms = 0.075
+  uint32_t ServoMiddle = period_counter * 0.075;
+  uint32_t ServoOffset = period_counter * (min_pulse_ms / 20);
+  float ServoRatio = (ServoMiddle - ServoOffset) / 90;
+
+  // Using servo parameters to get duty
+  uint16_t duty_counter = (ServoRatio * angle) + ServoOffset;
+
+  uint16_t bank = pin / 4;
+  uint16_t channel = pin % 4;
+
+  bool b1 = SetPrescaler(bank, GPIOPrescaler);
+  bool b2 = Bank(bank).SetPeriod(period_counter);
+  bool b3 = Bank(bank).SetDuty(channel, duty_counter);
+
+  return (b1 && b2 && b3);
+}
+
+bool GPIOControl::SetPWM(float frequency, float percentage, uint16_t pin) {
+  if (!bus_) return false;
+  if (pin > 15) return false;
+
+  // We choose a prescaler of 32 to work with a lower frequency
+  // FPGA clock is being divided by 32
+  // 32 = (1 << 5)
+  const uint16_t GPIOPrescaler = 0x5;
+  float period_seconds = 1 / frequency;
+
+  /* Getting period_counter to generate desired frequency:
+
+  FPGAClock = 150000000
+  FPGAClockAfterPrescaler = 150000000 / 32 = 4687500
+
+  Period counter required for desired frequency
+  period_counter = period_seconds / ( 1 / 4687500 )
+
+  FPGA firmware need only half of the period counter
+  half_period_counter = period_counter / 2
+
+  When all math is combined you get
+  final_period_counter =
+      (period_seconds * FPGAClock / ((1 << GPIOPrescaler) * 2);
+
+  */
+  uint32_t period_counter =
+      (period_seconds * bus_->FPGAClock()) / ((1 << GPIOPrescaler) * 2);
+  uint16_t duty_counter = (period_counter * percentage) / 100;
+
+  uint16_t bank = pin / 4;
+  uint16_t channel = pin % 4;
+
+  bool b1 = SetPrescaler(bank, GPIOPrescaler);
+  bool b2 = Bank(bank).SetPeriod(period_counter);
+  bool b3 = Bank(bank).SetDuty(channel, duty_counter);
+
+  return (b1 && b2 && b3);
+}
+
 };  // namespace matrix_hal

cpp/driver/gpio_control.h

@@ -53,6 +53,10 @@ class GPIOControl : public MatrixDriver {
   bool SetPrescaler(uint16_t bank, uint16_t prescaler);
   GPIOBank &Bank(uint16_t bank) { return banks_[bank]; }
 
+  bool Set9GServoAngle(float angle, uint16_t pin);
+  bool SetServoAngle(float angle, float min_pulse_ms, uint16_t pin);
+  bool SetPWM(float frequency, float percentage, uint16_t pin);
+
   std::vector<GPIOBank> banks_;
   uint16_t mode_;
   uint16_t value_;

debian/changelog

@@ -1,3 +1,9 @@
+libmatrixio-creator-hal (0.3.6) unstable; urgency=medium
+
+  * Add easy to use PWM functions in GPIOControl
+
+ -- Admobilize <info@admobilize.com>  Fri, 29 Jun 2018 15:51:50 +0000
+
 libmatrixio-creator-hal (0.3.5) unstable; urgency=medium
 
   * Fix getTimerCounter return bug