adds PCA9685 (i2c servo driver) library

PCA9685/Makefile

@@ -0,0 +1,9 @@
+all: PCA9685.o PCA9685tester
+
+HDRS = PCA9685.h PCA9685_Addresses.h
+CXXFLAGS = -Wall -lwiringPi
+
+PCA9685.o PCA9685tester.o: $(HDRS)
+
+PCA9685tester: PCA9685.o PCA9685tester.o
+	$(CXX) $^ -lwiringPi -o $@

PCA9685/PCA9685.cpp

@@ -0,0 +1,88 @@
+/*
+* PCA9685.cpp
+*/
+
+#include "PCA9685.h"
+
+#include "stdio.h"
+#include "PCA9685_Addresses.h"
+#include "wiringPi.h"
+#include "wiringPiI2C.h"
+
+PCA9685::PCA9685()
+{
+    PCA9685( PCA9685_DEFAULT_ADDRESS );
+}
+
+PCA9685::PCA9685( int devAddr )
+{
+    i2cFileHandle = wiringPiI2CSetup( devAddr );  // parameter is dependent on board revision
+    if( i2cFileHandle < 0 )
+    {
+        printf( "failed to open i2c\n" );
+    }
+
+    setAllPwm( 0, 0 );
+
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE2, PCA9685_OUTDRV );
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE1, PCA9685_ALLCALL );
+    delay( 5 );
+
+    int mode1 = wiringPiI2CReadReg8( i2cFileHandle, PCA9685_MODE1 );
+    mode1 = mode1 & ~PCA9685_SLEEP;
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE1, mode1 );
+
+   /* wiringPiI2CWriteReg8( i2cFileHandle,
+                          PCA9685_MODE1,
+                          wiringPiI2CReadReg8( i2cFileHandle, PCA9685_MODE1 ) & ~PCA9685_SLEEP );
+    */delay( 5 );
+}
+
+void PCA9685::setFrequency( int hertz )
+{
+    double preScale;
+    int oldMode, newMode;
+
+    preScale = 25000000; // 25Mhz
+    preScale /= 4096;   // 12 bit
+    preScale /= hertz;
+    preScale -= 1;  // fencepost
+    printf( "preScale: %f\n", preScale );
+
+    oldMode = wiringPiI2CReadReg8( i2cFileHandle, PCA9685_MODE1 );
+    newMode = ( oldMode & 0x7F ) | 0x10;
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE1, newMode );  // go to sleep
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_PRE_SCALE, (int) preScale );
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE1, oldMode );  // wake back up
+    delay( 5 );
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE1, oldMode | 0x80 );
+}
+
+void PCA9685::setPwm( int channel, int on, int off )
+{
+    //wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_LED_ADDRS[ 4 * channel ], on & 0xFF );
+    //wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_LED_ADDRS[ 4 * channel +1 ], on >> 8 );
+    //wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_LED_ADDRS[ 4 * channel +2 ], off & 0xFF );
+    //wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_LED_ADDRS[ 4 * channel +3 ], off >> 8 );
+
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_LED0_ON_L + 4*channel, on & 0xFF );
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_LED0_ON_H + 4*channel, on >> 8 );
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_LED0_OFF_L + 4*channel, off & 0xFF );
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_LED0_OFF_H + 4*channel, off >> 8 );
+}
+
+void PCA9685::setAllPwm( int on, int off )
+{
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_ALL_LED_ON_L, on & 0xFF );
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_ALL_LED_OFF_H, on >> 8 );
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_ALL_LED_ON_L, on & 0xFF );
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_ALL_LED_OFF_H, on >> 8);
+
+}
+
+
+
+
+
+
+

PCA9685/PCA9685.h

@@ -0,0 +1,17 @@
+#ifndef PCA9685_H
+#define PCA9685_H
+
+class PCA9685
+{
+public:
+    PCA9685();  // no-args constructor: uses default device address @ 50hz
+    PCA9685( int devAddr ); // frequency: 50hz
+
+    void setFrequency( int hertz );
+    void setPwm( int channel, int on, int off );   // for standard servo behavior: 150 <= value <= 600
+    void setAllPwm( int on, int off );
+private:
+    int i2cFileHandle;
+};
+
+#endif

