collab/igvc2018
git clone https://git.julia.blue/collab/igvc2018
fbe4edd
documentation
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}