/*
* Loopback test: CAN1 connected to CAN2
*
Based on demo sketch: Teensy41_240x240_LCD_CANFD from SK Pang.
for this board: http://skpang.co.uk/catalog/teensy-41-triple-can-board-with-240x240-ips-lcd-p-1592.html
v1: Modified to remove the FD stuff. Runs with CAN1 looped back to CAN2 (H to H, L to L)
v2: send messages from CAN1 to CAN2, read and display packet contents from CAN2 receive
v3: clean up unused FD stuff
*/
#include <FlexCAN_T4.h>
#include <SPI.h>
// Create CAN1 instance
FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can1;
FlexCAN_T4<CAN2, RX_SIZE_256, TX_SIZE_16> can2;
IntervalTimer timer;
uint8_t d = 0;
void setup(void) {
Serial.begin(115200); delay(1000);
Serial.println("Test CAN1 to RMD");
can1.begin();
can1.setBaudRate(500000); // start with 500kbps, try 1Mbit/s later (note: does not work at 1M)
can1.setMBFilter(ACCEPT_ALL);
can1.distribute();
can1.mailboxStatus();
can2.begin();
can2.setBaudRate(500000);
can2.setMBFilter(ACCEPT_ALL);
can2.distribute();
can2.mailboxStatus();
timer.begin(sendframe, 1000000); // Send frame every 1s (calls sendframe())
}
void sendframe() // triggered by timer
{
Serial.println("sending msg1");
CAN_message_t msg1;
msg1.id = 0x141; // Message ID = 0x140 + NodeID (as set by DIP switches on motor) - typically "1"
msg1.buf[0] = 0x9A; //0x90 - read encoder data command, 0x9A - read motor status
msg1.buf[1] = 0x01;
msg1.buf[2] = 0x02;
msg1.buf[3] = 0x03;
msg1.buf[4] = 0x04;
msg1.buf[5] = 0x05;
msg1.buf[6] = 0x06;
msg1.buf[7] = 0x07;
can1.write(msg1);
}
void loop() {
CAN_message_t msgcan2;
if (can2.read(msgcan2))
{ /* check if we received a CAN frame */
Serial.println("Got can2");
// Serial.print("MB: "); Serial.print(msgcan2.mb);
// Serial.print(" OVERRUN: "); Serial.print(msgcan2.flags.overrun);
Serial.print(" ID: 0x"); Serial.print(msgcan2.id, HEX );
// Serial.print(" EXT: "); Serial.print(msgcan2.flags.extended );
// Serial.print(" LEN: "); Serial.print(msgcan2.len);
Serial.print(" DATA: ");
for ( uint8_t i = 0; i < msgcan2.len ; i++ ) {
Serial.print(msgcan2.buf[i]); Serial.print(" ");
}
Serial.print(" TS: "); Serial.println(msgcan2.timestamp);
}
}