//Required Libraries
#include "Arduino.h"
//
//Set debug printing
bool SERIAL_DEBUG = true;
// CAN Layer functions
// -----------------------------------------------------------------------------------------------
#include "DueCANLayer.h"
extern byte canInit(byte cPort, long lBaudRate, int nTxMailboxes);
extern byte canTx(byte cPort, long lMsgID, bool bExtendedFormat, byte* cData, byte cDataLen);
extern byte canRx(byte cPort, long* lMsgID, bool* bExtendedFormat, byte* cData, byte* cDataLen);
// Timer functions
// -----------------------------------------------------------------------------------------------
#include "TimerControl.h"
extern void TimerInit(void);
extern void TimerControl(void);
extern void TimerStart(struct Timer* pTimer, int nCount);
extern void TimerReset(struct Timer* pTimer);
extern struct Timer pTimer[];
// CAN Bus Data Mapping
// -----------------------------------------------------------------------------------------------
struct Mapping
{
byte cReceivingPort; // 0/1
long lReceivingMsgID;
long lTransmittedMsgID;
};
//recieving port and trasmitted ID function retained for future usage
struct Mapping CAN_DataMapping[] = {
// cReceivingPort, lReceivingMsgID, lTransmittedMsgID
0, 0x208, 0x208,
0, 0x200, 0x200 // End of Table
};
int nMappingEntries = 0; // Will be determined in setup()
// Internal functions
// -----------------------------------------------------------------------------------------------
void LEDControl(void);
// Module variables
// -----------------------------------------------------------------------------------------------
int TimerActivity_CAN0 = 0;
int TimerActivity_CAN1 = 0;
int LED1 = 14;
int LED2 = 15;
//Low Range Switch Pin Number (TBD)
int LRSW = 22;
int nTxMailboxes = 3; // Equal portion between transmission and reception
void setup()
{
// Set the serial interface baud rate
Serial.begin(115200);
// Initialzie the timer control; also resets all timers
TimerInit();
// Determine simulation entries
nMappingEntries = 0;
while(1)
{
if(CAN_DataMapping[nMappingEntries].cReceivingPort == 0
|| CAN_DataMapping[nMappingEntries].cReceivingPort == 1)
nMappingEntries++;
else
break;
}
// Initialize the outputs for the LEDs
pinMode(LED1, OUTPUT);
pinMode(LED2, OUTPUT);
// Initialize Low Range Input pin
pinMode(LRSW, INPUT);
// Initialize Low Range Input pin
// Initialize both CAN controllers
if(canInit(0, CAN_BPS_500K, nTxMailboxes) == CAN_OK)
Serial.print("CAN0: Initialized Successfully.\n\r");
else
Serial.print("CAN0: Initialization Failed.\n\r");
if(canInit(1, CAN_BPS_500K, nTxMailboxes) == CAN_OK)
Serial.print("CAN1: Initialized Successfully.\n\r");
else
Serial.print("CAN1: Initialization Failed.\n\r");
}// end setup
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
// ------------------------------------------------------------------------
// LED Data Traffic
// ------------------------------------------------------------------------
// Note: CAN0 --> LED1
// CAN1 --> LED2
//
void LEDControl(void)
{
if(pTimerLEDs.bExpired == true)
{
// Restart the timer
TimerStart(&pTimerLEDs, TIMER_RATE_LED_BLINK);
// Check for activity on CAN0
if(TimerActivity_CAN0 > 0)
{
TimerActivity_CAN0--;
digitalWrite(LED1, HIGH);
}// end if
else
digitalWrite(LED1, LOW);
// Check for activity on CAN1
if(TimerActivity_CAN1 > 0)
{
TimerActivity_CAN1--;
digitalWrite(LED2, HIGH);
}// end if
else
digitalWrite(LED2, LOW);
}// end if
}// end LEDControl
.
