Merge pull request #62 from photodude/patch-1

Add a `writeMicroseconds()` function

Author: ladyada

Adafruit_PWMServoDriver.cpp

@@ -278,6 +278,56 @@ void Adafruit_PWMServoDriver::setPin(uint8_t num, uint16_t val, bool invert) {
   }
 }
 
+/*!
+ *  @brief  Sets the PWM output of one of the PCA9685 pins based on the input microseconds, output is not precise
+ *  @param  num One of the PWM output pins, from 0 to 15
+ *  @param  Microseconds The number of Microseconds to turn the PWM output ON
+ */
+void Adafruit_PWMServoDriver::writeMicroseconds(uint8_t num, uint16_t Microseconds) {
+  #ifdef ENABLE_DEBUG_OUTPUT
+  Serial.print("Setting PWM Via Microseconds on output");
+  Serial.print(num);
+  Serial.print(": ");
+  Serial.print(Microseconds);
+  Serial.println("->");
+  #endif
+
+  double pulse = Microseconds;
+  double pulselength;
+  pulselength = 1000000;   // 1,000,000 us per second
+
+  // Read prescale and convert to frequency
+  double prescale = Adafruit_PWMServoDriver::readPrescale();
+  prescale += 1;
+  uint32_t freq = 25000000; // Chip frequency is 25MHz
+  freq /= prescale;
+  freq /= 4096; // 12 bits of resolution
+ 
+  #ifdef ENABLE_DEBUG_OUTPUT
+  Serial.print(freq); Serial.println(" Calculated PCA9685 chip PWM Frequency");
+  #endif
+  
+  pulselength /= freq;   //  us per period from PCA9685 chip PWM Frequency using prescale reverse frequency calc
+
+  #ifdef ENABLE_DEBUG_OUTPUT
+  Serial.print(pulselength); Serial.println(" us per period");
+  #endif
+
+  pulselength /= 4096;  // 12 bits of resolution
+
+  #ifdef ENABLE_DEBUG_OUTPUT
+  Serial.print(pulselength); Serial.println(" us per bit"); 
+  #endif
+
+  pulse /= pulselength;
+
+  #ifdef ENABLE_DEBUG_OUTPUT
+  Serial.print(pulse);Serial.println(" pulse for PWM"); 
+  #endif
+
+  Adafruit_PWMServoDriver::setPWM(num, 0, pulse);
+}
+
 uint8_t Adafruit_PWMServoDriver::read8(uint8_t addr) {
   _i2c->beginTransmission(_i2caddr);
   _i2c->write(addr);

Adafruit_PWMServoDriver.h

@@ -88,6 +88,7 @@ class Adafruit_PWMServoDriver {
   void setPWM(uint8_t num, uint16_t on, uint16_t off);
   void setPin(uint8_t num, uint16_t val, bool invert=false);
   uint8_t readPrescale(void);
+  void writeMicroseconds(uint8_t num, uint16_t Microseconds);
 
  private:
   uint8_t _i2caddr;

examples/servo/servo.ino

@@ -31,8 +31,11 @@ Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
 // want these to be as small/large as possible without hitting the hard stop
 // for max range. You'll have to tweak them as necessary to match the servos you
 // have!
-#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
-#define SERVOMAX  600 // this is the 'maximum' pulse length count (out of 4096)
+#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
+#define SERVOMAX  600 // This is the 'maximum' pulse length count (out of 4096)
+#define USMIN  611 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
+#define USMAX  2441 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
+#define FREQ 60 // Analog servos run at ~60 Hz updates
 
 // our servo # counter
 uint8_t servonum = 0;
@@ -43,29 +46,29 @@ void setup() {
 
   pwm.begin();
 
-  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
+  pwm.setPWMFreq(FREQ);  // Analog servos run at ~60 Hz updates
 
   delay(10);
 }
 
-// you can use this function if you'd like to set the pulse length in seconds
-// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. its not precise!
+// You can use this function if you'd like to set the pulse length in seconds
+// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. It's not precise!
 void setServoPulse(uint8_t n, double pulse) {
   double pulselength;
 
   pulselength = 1000000;   // 1,000,000 us per second
-  pulselength /= 60;   // 60 Hz
+  pulselength /= FREQ;   // Analog servos run at ~60 Hz updates
   Serial.print(pulselength); Serial.println(" us per period"); 
   pulselength /= 4096;  // 12 bits of resolution
   Serial.print(pulselength); Serial.println(" us per bit"); 
-  pulse *= 1000000;  // convert to us
+  pulse *= 1000000;  // convert input seconds to us
   pulse /= pulselength;
   Serial.println(pulse);
   pwm.setPWM(n, 0, pulse);
 }
 
 void loop() {
-  // Drive each servo one at a time
+  // Drive each servo one at a time using setPWM()
   Serial.println(servonum);
   for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) {
     pwm.setPWM(servonum, 0, pulselen);
@@ -78,6 +81,19 @@ void loop() {
 
   delay(500);
 
+  // Drive each servo one at a time using writeMicroseconds(), it's not precise due to calculation rounding!
+  // The writeMicroseconds() function is used to mimic the Arduino Servo library writeMicroseconds() behavior. 
+  for (uint16_t microsec = USMIN; microsec < USMAX; microsec++) {
+    pwm.writeMicroseconds(servonum, microsec);
+  }
+
+  delay(500);
+  for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) {
+    pwm.writeMicroseconds(servonum, microsec);
+  }
+
+  delay(500);
+
   servonum ++;
-  if (servonum > 7) servonum = 0;
+  if (servonum > 7) servonum = 0; // Testing the first 8 servo channels
 }