Alternative PivotPi CodeΒΆ

A simple example of programming the PivotPi directly with wiringPi. Note that I2C must be enabled (the simplest way to do this is with raspi-config and a reboot).

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
// C+- code: C++ constructs but more of a C style of coding
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <assert.h>
#include <math.h>
////////////////////////////////////////////////////////////////////////////////////////////////////
// build with: gcc PivotPi.cc -lwiringPi -lm -o PivotPi
////////////////////////////////////////////////////////////////////////////////////////////////////
// Boilerplate - things I'm used to...
typedef unsigned char      u8;
typedef unsigned short     u16;
typedef unsigned long      u32;
typedef unsigned long long u64;
typedef float              f32;
typedef double             f64;
static_assert(sizeof(u8)  == 1);
static_assert(sizeof(u16) == 2);
static_assert(sizeof(u32) == 4);
static_assert(sizeof(u64) == 8);
static_assert(sizeof(f32) == 4);
static_assert(sizeof(f64) == 8);
////////////////////////////////////////////////////////////////////////////////////////////////////
f32 clamp01(f32 x)
{
  return x > 1.0 ? 1.0 : (x < 0.0 ? 0.0 : x);
}
////////////////////////////////////////////////////////////////////////////////////////////////////
struct PivotPi
{
  int fd;
  f32 frequency;
};
////////////////////////////////////////////////////////////////////////////////////////////////////
namespace pca9685
{
  const u8 mode1          = 0x00;
  const u8 mode2          = 0x01;
  const u8 prescale       = 0xFE;

  const u8 led_base_on_l  = 0x06;
  const u8 led_base_on_h  = 0x07;
  const u8 led_base_off_l = 0x08;
  const u8 led_base_off_h = 0x09;

