int main (void)
{
int myLeftCounter, myRightCounter = 0 ;
setup () ;
for (;;)
{
printf ("Waiting ... \n") ; fflush (stdout) ;
//while (myCounter == lastCounter)
while (1)
{
piLock (COUNT_LEFT) ;
myLeftCounter = globalCounterLeft ;
piUnlock (COUNT_LEFT) ;
piLock (COUNT_RIGHT) ;
myRightCounter = globalCounterRight ;
piUnlock (COUNT_RIGHT) ;
printf("\r Left Count = %d, Right Count = %d (Ctrl^C to quit)\n", myLeftCounter, myRightCounter);
delay (10) ; //wait 10ms
}
}
return 0 ;
}
int main (void)
{
int lastCounter = 0 ;
int myCounter = 0 ;
setup () ;
for (;;)
{
printf ("Waiting ... ") ;
fflush (stdout) ;
while (myCounter == lastCounter)
{
piLock (COUNT_KEY) ;
myCounter = globalCounter ;
piUnlock (COUNT_KEY) ;
delay (500) ;
}
printf (" Done. myCounter: %5d\n", myCounter) ;
lastCounter = myCounter ;
}
return 0 ;
}
cPwmBoard::cPwmBoard()
{
pwmFd = -1;
if (setup() <0)
{
#ifdef PWM_DEBUG
std::cout << "PWM | Failed to initialise PWM board I2C" << std::endl;;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Failed to initialise PWM board I2C" << std::endl;
#endif
}
piLock(0);
if(wiringPiI2CWriteReg8(pwmFd,MODE1,0x01)) //Wake the device
{
#ifdef PWM_DEBUG
std::cout<< "PWM | Could not wake device (MODE1 WRITE)"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Could not wake device (MODE1 WRITE)" << std::endl;
#endif
}
piUnlock(0);
}
void RF22::spiBurstRead(uint8_t reg, uint8_t* dest, uint8_t len) {
piLock(0);
uint8_t data[len + 1];
data[0] = reg & ~RF22_SPI_WRITE_MASK;
for (int i = 0; i < len; i++) {
data[i + 1] = 0x00;
}
if (wiringPiSPIDataRW(_slaveSelectPin, data, len + 1) < 0) {
fprintf(stderr, "SPI send failed: %s\n", strerror(errno));
}
for (int i = 0; i < len; i++) {
dest[i] = data[i + 1];
// fprintf(stdout,"%x ",dest[i]);
}
// xor_pn9(data,20);
// fprintf(stdout,"dewhitened ");
// for(int i=0;i<20;i++){
// fprintf(stdout,"%x ",data[i]);
// }
// fprintf(stdout,"burst read finished:\n");
// fflush(stdout);
piUnlock(0);
}
bool RF22::appendTxBuf(const uint8_t* data, uint8_t len) {
if (((uint16_t) _bufLen + len) > RF22_MAX_MESSAGE_LEN)
return false;
//TODO check
piLock(0);
memcpy(_buf + _bufLen, data, len);
_bufLen += len;
piUnlock(0);
// printBuffer("txbuf:", _buf, _bufLen);
return true;
}
bool RF22::send(const uint8_t* data, uint8_t len) {
waitPacketSent();
//TODO check
piLock(1);
if (!fillTxBuf(data, len))
return false;
startTransmit();
piUnlock(1);
// printBuffer("send:", data, len);
return true;
}
/* piUnlock
*
* Parameters:
* - key: int
* Return Type: void
*/
mrb_value
mrb_Pi_piUnlock(mrb_state* mrb, mrb_value self) {
mrb_int native_key;
/* Fetch the args */
mrb_get_args(mrb, "i", &native_key);
/* Invocation */
piUnlock(native_key);
return mrb_nil_value();
}
void RF22::spiWrite(uint8_t reg, uint8_t val) {
piLock(0);
uint8_t data[2];
data[0] = reg | RF22_SPI_WRITE_MASK;
data[1] = val;
if (wiringPiSPIDataRW(_slaveSelectPin, data, 2) < 0) {
fprintf(stderr, "SPI send failed: %s\n", strerror(errno));
}
piUnlock(0);
}
uint8_t RF22::spiRead(uint8_t reg) {
piLock(0);
uint8_t data[2];
data[0] = reg & ~RF22_SPI_WRITE_MASK;
data[1] = 0x00;
if (wiringPiSPIDataRW(_slaveSelectPin, data, 2) < 0) {
fprintf(stderr, "SPI send failed: %s\n", strerror(errno));
}
piUnlock(0);
return data[1];
}
bool RF22::recv(uint8_t* buf, uint8_t* len) {
if (!available())
return false;
//TODO check
piLock(1);
if (*len > _bufLen)
*len = _bufLen;
memcpy(buf, _buf, *len);
clearRxBuf();
// printBuffer("recv:", buf, *len);
piUnlock(1);
return true;
}
void RF22::spiBurstWrite(uint8_t reg, const uint8_t* src, uint8_t len) {
piLock(0);
uint8_t data[1 + len];
data[0] = reg | RF22_SPI_WRITE_MASK;
for (int i = 0; i < len; i++) {
data[i + 1] = src[i];
}
if (wiringPiSPIDataRW(_slaveSelectPin, data, len + 1) < 0) {
fprintf(stderr, "SPI send failed: %s\n", strerror(errno));
}
piUnlock(0);
}
int cPwmBoard::setDrive(int mode)
{
piLock(0);
int regval = wiringPiI2CReadReg8(pwmFd,MODE2);
if (mode == OPEN_DRAIN)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | Setting drive to Open Drain"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Setting drive to Open Drain" << std::endl;
#endif
regval = bitLow(2,regval); //Bit 2 = 0
}
else if (mode == TOTEM_POLE)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | Setting drive to Totem Pole"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Setting drive to Totem Pole" << std::endl;
#endif
regval = bitHigh(2,regval); //Bit 2 = 1
}
if(wiringPiI2CWriteReg8(pwmFd,MODE2,regval)<0)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | Setting drive mode failed (MODE2)"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Setting drive mode failed (MODE2)" << std::endl;
#endif
return -1;
}
piUnlock(0);
return 0;
}
int cPwmBoard::setFreq(float freq)
{
piLock(0);
if(wiringPiI2CWriteReg8(pwmFd,MODE1,0x11)) //Sleep the device
{
#ifdef PWM_DEBUG
std::cout<<"PWM | Sleeping device failed (MODE1)"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Sleeping device failed (MODE1)" << std::endl;
#endif
}
int regvalue = (int)(25000000.0/(4096.0*freq)) -1;
if(wiringPiI2CWriteReg8(pwmFd,PRESCALE_REG,regvalue)<0)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | Setting prescaler failed (PRESCALE_REG)"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Setting prescaler failed (PRESCALE_REG)" << std::endl;
#endif
return -1;
}
if(wiringPiI2CWriteReg8(pwmFd,MODE1,0x01)) //Wake the device
{
#ifdef PWM_DEBUG
std::cout<<"PWM | Waking device failed (MODE1)"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Waking device failed (MODE1)" << std::endl;
#endif
}
piUnlock(0);
return 0;
}
int cPwmBoard::setPwm(int reg, float duty)
{
int val = (40.96 * duty)-1; //0~4095 values in PWM
int high = val>>8; //High nibble(4bit)
int low = val-(high<<8); //Low byte
#ifdef PWM_DEBUG
std::cout<<"PWM | Writing to "<<std::hex<<reg<<" with value "<<std::hex<<high<<"|"<<std::hex<<low<<std::dec<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Writing to " << std::hex<<reg << " with value " << std::hex<<high << "|" << std::hex<<low << std::dec << std::endl;
#endif
if(pwmFd == -1)
{
#ifdef PWM_DEBUG
std::cout << "PWM | Attempted to write to non-initalised PWM board" << std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Attempted to write to non-initalised PWM board" << std::endl;
#endif
return -1;
}
piLock(0);
if(wiringPiI2CWriteReg8(pwmFd,reg,0)<0)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | PWM write failed ("<<std::hex<< reg <<std::dec<<")"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | PWM write failed (" << std::hex<<reg <<std::dec << ")" << std::endl;
#endif
return -1;
}
if(wiringPiI2CWriteReg8(pwmFd,reg+1,0)<0)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | PWM write failed ("<<std::hex<< reg+1 <<std::dec<<")"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | PWM write failed (" << std::hex<<reg+1 <<std::dec << ")" << std::endl;
#endif
return -1;
}
if(wiringPiI2CWriteReg8(pwmFd,reg+2,low)<0)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | PWM write failed ("<<std::hex<< reg+2 <<std::dec<<")"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | PWM write failed (" << std::hex<<reg+2 <<std::dec << ")" << std::endl;
#endif
return -1;
}
if(wiringPiI2CWriteReg8(pwmFd,reg+3,high)<0)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | PWM write failed ("<<std::hex<< reg+3 <<std::dec<<")"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | PWM write failed (" << std::hex<<reg+3 <<std::dec << ")" << std::endl;
#endif
return -1;
}
piUnlock(0);
return 0;
}
int cPwmBoard::setPwmAll(float duty)
{
int val = 4096 * duty; //4096 values in PWM
int high = val/256; //High byte
int low = val-high; //Low byte
#ifdef PWM_DEBUG
std::cout<<"PWM | Writing to "<<std::hex<<PWM_ALL_ON<<" with value "<<std::hex<<high<<"|"<<std::hex<<low<<std::dec<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Writing to " << std::hex<<PWM_ALL_ON << " with value " << std::hex<<high << "|" << std::hex<<low <<std::dec << std::endl;
#endif
piLock(0);
if(wiringPiI2CWriteReg8(pwmFd,PWM_ALL_ON,0)<0)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | Setting all PWM failed ("<<std::hex<< PWM_ALL_ON <<std::dec<<")"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Setting all PWM failed (" << std::hex<<PWM_ALL_ON <<std::dec << ")" << std::endl;
#endif
return -1;
}
if(wiringPiI2CWriteReg8(pwmFd,PWM_ALL_ON+1,0)<0)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | Setting all PWM failed ("<<std::hex<< PWM_ALL_ON+1 <<std::dec<<")"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Setting all PWM failed (" << std::hex<<PWM_ALL_ON+1 <<std::dec << ")" << std::endl;
#endif
return -1;
}
if(wiringPiI2CWriteReg8(pwmFd,PWM_ALL_OFF,low)<0)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | Setting all PWM failed ("<<std::hex<< PWM_ALL_ON+2 <<std::dec<<")"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Setting all PWM failed (" << std::hex<<PWM_ALL_ON+2 <<std::dec << ")" << std::endl;
#endif
return -1;
}
if(wiringPiI2CWriteReg8(pwmFd,PWM_ALL_OFF+1,high)<0)
{
#ifdef PWM_DEBUG
std::cout<<"PWM | Setting all PWM failed ("<<std::hex<< PWM_ALL_ON+3 <<std::dec<<")"<<std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Setting all PWM failed (" << std::hex<<PWM_ALL_ON+3 <<std::dec << ")" << std::endl;
#endif
return -1;
}
piUnlock(0);
return 0;
}
int cPwmBoard::kill(int reg)
{
#ifdef PWM_DEBUG
std::cout << "PWM | Killing " << std::hex << reg << std::dec << std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Inverted PWM write failed (" << std::hex<<reg+3 <<std::dec << ")" << std::endl;
#endif
if (pwmFd == -1)
{
#ifdef PWM_DEBUG
std::cout << "PWM | Attempted to write to non-initalised PWM board" << std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | Attempted to write to non-initalised PWM board" << std::endl;
#endif
return -1;
}
piLock(0);
if (wiringPiI2CWriteReg8(pwmFd, reg, 0)<0)
{
#ifdef PWM_DEBUG
std::cout << "PWM | PWM write failed (" << std::hex << reg << std::dec << ")" << std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | PWM write failed (" << std::hex << reg << std::dec << ")" << std::endl;
#endif
return -1;
}
if (wiringPiI2CWriteReg8(pwmFd, reg + 1, 0)<0)
{
#ifdef PWM_DEBUG
std::cout << "PWM | PWM write failed (" << std::hex << reg + 1 << std::dec << ")" << std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | PWM write failed (" << std::hex << reg + 1 << std::dec << ")" << std::endl;
#endif
return -1;
}
if (wiringPiI2CWriteReg8(pwmFd, reg + 2, 0)<0)
{
#ifdef PWM_DEBUG
std::cout << "PWM | PWM write failed (" << std::hex << reg + 2 << std::dec << ")" << std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | PWM write failed (" << std::hex << reg + 2 << std::dec << ")" << std::endl;
#endif
return -1;
}
if (wiringPiI2CWriteReg8(pwmFd, reg + 3, 0x10)<0)
{
#ifdef PWM_DEBUG
std::cout << "PWM | PWM write failed (" << std::hex << reg + 3 << std::dec << ")" << std::endl;
#endif
#ifdef LOGGING_FULL
logfile << "PWM | PWM write failed (" << std::hex << reg + 3 << std::dec << ")" << std::endl;
#endif
return -1;
}
piUnlock(0);
return 0;
}
//Lock the thread and add data to the Que name ‘Q’ (Lock in due accessing the Que)
void publishBoxStatus(int drawer,int boxStatus){
piLock(LOCK_KEY);
enqueue(Q,drawer+boxStatus);
piUnlock(LOCK_KEY);
}
void RF22::clearRxBuf() {
piLock(0);
_bufLen = 0;
_rxBufValid = false;
piUnlock(0);
}
void RF22::clearTxBuf() {
piLock(0);
_bufLen = 0;
_txBufSentIndex = 0;
piUnlock(0);
}