void loop()
{
// Declarations
byte cPort, cTxPort;
long lMsgID;
bool bExtendedFormat;
byte cData[8];
byte bData[4];
byte cDataLen;
int lowrange;
unsigned short wSpeedL;
unsigned short wSpeedR;
unsigned short wSpeedA;
unsigned long previousMillis = 0;
unsigned long interval = 1000;
//Serial printing interval
bool SERIAL_OUTPUT= false;
// 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)
{
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval)
{
// save the last time Serial Printing Occured
previousMillis = currentMillis;
// Set Print to serial
SERIAL_OUTPUT = true;
}
else (SERIAL_OUTPUT = false);
}
// 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
wSpeedL = 0;
wSpeedR = 0;
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
if (SERIAL_OUTPUT)
{
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
if (SERIAL_OUTPUT)
{
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 by 2.73 using binary scale factor, ((1/2.73)*4096) = 1500
wSpeedA = ((wSpeedA * 1500)>>12);
if (SERIAL_OUTPUT)
{
Serial.print("wSpeedA Converted = ");
Serial.println(wSpeedA);
}//end If
//Copy revised speed to data array
// memcpy ((byte*)cData[2], (byte*)(wSpeedA), sizeof(wSpeedA));
//memcpy (cData[2], (byte*)&wSpeedA, sizeof(wSpeedA));
cData[2]=highByte(wSpeedA);
cData[3]=lowByte(wSpeedA);
if (SERIAL_OUTPUT)
{
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 using binary scale factor, ((1/2.73)*4096) = 1500
wSpeedL = ((wSpeedL * 1500)>>12);
wSpeedR = ((wSpeedR * 1500)>>12);
//print data for debug
if (SERIAL_OUTPUT)
{
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
} //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)
{
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
}//end If
//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;
if (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
