[136] | 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 | *********************************************************************************************************/
|
---|