PCA9685/PCA9685_Addresses.h

@@ -0,0 +1,147 @@
+#ifndef PCA9685_ADDRESSES_H
+#define PCA9685_ADDRESSES_H
+
+#define PCA9685_DEFAULT_ADDRESS 0x40
+
+
+#define PCA9685_RESTART 0x80
+#define PCA9685_SLEEP   0x10
+#define PCA9685_ALLCALL 0x01
+#define PCA9685_INVRT   0x10
+#define PCA9685_OUTDRV  0x04
+
+
+#define PCA9685_MODE1 0x00
+#define PCA9685_MODE2 0x01
+#define PCA9685_SUBADR1 0x02
+#define PCA9685_SUBADR2 0x03
+#define PCA9685_SUBADR3 0x04
+#define PCA9685_ALLCALLADR 0x05
+
+const int PCA9685_LED_ADDRS[] = {
+    0x06, 0x07, 0x08, 0x09, 
+    0x0A, 0x0B, 0x0C, 0x0D, 
+    0x0E, 0x0F, 0x10, 0x11, 
+    0x12, 0x13, 0x14, 0x15, 
+    0x16, 0x17, 0x18, 0x19, 
+    0x1A, 0x1B, 0x1C, 0x1D, 
+    0x1E, 0x1F, 0x20, 0x21, 
+    0x22, 0x23, 0x24, 0x25, 
+    0x26, 0x27, 0x28, 0x29, 
+    0x2A, 0x2B, 0x2C, 0x2D, 
+    0x2E, 0x2F, 0x30, 0x31, 
+    0x32, 0x33, 0x34, 0x35, 
+    0x36, 0x37, 0x38, 0x39, 
+    0x3A, 0x3B, 0x3C, 0x3D, 
+    0x3E, 0x3F, 0x40, 0x41, 
+    0x42, 0x43, 0x44, 0x45 }
+/*
+#define PCA9685_LED_ADDRS { \
+    0x06, 0x07, 0x08, 0x09, \
+    0x0A, 0x0B, 0x0C, 0x0D, \
+    0x0E, 0x0F, 0x10, 0x11, \
+    0x12, 0x13, 0x14, 0x15, \
+    0x16, 0x17, 0x18, 0x19, \
+    0x1A, 0x1B, 0x1C, 0x1D, \
+    0x1E, 0x1F, 0x20, 0x21, \
+    0x22, 0x23, 0x24, 0x25, \
+    0x26, 0x27, 0x28, 0x29, \
+    0x2A, 0x2B, 0x2C, 0x2D, \
+    0x2E, 0x2F, 0x30, 0x31, \
+    0x32, 0x33, 0x34, 0x35, \
+    0x36, 0x37, 0x38, 0x39, \
+    0x3A, 0x3B, 0x3C, 0x3D, \
+    0x3E, 0x3F, 0x40, 0x41, \
+    0x42, 0x43, 0x44, 0x45  \
+}
+*/
+
+#define PCA9685_LED0_ON_L   0x06
+#define PCA9685_LED0_ON_H   0x07
+#define PCA9685_LED0_OFF_L  0x08
+#define PCA9685_LED0_OFF_H  0x09
+
+#define PCA9685_LED1_ON_L   0x0A
+#define PCA9685_LED1_ON_H   0x0B
+#define PCA9685_LED1_OFF_L  0x0C
+#define PCA9685_LED1_OFF_H  0x0D
+
+#define PCA9685_LED2_ON_L   0x0E
+#define PCA9685_LED2_ON_H   0x0F
+#define PCA9685_LED2_OFF_L  0x10
+#define PCA9685_LED2_OFF_H  0x11
+
+#define PCA9685_LED3_ON_L   0x12
+#define PCA9685_LED3_ON_H   0x13
+#define PCA9685_LED3_OFF_L  0x14
+#define PCA9685_LED3_OFF_H  0x15
+
+#define PCA9685_LED4_ON_L   0x16
+#define PCA9685_LED4_ON_H   0x17
+#define PCA9685_LED4_OFF_L  0x18
+#define PCA9685_LED4_OFF_H  0x19
+
+#define PCA9685_LED5_ON_L   0x1A
+#define PCA9685_LED5_ON_H   0x1B
+#define PCA9685_LED5_OFF_L  0x1C
+#define PCA9685_LED5_OFF_H  0x1D
+
+#define PCA9685_LED6_ON_L   0x1E
+#define PCA9685_LED6_ON_H   0x1F
+#define PCA9685_LED6_OFF_L  0x20
+#define PCA9685_LED6_OFF_H  0x21
+
+#define PCA9685_LED7_ON_L   0x22
+#define PCA9685_LED7_ON_H   0x23
+#define PCA9685_LED7_OFF_L  0x24
+#define PCA9685_LED7_OFF_H  0x25
+
+#define PCA9685_LED8_ON_L   0x26
+#define PCA9685_LED8_ON_H   0x27
+#define PCA9685_LED8_OFF_L  0x28
+#define PCA9685_LED8_OFF_H  0x29
+
+#define PCA9685_LED9_ON_L   0x2A
+#define PCA9685_LED9_ON_H   0x2B
+#define PCA9685_LED9_OFF_L  0x2C
+#define PCA9685_LED9_OFF_H  0x2D
+
+#define PCA9685_LED10_ON_L  0x2E
+#define PCA9685_LED10_ON_H  0x2F
+#define PCA9685_LED10_OFF_L 0x30
+#define PCA9685_LED10_OFF_H 0x31
+
+#define PCA9685_LED11_ON_L  0x32
+#define PCA9685_LED11_ON_H  0x33
+#define PCA9685_LED11_OFF_L 0x34
+#define PCA9685_LED11_OFF_H 0x35
+
+#define PCA9685_LED12_ON_L  0x36
+#define PCA9685_LED12_ON_H  0x37
+#define PCA9685_LED12_OFF_L 0x38
+#define PCA9685_LED12_OFF_H 0x39
+
+#define PCA9685_LED13_ON_L  0x3A
+#define PCA9685_LED13_ON_H  0x3B
+#define PCA9685_LED13_OFF_L 0x3C
+#define PCA9685_LED13_OFF_H 0x3D
+
+#define PCA9685_LED14_ON_L  0x3E
+#define PCA9685_LED14_ON_H  0x3F
+#define PCA9685_LED14_OFF_L 0x40
+#define PCA9685_LED14_OFF_H 0x41
+
+#define PCA9685_LED15_ON_L  0x42
+#define PCA9685_LED15_ON_H  0x43
+#define PCA9685_LED15_OFF_L 0x44
+#define PCA9685_LED15_OFF_H 0x45
+
+#define PCA9685_ALL_LED_ON_L    0xFA
+#define PCA9685_ALL_LED_ON_H    0xFB
+#define PCA9685_ALL_LED_OFF_L   0xFC
+#define PCA9685_ALL_LED_OFF_H   0xFD
+
+#define PCA9685_PRE_SCALE   0xFE
+#define PCA9685_TESTMODE    0xFF
+
+#endif

PCA9685/PCA9685tester.cpp

@@ -0,0 +1,22 @@
+
+#include "stdio.h"
+#include "wiringPi.h"
+#include "PCA9685.h"
+
+int main( int argc, const char* argv[] )
+{
+	wiringPiSetup();
+	PCA9685 servo = PCA9685( 0x40 );
+	servo.setFrequency( 60 );
+	printf( "freq set\n" );
+
+	while( true )
+	{
+		servo.setPwm( 0, 0, 600 );
+		printf( "pwm: 600\n" );
+		delay( 1000 );
+		servo.setPwm( 0, 0, 150 );
+		printf("pwm: 150\n" );
+		delay( 1000 );
+	}
+}