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;
}
void getControllerData() {
uint8_t bitC = PINC;
uint8_t bitD = PIND;
uint8_t bitE = PINE;
// PORTC - buttons 1 to 8
usbControllerState.button01 = bitLow(bitC, 7); // C7
usbControllerState.button02 = bitLow(bitC, 6); // C6
usbControllerState.button03 = bitLow(bitC, 5); // C5
usbControllerState.button04 = bitLow(bitC, 4); // C4
usbControllerState.button05 = bitLow(bitC, 3); // C3
usbControllerState.button06 = bitLow(bitC, 2); // C2
usbControllerState.button07 = bitLow(bitC, 1); // C1
usbControllerState.button08 = bitLow(bitC, 0); // C0
// PORTD - buttons 9 to 16
usbControllerState.button09 = bitLow(bitD, 7); // D7
usbControllerState.button10 = bitLow(bitE, 0); // E0 (as D6 is hardwired to the LED)
usbControllerState.button11 = bitLow(bitD, 5); // D5
usbControllerState.button12 = bitLow(bitD, 4); // D4
usbControllerState.button16 = bitLow(bitD, 0); // D0
// unused buttons
usbControllerState.button17 = 0;
usbControllerState.button18 = 0;
usbControllerState.button19 = 0;
// OPTION0 (E6) - DECODE
if (bitLow(bitE, 6)) {
// decoder disabled
usbControllerState.button13 = bitLow(bitD, 3); // D3
usbControllerState.button14 = bitLow(bitD, 2); // D2
usbControllerState.button15 = bitLow(bitD, 1); // D1
usbControllerState.button20 = 0;
usbControllerState.button21 = 0;
usbControllerState.button22 = 0;
usbControllerState.button23 = 0;
usbControllerState.button24 = 0;
} else {
// decoder active
usbControllerState.button13 = 0;
usbControllerState.button14 = 0;
usbControllerState.button15 = 0;
// Gear
// 4=1+2/!3+4 = 4
// 3=2/4 = 2
// 2=1/3 = 1
switch (bitLow(bitD, 3) + bitLow(bitD, 2) * 2 + bitLow(bitD, 1) * 4) {
case 0x05:
// gear 1
usbControllerState.button20 = 0;
usbControllerState.button21 = 1;
usbControllerState.button22 = 0;
usbControllerState.button23 = 0;
usbControllerState.button24 = 0;
break;
case 0x06:
// gear 2
usbControllerState.button20 = 0;
usbControllerState.button21 = 0;
usbControllerState.button22 = 1;
usbControllerState.button23 = 0;
usbControllerState.button24 = 0;
break;
case 1:
// gear 3
usbControllerState.button20 = 0;
usbControllerState.button21 = 0;
usbControllerState.button22 = 0;
usbControllerState.button23 = 1;
usbControllerState.button24 = 0;
break;
case 2:
// gear 4
usbControllerState.button20 = 0;
usbControllerState.button21 = 0;
usbControllerState.button22 = 0;
usbControllerState.button23 = 0;
usbControllerState.button24 = 1;
break;
default:
// neutral
usbControllerState.button20 = 1;
usbControllerState.button21 = 0;
usbControllerState.button22 = 0;
usbControllerState.button23 = 0;
usbControllerState.button24 = 0;
}
}
// get the analog data
// OPTION1 (E7) - mode selection
if (bitLow(bitE, 7)) {
// flight stick mode
usbControllerState.rz_axis = 0x0200;
usbControllerState.z_axis = analogRead(2);
usbControllerState.y_axis = analogRead(1);
usbControllerState.x_axis = analogRead(0);
} else {
// racing mode
usbControllerState.rz_axis = analogRead(2);
usbControllerState.z_axis = analogRead(1);
usbControllerState.y_axis = 0x0200;
usbControllerState.x_axis = analogRead(0);
}
}
