1 | // demo: CAN-BUS Shield, send data
|
---|
2 | // Hardware: TWO ArduinoMega with CAN-SHIELDS
|
---|
3 | // Run this send_Blink_ROS.ino file on one of the Arduinos
|
---|
4 | // Run receive_Blink on the other
|
---|
5 | // ***ROS commands to be followe***//
|
---|
6 | // roscore
|
---|
7 | // rosrun roial_python serial_node.py /dev/ttyACM1 _baud:=57600
|
---|
8 | // rostopic pub toggle_led std_msgs/Empty -r 100
|
---|
9 | // Jaghvi: jaghvim@andrew.cmu.edu
|
---|
10 |
|
---|
11 |
|
---|
12 | #include <mcp_can.h>
|
---|
13 | #include <SPI.h>
|
---|
14 |
|
---|
15 | //ROS
|
---|
16 | #include <ros.h>
|
---|
17 | #include <std_msgs/Empty.h>
|
---|
18 |
|
---|
19 | ros::NodeHandle nh;
|
---|
20 |
|
---|
21 | const int SPI_CS_PIN = 9;
|
---|
22 | const int ledHIGH=1;
|
---|
23 | const int ledLOW=0;
|
---|
24 | unsigned char stmp[8] = {ledHIGH, 1, 2, 3, ledLOW, 5, 6, 7};
|
---|
25 |
|
---|
26 | MCP_CAN CAN(SPI_CS_PIN); // Set CS pin
|
---|
27 |
|
---|
28 | void messageCb( const std_msgs::Empty& toggle_msg){
|
---|
29 | //digitalWrite(13, HIGH-digitalRead(13)); // blink the led
|
---|
30 | // send data: id = 0x00, standrad frame, data len = 8, stmp: data buf
|
---|
31 | CAN.sendMsgBuf(0x70,0, 8, stmp);
|
---|
32 | delay(1000); // send data per 100ms
|
---|
33 | }
|
---|
34 |
|
---|
35 | ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );
|
---|
36 |
|
---|
37 | // the cs pin of the version after v1.1 is default to D9
|
---|
38 | // v0.9b and v1.0 is default D10
|
---|
39 |
|
---|
40 |
|
---|
41 |
|
---|
42 | void setup()
|
---|
43 | {
|
---|
44 | Serial.begin(115200);
|
---|
45 | nh.initNode();
|
---|
46 | nh.subscribe(sub);
|
---|
47 |
|
---|
48 | START_INIT:
|
---|
49 |
|
---|
50 | if(CAN_OK == CAN.begin(CAN_500KBPS)) // init can bus : baudrate = 500k
|
---|
51 | {
|
---|
52 | Serial.println("CAN BUS Shield init ok!");
|
---|
53 | }
|
---|
54 | else
|
---|
55 | {
|
---|
56 | Serial.println("CAN BUS Shield init fail");
|
---|
57 | Serial.println("Init CAN BUS Shield again");
|
---|
58 | delay(100);
|
---|
59 | goto START_INIT;
|
---|
60 | }
|
---|
61 | }
|
---|
62 |
|
---|
63 |
|
---|
64 | void loop()
|
---|
65 | {
|
---|
66 |
|
---|
67 | nh.spinOnce();
|
---|
68 | delay(1);
|
---|
69 | }
|
---|
70 |
|
---|
71 | /*********************************************************************************************************
|
---|
72 | END FILE
|
---|
73 | *********************************************************************************************************/
|
---|