Changes PCA9685 code

CopterController/PCA9685/PCA9685.cpp

@@ -3,9 +3,9 @@
 */
 
 #include "PCA9685.h"
-
-#include "stdio.h"
 #include "PCA9685_Addresses.h"
+;
+#include <iostream>
 #include "wiringPi.h"
 #include "wiringPiI2C.h"
 
@@ -19,23 +19,20 @@ PCA9685::PCA9685( int devAddr )
     i2cFileHandle = wiringPiI2CSetup( devAddr );  // parameter is dependent on board revision
     if( i2cFileHandle < 0 )
     {
-        printf( "failed to open i2c\n" );
+        std::cerr << "failed to open i2c" << std::endl;
     }
 
     setAllPwm( 0, 0 );
+    std::cout << "all pwm set to o" << std::endl;
 
     wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE2, PCA9685_OUTDRV );
     wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE1, PCA9685_ALLCALL );
     delay( 5 );
 
     int mode1 = wiringPiI2CReadReg8( i2cFileHandle, PCA9685_MODE1 );
-    mode1 = mode1 & ~PCA9685_SLEEP;
+    mode1 = mode1 & ~PCA9685_SLEEP; // set sleep bit (to wake)
     wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE1, mode1 );
-
+    delay( 5 );
-   /* wiringPiI2CWriteReg8( i2cFileHandle,
-                          PCA9685_MODE1,
-                          wiringPiI2CReadReg8( i2cFileHandle, PCA9685_MODE1 ) & ~PCA9685_SLEEP );
-    */delay( 5 );
 }
 
 void PCA9685::setFrequency( int hertz )
@@ -47,24 +44,24 @@ void PCA9685::setFrequency( int hertz )
     preScale /= 4096;   // 12 bit
     preScale /= hertz;
     preScale -= 1;  // fencepost
-    printf( "preScale: %f\n", preScale );
+
+    std::cout << "setting pwm frequency to: " << hertz << " hertz" << std::endl;
+    std::cout << "estimate preScale: " << preScale << std::endl;
+    preScale += 0.5;
+    std::cout << "final preScale: " << (int) preScale << std::endl;
 
     oldMode = wiringPiI2CReadReg8( i2cFileHandle, PCA9685_MODE1 );
-    newMode = ( oldMode & 0x7F ) | 0x10;
+    newMode = ( oldMode & 0x7F ) | PCA9685_SLEEP;   // set sleep bit (to sleep)
     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 );
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE1, oldMode | PCA9685_RESTART );   // set restart bit
+    delay( 5 ); // wait for restart
 }
 
 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 );
@@ -74,8 +71,8 @@ void PCA9685::setPwm( int channel, int on, int off )
 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_H, on >> 8 );
-    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_ALL_LED_ON_L, on & 0xFF );
+    wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_ALL_LED_OFF_L, on & 0xFF );
     wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_ALL_LED_OFF_H, on >> 8);
 
 }

CopterController/PCA9685/PCA9685_Addresses.h

@@ -18,43 +18,32 @@
 #define PCA9685_SUBADR3 0x04
 #define PCA9685_ALLCALLADR 0x05
 
+#define PCA9685_PRE_SCALE   0xFE
+#define PCA9685_TESTMODE    0xFF
+
 const int PCA9685_LED_ADDRS[] = {
-    0x06, 0x07, 0x08, 0x09, 
+ // ON_L  ON_H  OFF_L OFF_H // channel
-    0x0A, 0x0B, 0x0C, 0x0D, 
+    0x06, 0x07, 0x08, 0x09, // 0
-    0x0E, 0x0F, 0x10, 0x11, 
+    0x0A, 0x0B, 0x0C, 0x0D, // 1
-    0x12, 0x13, 0x14, 0x15, 
+    0x0E, 0x0F, 0x10, 0x11, // 2
-    0x16, 0x17, 0x18, 0x19, 
+    0x12, 0x13, 0x14, 0x15, // 3
-    0x1A, 0x1B, 0x1C, 0x1D, 
+    0x16, 0x17, 0x18, 0x19, // 4
-    0x1E, 0x1F, 0x20, 0x21, 
+    0x1A, 0x1B, 0x1C, 0x1D, // 5
-    0x22, 0x23, 0x24, 0x25, 
+    0x1E, 0x1F, 0x20, 0x21, // 6
-    0x26, 0x27, 0x28, 0x29, 
+    0x22, 0x23, 0x24, 0x25, // 7
-    0x2A, 0x2B, 0x2C, 0x2D, 
+    0x26, 0x27, 0x28, 0x29, // 8
-    0x2E, 0x2F, 0x30, 0x31, 
+    0x2A, 0x2B, 0x2C, 0x2D, // 9
-    0x32, 0x33, 0x34, 0x35, 
+    0x2E, 0x2F, 0x30, 0x31, // 10
-    0x36, 0x37, 0x38, 0x39, 
+    0x32, 0x33, 0x34, 0x35, // 11
-    0x3A, 0x3B, 0x3C, 0x3D, 
+    0x36, 0x37, 0x38, 0x39, // 12
-    0x3E, 0x3F, 0x40, 0x41, 
+    0x3A, 0x3B, 0x3C, 0x3D, // 13
-    0x42, 0x43, 0x44, 0x45 }
+    0x3E, 0x3F, 0x40, 0x41, // 14
-/*
+    0x42, 0x43, 0x44, 0x45 }// 15
-#define PCA9685_LED_ADDRS { \
+
-    0x06, 0x07, 0x08, 0x09, \
+#define PCA9685_ALL_LED_ON_L    0xFA
-    0x0A, 0x0B, 0x0C, 0x0D, \
+#define PCA9685_ALL_LED_ON_H    0xFB
-    0x0E, 0x0F, 0x10, 0x11, \
+#define PCA9685_ALL_LED_OFF_L   0xFC
-    0x12, 0x13, 0x14, 0x15, \
+#define PCA9685_ALL_LED_OFF_H   0xFD
-    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
@@ -136,12 +125,4 @@ const int PCA9685_LED_ADDRS[] = {
 #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

CopterController/PCA9685/PCA9685tester.cpp

@@ -0,0 +1,31 @@
+
+#include <iostream>
+#include "stdio.h"
+#include "wiringPi.h"
+#include "PCA9685.h"
+
+int main( int argc, const char* argv[] )
+{
+    wiringPiSetup();
+    PCA9685 servo = PCA9685( 0x40 );
+    servo.setFrequency( 60 );
+    std::cout << "freq set" << std::endl;
+
+    servo.setPwm( 0, 0, 600 );
+    std::cout << "pwm set 600" << std::endl;
+    while( std::cin.get() != '\n' );
+
+    servo.setPwm( 0, 0, 150 );
+    std::cout << "pwm set 150" << std::endl;
+    while( std::cin.get() != '\n' );
+
+    while( true )
+    {
+        servo.setPwm( 0, 0, 600 );
+        std::cout << "pwm: 600" << std::endl;
+        delay( 1000 );
+        servo.setPwm( 0, 0, 150 );
+        std::cout << "pwm: 150" << std::endl;
+        delay( 1000 );
+    }
+}

PCA9685/PCA9685tester.cpp

@@ -1,22 +0,0 @@
-
-#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 );
-	}
-}