void JTNGObj::engine(int n, JLinkObj& link) {
JBlock data;
link.access(JBlockData(data));
int size = data.size();
data.Dereference();
for (int i=0; i<size; i++)
processByte((uchar)data[i]);
}
/** Checks for new bytes available on the USB virtual COM port
* and processes all that are available. */
void processBytesFromUsb()
{
uint8 bytesLeft = usbComRxAvailable();
while(bytesLeft && usbComTxAvailable() >= sizeof(response))
{
processByte(usbComRxReceiveByte());
bytesLeft--;
}
}
void PS2Keyboard::processBytes() {
if (!ps2_protocol_)
return;
int count = ps2_protocol_->available();
while (count > 0) {
byte b = ps2_protocol_->read();
processByte(b);
--count;
}
}
/** Checks for new bytes available from the wireless radio
* and processes all that are available. */
void processBytesFromRadio()
{
uint8 i, n;
uint8 XDATA *rcv;
rcv = radioLinkRxCurrentPacket();
if(rcv != NULL) {
n = rcv[0]; // payload length
for(i = 1; i <= n && radioLinkTxAvailable(); i++) {
processByte(rcv[i]);
}
radioLinkRxDoneWithPacket();
}
}
// callback on serial data received
void ParseSerial::callbackSerialRx(const boost::shared_ptr<std_msgs::UInt8MultiArray const> &msg){
mel.lock();
// obtain message and the size of the packet
message = (*msg);
size = message.data.size();
// process each byte in the message packet separately
for(unsigned int i=0;i<size;i++){
c = message.data.at(i);
processByte();
}
mel.unlock();
}
void kbInterrupt(void)
{
unsigned char k;
if (input(CLOCK) == 0) { // Check for clock
k = processByte();
if(k != 0)
{
kbBuffer[kbPos] = k;
kbPos = ++kbPos & 0x0F;
kbSize++;
}
return;
// xprintf("%c", k);
}
}
int main()
{
can1.frequency(105263);
can2.frequency(105263);
pc.baud(115200);
ioioSerial.baud(9600);
failsafeTimer.start();
pc.printf("---initialised---\r\n");
ticker.attach(&sendJoystick, 0.05);
canSwitch = 0;
ignition = 0;
wait_ms(500);
canSwitch = 1; // To Joystick unit
wait_ms(500);
ignition = 1;
wait_ms(500);
ignition = 0;
wait_ms(1000);
canSwitch = 0;
CANMessage msg;
while(1) {
if(can1.read(msg)) {
if(debug) pc.printf("(%d) ", msg.id);
for(int i=0; i<msg.len; i++) {
if(debug) pc.printf("[%2.2X]",msg.data[i]);
}
if(debug) pc.printf("\r\n");
led2 = !led2;
}
char c = 0;
if(ioioSerial.readable()) {
c = ioioSerial.getc();
pc.printf("%c",c);
} else {
if(pc.readable()) {
c = pc.getc();
}
}
if(c!=0) {
processByte(c);
}
}
}
void PS2Keyboard::processByteForTesting(byte b) {
processByte(b);
}
void JTNG4Obj::engine(int n, JLinkObj& link) {
switch (n) {
case IN_COM: {
JBlock data;
link.access(JBlockData(data));
int size = data.size();
data.Dereference();
for (int i=0; i<size; i++)
processByte((uchar)data[i]);
break;
}
case IN_A1: {
int v;
link.access(JIntegerData(v));
vi[IN_A1] = (char)(v & 0xFF);
if (!exclusive || !inputSet(IN_CLOCK).last()) procCOM(16);
break;
}
case IN_A2: {
int v;
link.access(JIntegerData(v));
vi[IN_A2] = (char)(v & 0xFF);
if (!exclusive || !inputSet(IN_CLOCK).last()) procCOM(32);
break;
}
case IN_A3: {
int v;
link.access(JIntegerData(v));
vi[IN_A3] = (char)(v & 0xFF);
if (!exclusive || !inputSet(IN_CLOCK).last()) procCOM(64);
break;
}
case IN_A4: {
int v;
link.access(JIntegerData(v));
vi[IN_A4] = (char)(v & 0xFF);
if (!exclusive || !inputSet(IN_CLOCK).last()) procCOM(128);
break;
}
case IN_B: {
int v;
link.access(JIntegerData(v));
vi[IN_B] = (char)(v & 0xFF);
if (!exclusive || !inputSet(IN_CLOCK).last()) procCOM(1);
break;
}
case IN_C: {
int v;
link.access(JIntegerData(v));
vi[IN_C] = (char)(v & 0xFF);
if (!exclusive || !inputSet(IN_CLOCK).last()) procCOM(2);
break;
}
case IN_D: {
int v;
link.access(JIntegerData(v));
vi[IN_D] = (char)(v & 0xFF);
if (!exclusive || !inputSet(IN_CLOCK).last()) procCOM(4);
break;
}
case IN_BCONFIG: {
int v;
link.access(JIntegerData(v));
vi[IN_BCONFIG] = (char)(v & 0xFF);
if (!exclusive || !inputSet(IN_CLOCK).last() && inputSet(IN_B).last()) procCOM(1);
break;
}
case IN_CCONFIG: {
int v;
link.access(JIntegerData(v));
vi[IN_CCONFIG] = (char)(v & 0xFF);
if (!exclusive || !inputSet(IN_CLOCK).last() && inputSet(IN_C).last()) procCOM(2);
break;
}
case IN_DCONFIG: {
int v;
link.access(JIntegerData(v));
vi[IN_DCONFIG] = (char)(v & 0xFF);
if (!exclusive || !inputSet(IN_CLOCK).last() && inputSet(IN_D).last()) procCOM(4);
break;
}
case IN_CLOCK: {
int v;
link.access(JIntegerData(v));
v = (v != 0);
if (v != vi[IN_CLOCK]) {
vi[IN_CLOCK] = (char)v;
if (v) {
char code = 0;
if (inputSet(IN_B).last() ||
inputSet(IN_BCONFIG).last()) code |= 1;
if (inputSet(IN_C).last() ||
inputSet(IN_CCONFIG).last()) code |= 2;
if (inputSet(IN_D).last() ||
inputSet(IN_DCONFIG).last()) code |= 4;
if (inputSet(IN_A1).last()) code |= 16;
if (inputSet(IN_A2).last()) code |= 32;
if (inputSet(IN_A3).last()) code |= 64;
if (inputSet(IN_A4).last()) code |= 128;
if (code) procCOM(code);
}
}
break;
}
default: {
int v;
link.access(JIntegerData(v));
vi[n] = (char)(v & 0xFF);
if (!exclusive || !inputSet(IN_CLOCK).last())
procCOM(0);
break;
}
}
}