  const u8 outdrive_bit   = 0x04;
  const u8 sleep_bit      = 0x10;
  const u8 restart_bit    = 0x80;
};
////////////////////////////////////////////////////////////////////////////////////////////////////
// Okay to call with frequency unset.
void PivotPiWriteChannel(PivotPi* pp, u8 channel, u16 hw_on, u16 hw_off)
{
  // Should never send a bad channel, but if it happens in release, be safe!
  assert(channel < 16);
  if(channel >= 16)
    return;

  wiringPiI2CWriteReg8(pp->fd, pca9685::led_base_on_l  + 4 * channel, hw_on & 0xFF);
  wiringPiI2CWriteReg8(pp->fd, pca9685::led_base_on_h  + 4 * channel, hw_on >> 8);
  wiringPiI2CWriteReg8(pp->fd, pca9685::led_base_off_l + 4 * channel, hw_off & 0xFF);
  wiringPiI2CWriteReg8(pp->fd, pca9685::led_base_off_h + 4 * channel, hw_off >> 8);
}
////////////////////////////////////////////////////////////////////////////////////////////////////
// Okay to call with frequency unset.
void PivotPiClear(PivotPi* pp)
{
  for(u8 i = 0; i < 8; ++ i)
  {
    PivotPiWriteChannel(pp, i, 0, 0x1000);
    // PivotPi LEDs are in hi 8 channels and are inverted  
    PivotPiWriteChannel(pp, i + 8, 0x1000, 0);
  }
}
////////////////////////////////////////////////////////////////////////////////////////////////////
bool PivotPiInlz(PivotPi* pp, const int i2cAddress, f32 frequency)
{
  // Check i2c address
  int fd = wiringPiI2CSetup(i2cAddress);
  if (fd < 0)
    return false;

  pp->fd = fd;

  // See http://www.nxp.com/documents/data_sheet/PCA9685.pdf page 25
  f64 clock = 25.0 * 1000 * 1000;
  f64 prescalef64 = round( clock / (4096.0 * frequency) ) - 1.0;

  u8 prescale = prescalef64 > 255.0 ? 255 : (prescalef64 < 3.0 ? 3 : u8(prescalef64));
  
  // Prescale = 0x03 -> 1525.9 Hz, 0xFF -> 23.8 Hz
  pp->frequency = f32(clock / (prescale + 1.0) / 4096.0);

  // Outdrive is set based on PivotPi sample code from Dexter Industries
  // See https://github.com/DexterInd/PivotPi/tree/master/Software/C
  u8 cur_mode1 = wiringPiI2CReadReg8(fd, pca9685::mode1);
  u8 cur_mode2 = wiringPiI2CReadReg8(fd, pca9685::mode2);
  if(cur_mode1 != 0 || cur_mode2 != pca9685::outdrive_bit)
  {
    wiringPiI2CWriteReg8(fd, pca9685::mode2, pca9685::outdrive_bit);
    wiringPiI2CWriteReg8(fd, pca9685::mode1, 0);
    delay(5);
  }

  u8 cur_prescale = wiringPiI2CReadReg8(fd, pca9685::prescale);
  if(cur_prescale != prescale)
  {
    // See http://www.nxp.com/documents/data_sheet/PCA9685.pd
    wiringPiI2CWriteReg8(fd, pca9685::mode1, pca9685::sleep_bit);
    wiringPiI2CWriteReg8(fd, pca9685::prescale, prescale);
    wiringPiI2CWriteReg8(fd, pca9685::mode1, 0);
    delay(1);
    // Assume restart is required.
    wiringPiI2CWriteReg8(fd, pca9685::mode1, pca9685::restart_bit);
    // All channels should be off; LEDs need to be inverted
    PivotPiClear(pp);
  }
  return true;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
void PivotPiPwmFracOn(PivotPi* pp, u8 channel, f32 amount)
{  
  u16 hw_on  = 0;
  u16 hw_off = (unsigned int)(clamp01(amount) * 4096.0f + 0.5f);
  // Special cases; see http://www.nxp.com/documents/data_sheet/PCA9685.pdf
  // If we want to be fully off, use full off bit.
  // If we want to be fully on, use full on bit.
  if(hw_off == 0)
    hw_off = 4096;
  else if(hw_off == 4096)
  {
    hw_on = 4096;
    hw_off = 0;
  }

  PivotPiWriteChannel(pp, channel, hw_on, hw_off);
}
////////////////////////////////////////////////////////////////////////////////////////////////////
// Set the servo pulse to send in microseconds; pass 0 for off
void PivotPiServoPulse(PivotPi* pp, u16 servo, f32 wd)
{
  assert(servo < 8);
  if(servo >= 8)
    return;
  
  f32 swd = wd / 1000.0f / 1000.0f;
  f32 window = 1.0f / pp->frequency;
  PivotPiPwmFracOn(pp, servo, swd / window );
}
////////////////////////////////////////////////////////////////////////////////////////////////////
void PivotPiLedBrightness(PivotPi* pp, u8 led, f32 brightness)
{
  assert(led < 8);
  if(led >= 8)
    return;

  // Leds are inverted from output
  PivotPiPwmFracOn(pp, led + 8, 1.0 - brightness);
}
////////////////////////////////////////////////////////////////////////////////////////////////////
PivotPi pp;
////////////////////////////////////////////////////////////////////////////////////////////////////
void clear_atexit()
{
  PivotPiClear(&pp);
}
////////////////////////////////////////////////////////////////////////////////////////////////////
void sigint_handler(int dummy)
{
  // Make sure the atext clear gets run
  exit(1);
}
////////////////////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char** arg)
{
  // Catch control-C or other exit and clear LEDs and servos
  atexit(clear_atexit);
  signal(SIGINT, sigint_handler);
  
  // I2C Address 0x40 corresponds to switches set to 00 and use 60 Hz for PWM.
  if(!PivotPiInlz(&pp, 0x40, 60))
  {
    printf("Error in setup\n");
    return -1;
  }

  // Any parameter on the command line will clear through atexit
  if(argc > 1)
    exit(0);

  // Cycle the lights and servo 1 (channel 0)'s position
  int j = 0;
  while(1)
  {
    for (int i = 0; i < 8; i++)
      PivotPiLedBrightness(&pp, i, i == j);

    // Range determined experimentally for one of my servos.
    PivotPiServoPulse(&pp, 0, 550 + (j * (2454 - 550)/7));
    j = j + 1;
    j = j % 8;

    // Give servo time to move.
    delay(400);
  }
}
////////////////////////////////////////////////////////////////////////////////////////////////////