Code:
#ifndef _CANOPEN_
#define _CANOPEN_
#include <Arduino.h>
#include <FlexCAN_T4.h>
/*
* Contains several individual
* CANopen messages that can
* be sent sequentially
*/
template <size_t N>
class Instruction
{
public:
template <CAN_DEV_TABLE _bus,
FLEXCAN_RXQUEUE_TABLE _rxSize,
FLEXCAN_TXQUEUE_TABLE _txSize>
void sendInstruction(FlexCAN_T4<_bus, _rxSize, _txSize> canBus);
void printInstruction();
void addFrame(uint32_t header, uint8_t buf[8]);
private:
CAN_message_t msgs[N]; // array of messages to send
int index = 0;
template <CAN_DEV_TABLE _bus,
FLEXCAN_RXQUEUE_TABLE _rxSize,
FLEXCAN_TXQUEUE_TABLE _txSize>
void transmitFrame(FlexCAN_T4<_bus, _rxSize, _txSize> canBus, CAN_message_t &msg);
void errorHandler(int e);
};
template <size_t N>
template <CAN_DEV_TABLE _bus,
FLEXCAN_RXQUEUE_TABLE _rxSize,
FLEXCAN_TXQUEUE_TABLE _txSize>
void Instruction<N>::sendInstruction(FlexCAN_T4<_bus, _rxSize, _txSize> canBus)
{
for (unsigned int i = 0; i < N; i++)
{
transmitFrame(canBus, this->msgs[i]);
// TODO delay or find a way to only send one message or delay
// TODO send in sequential order
}
return;
}
template <size_t N>
template <CAN_DEV_TABLE _bus,
FLEXCAN_RXQUEUE_TABLE _rxSize,
FLEXCAN_TXQUEUE_TABLE _txSize>
void Instruction<N>::transmitFrame(FlexCAN_T4<_bus, _rxSize, _txSize> canBus, CAN_message_t &msg)
{
Serial.print("MB ");
Serial.print(msg.mb);
Serial.print(" OVERRUN: ");
Serial.print(msg.flags.overrun);
Serial.print(" LEN: ");
Serial.print(msg.len);
Serial.print(" EXT: ");
Serial.print(msg.flags.extended);
Serial.print(" TS: ");
Serial.print(msg.timestamp);
Serial.print(" ID: ");
Serial.print(msg.id, HEX);
Serial.print(" Buffer: ");
for (uint8_t i = 0; i < msg.len; i++)
{
Serial.print(msg.buf[i], HEX);
Serial.print(" ");
}
Serial.println();
canBus.write(msg);
return;
}
template <size_t N>
void Instruction<N>::addFrame(uint32_t header, uint8_t buf[8])
{
CAN_message_t msg;
msg.id = header;
for (int i = 0; i < 8; i++)
{
msg.buf[i] = buf[i]; // copy buf into new message
}
this->msgs[this->index] = msg; // assign member var and increment index
this->index++;
return;
};
template <size_t N>
void Instruction<N>::printInstruction()
{
Serial.println("=======================");
Serial.print("index: ");
Serial.println(this->index);
for (int i = 0; i < this->index; i++)
{
Serial.print(i);
Serial.print(" : ");
Serial.println(this->msgs[i].id, HEX);
for (int j = 0; j < 8; j++)
{
Serial.print(" =>");
Serial.println(this->msgs[i].buf[j], HEX);
}
}
return;
}
template <size_t N>
void Instruction<N>::errorHandler(int e)
{
switch (e)
{
case 102: // too may frames added
Serial.println("**ERROR**");
Serial.println("Attempted to add too many frames,");
Serial.print(" => Current frame limit: ");
Serial.println(N);
Serial.println("Increase your instruction size...");
break;
default:
Serial.println("Something went wrong");
break;
}
return;
}
#endif
#ifndef _CANOPEN_MSGS_
#define _CANOPEN_MSGS_
/* this would most likely be moved to a file */
/* set Number of mapped objects TPDO 1 to 0x00 */
uint8_t msg0[] = {
0x2f,
0x00,
0x1A,
0x00,
0x00,
0x00,
0x00,
0x00};
/* set TPDO 1 mapping information 1 to 0x60640020 */
uint8_t msg1[] = {
0x23,
0x00,
0x1a,
0x01,
0x20,
0x00,
0x64,
0x60};
/* set Number of mapped objects TPDO 1 to 0x01 */
uint8_t msg2[] = {
0x2f,
0x00,
0x1a,
0x00,
0x01,
0x00,
0x00,
0x00};
/* set Transmission type TPDO 1 to 0x01 */
uint8_t msg3[] = {
0x2f,
0x00,
0x18,
0x02,
0x01,
0x00,
0x00,
0x00};
/* set Communication Cycle Period to 0x000186a0 */
uint8_t msg4[] = {
0x23,
0x06,
0x10,
0x00,
0xa0,
0x86,
0x01,
0x00};
/* set COB-ID SYNC message to 0xc0000080 */
uint8_t msg5[] = {
0x23,
0x05,
0x10,
0x00,
0x80,
0x00,
0x00,
0xc0};
/* start remote node - node 1 */
uint8_t msg6[] = {
0x01,
0x01,
0x00,
0x00,
0x00,
0x00,
0x00,
0x00};
#endif