void loop()
{
// Declarations
byte cPort, cTxPort;
long lMsgID;
bool bExtendedFormat;
byte cData[8];
byte cDataLen;
int lowrange;
// 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);
// Check for received CAN messages
for(cPort = 0; cPort <= 1; cPort++)
{
if(canRx(cPort, &lMsgID, &bExtendedFormat, &cData[0], &cDataLen) == CAN_OK)
{
// Scan through the mapping list, If frame matches ID and lowrange is high.
for(int nIndex = 0; nIndex < nMappingEntries; nIndex++)
{
if(lMsgID == CAN_DataMapping[nIndex].lReceivingMsgID
// Removed matching for source CAN port
// && cPort == CAN_DataMapping[nIndex].cReceivingPort
&& lowrange == 1
)
{
//If recieved port is CAN0, transmit on CAN1, otherwise transmit on CAN0.
cTxPort = 0;
if(cPort == 0) cTxPort = 1;
//Wheel speed conversion
// Transmit Frame, print error message if CAN_ERROR is returned
if(canTx(cTxPort, CAN_DataMapping[nIndex].lTransmittedMsgID, bExtendedFormat, &cData[0], cDataLen) == CAN_ERROR)
Serial.println("Transmision Error.");
}
//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;
(canTx(cTxPort, lMsgID, bExtendedFormat, &cData[0], cDataLen) == CAN_ERROR)
Serial.println("Transmision Error.");
}// end if else
}// end for
}// end if
}// end for
}// end while
}// end loopvoid loop()
{
// Declarations
byte cPort, cTxPort;
long lMsgID;
bool bExtendedFormat;
byte cData[8];
byte bData[4];
byte cDataLen;
int lowrange;
// 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);
// Check for received CAN messages
for(cPort = 0; cPort <= 1; cPort++)
{
if(canRx(cPort, &lMsgID, &bExtendedFormat, &cData[0], &cDataLen) == CAN_OK)
{
// Scan through the mapping list, If frame matches ID and lowrange is high.
for(int nIndex = 0; nIndex < nMappingEntries; nIndex++)
{
if(lMsgID == CAN_DataMapping[nIndex].lReceivingMsgID
// Removed matching for source CAN port
// && cPort == CAN_DataMapping[nIndex].cReceivingPort
&& lowrange == 1
)
{
//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
short wSpeedL = 0;
short wSpeedR = 0;
short wSpeedA = 0;
//copy speed data bytes to working array from CAN data array
bData[0] = cData[4];
bData[1] = cData[5];
bData[2] = cData[6];
bData[3] = cData[7];
//Set wSpeed to array data
wSpeedL = (wSpeedL << 8) + bData[0];
wSpeedL = (wSpeedL << 8) + bData[1];
wSpeedR = (wSpeedR << 8) + bData[2];
wSpeedR = (wSpeedR << 8) + bData[3];
//print data for debug
Serial.print("wSpeedL raw =");
Serial.println(wSpeedL);
Serial.print("wSpeedR raw =");
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[2];
wSpeedA = (wSpeedA << 8) + cData[3];
//debug printing
Serial.print("Front average speed CAN Raw, cData[2]..[3] =");
Serial.print(cdata[2], HEX);
Serial.println(cdata[3], HEX);
Serial.print("wSpeedA = ");
Serial.println(wSpeedA);
//divide average speed
wSpeedA = (wSpeedL/2.73);
Serial.print("wSpeedA divided = ");
Serial.println(wSpeedA);
//Copy revised speed to data array
void memcpy (cData[2], (byte*)&(wSpeedA),sizeof(wSpeedA));
Serial.print("Front average speed Converted Raw, cData[2]..[3] =");
Serial.print(cdata[2], HEX);
Serial.println(cdata[3], HEX);
} //end if
//Divide Wheel speeds by 2.73
wSpeedL = (wSpeedL/2.73);
wSpeedR = (wSpeedR/2.73);
//print data for debug
for (int mIndex=4; mIndex <= 7; mIndex++)
{
Serial.println("cData [4]..[7]");
Serial.print(mIndex);
Serial.print("-");
Serial.println(cdata[mIndex], HEX);
}//end for loop
//Copy wSpeedL and wSpeedR to working array, Note offset for start byte. Assumes both CAN frame and chip are little endian
void memcpy (bData[0], (byte*)&(wSpeedL),sizeof(wSpeedL));
void memcpy (bData[2], (byte*)&(wSpeedR),sizeof(wSpeedR));
//Print working array contents
for (int mIndex=0; mIndex <= 3; mIndex++)
{
Serial.println("bData[0]..[3]");
Serial.print(mIndex);
Serial.print("-");
Serial.println(bdata[mIndex], HEX);
}//end for loop
//Copy bData to data array
cData[4] = bData[0];
cData[5] = bData[1];
cData[6] = bData[2];
cData[7] = bData[3];
// Transmit Frame, print error message if CAN_ERROR is returned
if(canTx(cTxPort, CAN_DataMapping[nIndex].lTransmittedMsgID, bExtendedFormat, &cData[0], cDataLen) == CAN_ERROR)
Serial.println("Transmision Error.");
}
//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;
(canTx(cTxPort, lMsgID, bExtendedFormat, &cData[0], cDataLen) == CAN_ERROR)
Serial.println("Transmision Error.");
}// end if else
}// end for
}// end if
}// end for
}// end while
}// end loop