collab/igvc2018

git clone https://git.julia.blue/collab/igvc2018

Joseph RyanMerge pull request #2 from ut-ras/motorfbe4edd

documentation
1.3 KiB55 linesraw
1/**
2This node runs on the MCU. To edit it you need to follow the tutorials at
3http://wiki.ros.org/rosserial_tivac/Tutorials/Energia%20Setup.
4*/
5
6#include <ros.h>
7#include <std_msgs/ColorRGBA.h>
8#include <std_msgs/Float32.h>
9#include <Servo.h>
10
11Servo left_motor;
12Servo right_motor;
13
14void color_cb(const std_msgs::ColorRGBA& msg){
15    analogWrite(RED_LED, msg.r * msg.a);
16    analogWrite(GREEN_LED, msg.g * msg.a);
17    analogWrite(BLUE_LED, msg.b * msg.a);
18}
19
20//TODO: add watchdog
21void left_cb(const std_msgs::Float32& msg){
22    left_motor.write(90 + msg.data * 90);
23}
24
25void right_cb(const std_msgs::Float32& msg){
26    right_motor.write(90 + msg.data * 90);
27}
28
29ros::NodeHandle nh;
30ros::Subscriber < std_msgs::ColorRGBA > color_sub("led", &color_cb);
31ros::Subscriber < std_msgs::Float32 > left_sub("left", &left_cb);
32ros::Subscriber < std_msgs::Float32 > right_sub("right", &right_cb);
33
34void setup(){
35    pinMode(RED_LED, OUTPUT);
36    pinMode(GREEN_LED, OUTPUT);
37    pinMode(BLUE_LED, OUTPUT);
38
39    analogWrite(RED_LED, 255);
40    analogWrite(GREEN_LED, 255);
41    analogWrite(BLUE_LED, 255);
42
43    left_motor.attach(PD_2);
44    right_motor.attach(PD_3);
45
46    nh.initNode();
47    nh.subscribe(color_sub);
48    nh.subscribe(left_sub);
49    nh.subscribe(right_sub);
50}
51
52void loop(){
53    nh.spinOnce();
54    delay(10);
55}