void loop()
{
// Declarations
byte cPort, cTxPort;
long lMsgID;
bool bExtendedFormat;
byte cData[8];
byte bData[4];
byte cDataLen;
int lowrange = false;
unsigned short wSpeedL;
unsigned short wSpeedR;
unsigned short wSpeedA;
unsigned long previousMillis = 0;
unsigned long Pinterval = 1000;
//Serial printing interval
bool SERIAL_OUTPUT = true;
// Start timer for LED indicators
TimerStart(&pTimerLEDs, TIMER_RATE_LED_BLINK);
while (1) // Endless loop
{
// Control LED status according to CAN traffic
LEDControl();
//Read low range switch and assign to lowrange varaible
//lowrange = digitalRead(LRSW);
//Serial Print Interval
if ( SERIAL_DEBUG = true)
{
unsigned long currentMillis = millis();
if ((currentMillis - previousMillis) >= Pinterval)
{
// save the last time Serial Printing Occured
previousMillis = currentMillis;
// Set Print to serial
SERIAL_OUTPUT = true;
}// end if
else (SERIAL_OUTPUT = false);
}//end if
// Check for received CAN messages
for (cPort = 0; cPort <= 1; cPort++)
{
// removed lowrange == true
if (canRx(cPort, &lMsgID, &bExtendedFormat, &cData[0], &cDataLen) == CAN_OK )
{
// Scan through the mapping list, If frame matches ID
for (int nIndex = 0; nIndex < nMappingEntries; nIndex++)
{
if (lMsgID == CAN_DataMapping[nIndex].lReceivingMsgID)
// Removed matching for source CAN port
// && cPort == CAN_DataMapping[nIndex].cReceivingPort
{
if (SERIAL_OUTPUT)
{
Serial.print("cPort");
Serial.print("\t");
Serial.print("lMsgID");
Serial.print("\t");
Serial.println("cData [4]..[7]");
Serial.print(cPort);
Serial.print("\t");
Serial.print(lMsgID, HEX);
Serial.print("\t");
for (int mIndex = 4; mIndex <= 7; mIndex++)
{
Serial.print(cData[mIndex], HEX);
Serial.print(":");
}//end for loop
Serial.println();
}
//If recieved port is CAN0, transmit on CAN1, otherwise transmit on CAN0.
cTxPort = 0;
if (cPort == 0) cTxPort = 1;
//Initialize wheel speed int to zero
wSpeedL = 0;
wSpeedR = 0;
wSpeedA = 0;
//copy speed data bytes to working array from CAN data array
bData[0] = cData[5];
bData[1] = cData[4];
bData[2] = cData[7];
bData[3] = cData[6];
//Set wSpeed to array data
wSpeedR = (wSpeedL << 8) + bData[0];
wSpeedR = (wSpeedL << 8) + bData[1];
wSpeedL = (wSpeedR << 8) + bData[2];
wSpeedL = (wSpeedR << 8) + bData[3];
//print data for debug
if (SERIAL_OUTPUT)
{
Serial.print("wSpeedL raw");
Serial.print("\t");
Serial.println("wSpeedR raw");
Serial.print(wSpeedL);
Serial.print("\t");
Serial.println(wSpeedR);
}
//For Front wheels, frame 0x200, adjust front wheel average speed.
if (lMsgID == 0x200)
{
// copy speed data to wSpeedA
wSpeedA = (wSpeedA << 8) + cData[3];
wSpeedA = (wSpeedA << 8) + cData[2];
//debug printing
//if (SERIAL_OUTPUT)
// {
// Serial.println("Front average speed CAN Raw, cData[2]..[3] wSpeedA ");
// Serial.print(cData[2], HEX);
// Serial.print(":");
// Serial.print(cData[3], HEX);
// Serial.println(wSpeedA);
// }
//divide average speed by 2.73 using binary scale factor, ((1/2.73)*4096) = 1500
wSpeedA = ((wSpeedA * 1500) >> 12);
if (SERIAL_OUTPUT)
{
Serial.print("wSpeedA divided = ");
Serial.println(wSpeedA);
}//end If
//Copy revised speed to data array
// memcpy ((byte*)cData[2], (byte*) & (wSpeedA), sizeof(wSpeedA));
cData[2] = highByte(wSpeedA);
cData[3] = lowByte(wSpeedA);
if (SERIAL_OUTPUT)
{
Serial.println("Front average speed Converted Raw, cData[2]..[3]");
Serial.print(cData[2], HEX);
Serial.print("\t");
Serial.println(cData[3], HEX);
}
} //end if
//Divide Wheel speeds by 2.73 using binary scale factor, ((1/2.73)*4096) = 1500
wSpeedL = ((wSpeedL * 1500) >> 12);
wSpeedR = ((wSpeedR * 1500) >> 12);
//print data for debug
if (SERIAL_OUTPUT)
{
Serial.println();
Serial.print("wSpeedL divided, wSpeedR divided ");
Serial.print(wSpeedL);
Serial.print("\t");
Serial.println(wSpeedR);
}//end If
//Copy wSpeedL and wSpeedR to working array, Note offset for start byte. Assumes both CAN frame and chip are little endian
// memcpy( (byte*)bData[0], (byte*) & (wSpeedL), sizeof(wSpeedL));
//memcpy( (byte*)bData[2], (byte*) & (wSpeedR), sizeof(wSpeedR));
bData[0] = highByte(wSpeedL);
bData[1] = lowByte(wSpeedL);
bData[2] = highByte(wSpeedL);
bData[3] = lowByte(wSpeedL);
//Print working array contents
if (SERIAL_OUTPUT)
{
Serial.println("bData[0]..[3]");
for (int mIndex = 0; mIndex <= 3; mIndex++)
{
Serial.print(bData[mIndex], HEX);
Serial.print(":");
}//end for loop
Serial.println();
}//end If
//Copy bData to data array
cData[5] = bData[0];
cData[4] = bData[1];
cData[7] = bData[2];
cData[6] = bData[3];
// Transmit Frame, print error message if CAN_ERROR is returned
if (canTx(cTxPort, lMsgID, bExtendedFormat, &cData[0], cDataLen) == CAN_ERROR)
{
Serial.println("Transmision Error2.");
Serial.print(cTxPort);
Serial.print("\t");
Serial.print(lMsgID, HEX);
Serial.print("\t");
Serial.print(bExtendedFormat);
Serial.print("\t");
for (int mIndex = 0; mIndex < cDataLen; mIndex++)
{
Serial.print(cData[mIndex], HEX);
Serial.print(":");
}
Serial.print("\t");
Serial.println(cDataLen);
}
} //end if
//Transmit Frames If low range is not active, or for non-matching frames
else
{
//If recieved port is CAN0, transmit on CAN1, otherwise transmit on CAN0.
cTxPort = 0;
if (cPort == 0) cTxPort = 1;
if (canTx(cTxPort, lMsgID, bExtendedFormat, &cData[0], cDataLen) == CAN_ERROR)
{
if (lMsgID == 0x200 || lMsgID == 0x208)
{
//Serial.println("Transmision Error1.");
Serial.print(cTxPort);
Serial.print("\t");
Serial.print(lMsgID, HEX);
Serial.print("\t");
Serial.print(bExtendedFormat);
Serial.print("\t");
for (int mIndex = 0; mIndex < cDataLen; mIndex++)
{
Serial.print(cData[mIndex], HEX);
Serial.print(":");
}
Serial.print("\t");
Serial.println(cDataLen);
}
}
}// end if else
}// end for
}// end if
}// end for
} //end while
} //end loop
