Arduino程序 16路模塊PWM舵機驅動板控制器 機器人 IIC接口

1. #include <Adafruit_PWMServoDriver.h>
   
2. #include <Wire.h>
   
3. #if defined(__AVR__)
   
4. #define WIRE Wire
   
5. #elif defined(CORE_TEENSY) // Teensy boards
   
6. #define WIRE Wire
   
7. #else // Arduino Due
   
8. #define WIRE Wire1
   
9. #endif
   
10. 
    
11. // Set to true to print some debug messages, or false to disable them.
    
12. #define ENABLE_DEBUG_OUTPUT true
    
13. 
    
14. Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(uint8_t addr) {
    
15.   _i2caddr = addr;
    
16. }
    
17. 
    
18. void Adafruit_PWMServoDriver::begin(void) {
    
19. WIRE.begin();
    
20. reset();
    
21. }
    
22. 
    
23. 
    
24. void Adafruit_PWMServoDriver::reset(void) {
    
25. write8(PCA9685_MODE1, 0x0);
    
26. }
    
27. 
    
28. void Adafruit_PWMServoDriver::setPWMFreq(float freq) {
    
29.   //Serial.print("Attempting to set freq ");
    
30.   //Serial.println(freq);
    
31.   freq *= 0.9;  // Correct for overshoot in the frequency setting (see issue #11).
    
32.   float prescaleval = 25000000;
    
33.   prescaleval /= 4096;
    
34.   prescaleval /= freq;
    
35.   prescaleval -= 1;
    
36.   if (ENABLE_DEBUG_OUTPUT) {
    
37.     Serial.print("Estimated pre-scale: "); Serial.println(prescaleval);
    
38.   }
    
39.   uint8_t prescale = floor(prescaleval + 0.5);
    
40.   if (ENABLE_DEBUG_OUTPUT) {
    
41.     Serial.print("Final pre-scale: "); Serial.println(prescale);
    
42.   }
    
43.   
    
44.   uint8_t oldmode = read8(PCA9685_MODE1);
    
45.   uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep
    
46.   write8(PCA9685_MODE1, newmode); // go to sleep
    
47.   write8(PCA9685_PRESCALE, prescale); // set the prescaler
    
48.   write8(PCA9685_MODE1, oldmode);
    
49.   delay(5);
    
50.   write8(PCA9685_MODE1, oldmode | 0xa1);  //  This sets the MODE1 register to turn on auto increment.
    
51.                                           // This is why the beginTransmission below was not working.
    
52.   //  Serial.print("Mode now 0x"); Serial.println(read8(PCA9685_MODE1), HEX);
    
53. }
    
54. 
    
55. void Adafruit_PWMServoDriver::setPWM(uint8_t num, uint16_t on, uint16_t off) {
    
56.   //Serial.print("Setting PWM "); Serial.print(num); Serial.print(": "); Serial.print(on); Serial.print("->"); Serial.println(off);
    
57. 
    
58.   WIRE.beginTransmission(_i2caddr);
    
59.   WIRE.write(LED0_ON_L+4*num);
    
60.   WIRE.write(on);
    
61.   WIRE.write(on>>8);
    
62.   WIRE.write(off);
    
63.   WIRE.write(off>>8);
    
64.   WIRE.endTransmission();
    
65. }
    
66. 
    
67. // Sets pin without having to deal with on/off tick placement and properly handles
    
68. // a zero value as completely off.  Optional invert parameter supports inverting
    
69. // the pulse for sinking to ground.  Val should be a value from 0 to 4095 inclusive.
    
70. void Adafruit_PWMServoDriver::setPin(uint8_t num, uint16_t val, bool invert)
    
71. {
    
72.   // Clamp value between 0 and 4095 inclusive.
    
73.   val = min(val, 4095);
    
74.   if (invert) {
    
75.     if (val == 0) {
    
76.       // Special value for signal fully on.
    
77.       setPWM(num, 4096, 0);
    
78.     }
    
79.     else if (val == 4095) {
    
80.       // Special value for signal fully off.
    
81.       setPWM(num, 0, 4096);
    
82.     }
    
83.     else {
    
84.       setPWM(num, 0, 4095-val);
    
85.     }
    
86.   }
    
87.   else {
    
88.     if (val == 4095) {
    
89.       // Special value for signal fully on.
    
90.       setPWM(num, 4096, 0);
    
91.     }
    
92.     else if (val == 0) {
    
93.       // Special value for signal fully off.
    
94.       setPWM(num, 0, 4096);
    
95.     }
    
96.     else {
    
97.       setPWM(num, 0, val);
    
98.     }
    
99.   }
    
100. }
     
101. 
     
102. uint8_t Adafruit_PWMServoDriver::read8(uint8_t addr) {
     
103.   WIRE.beginTransmission(_i2caddr);
     
104.   WIRE.write(addr);
     
105.   WIRE.endTransmission();
     
106. 
     
107.   WIRE.requestFrom((uint8_t)_i2caddr, (uint8_t)1);
     
108.   return WIRE.read();
     
109. }
     
110. 
     
111. void Adafruit_PWMServoDriver::write8(uint8_t addr, uint8_t d) {
     
112.   WIRE.beginTransmission(_i2caddr);
     
113.   WIRE.write(addr);
     
114.   WIRE.write(d);
     
115.   WIRE.endTransmission();
     
116. }
     

復制代碼