collab/igvc2018
git clone https://git.julia.blue/collab/igvc2018
c9fbf70
imu
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 <std_msgs/Bool.h> 10#include <std_msgs/Int32.h> 11#include <Servo.h> 12 13int lpwm_pin = PD_2, 14 rpwm_pin = PD_3, 15 lenca_pin= PE_2, 16 lencb_pin= PE_3, 17 renca_pin= PA_2, 18 rencb_pin= PA_3; 19 20Servo left_motor; 21Servo right_motor; 22 23ros::NodeHandle nh; 24 25std_msgs::Int32 left_ticks; 26std_msgs::Int32 right_ticks; 27std_msgs::Bool zero; 28 29void color_cb(const std_msgs::ColorRGBA& msg){ 30 analogWrite(RED_LED, msg.r * msg.a); 31 analogWrite(GREEN_LED, msg.g * msg.a); 32 analogWrite(BLUE_LED, msg.b * msg.a); 33} 34 35//TODO: add watchdog 36void left_cb(const std_msgs::Float32& msg){ 37 left_motor.write(90 + msg.data * 90); 38} 39 40void right_cb(const std_msgs::Float32& msg){ 41 right_motor.write(90 + msg.data * 90); 42} 43 44void zero_cb(const std_msgs::Bool& msg){ 45 if(msg.data){ 46 left_ticks.data = 0; 47 right_ticks.data = 0; 48 } 49} 50 51ros::Subscriber < std_msgs::ColorRGBA > color_sub("led", &color_cb); 52ros::Subscriber < std_msgs::Bool > zero_sub("zero", &zero_cb); 53ros::Subscriber < std_msgs::Float32 > left_sub("left", &left_cb); 54ros::Subscriber < std_msgs::Float32 > right_sub("right", &right_cb); 55 56void left_encoder_int(){ 57 if(digitalRead(lencb_pin)){ 58 --left_ticks.data; 59 } 60 else{ 61 ++left_ticks.data; 62 } 63} 64 65void right_encoder_int(){ 66 if(digitalRead(rencb_pin)){ 67 --right_ticks.data; 68 } 69 else{ 70 ++right_ticks.data; 71 } 72} 73 74ros::Publisher left_pub("lticks", &left_ticks); 75ros::Publisher right_pub("rticks", &right_ticks); 76 77void setup(){ 78 pinMode(RED_LED, OUTPUT); 79 pinMode(GREEN_LED, OUTPUT); 80 pinMode(BLUE_LED, OUTPUT); 81 82 pinMode(lenca_pin, INPUT); 83 pinMode(lencb_pin, INPUT); 84 pinMode(renca_pin, INPUT); 85 pinMode(rencb_pin, INPUT); 86 87 left_ticks.data = 0; 88 right_ticks.data = 0; 89 attachInterrupt(lenca_pin, &left_encoder_int, RISING); 90 attachInterrupt(renca_pin, &right_encoder_int, RISING); 91 92 analogWrite(RED_LED, 255); 93 analogWrite(GREEN_LED, 255); 94 analogWrite(BLUE_LED, 255); 95 96 left_motor.attach(PD_2); 97 right_motor.attach(PD_3); 98 99 nh.initNode(); 100 nh.subscribe(color_sub); 101 nh.subscribe(left_sub); 102 nh.subscribe(right_sub); 103 104 nh.advertise(left_pub); 105 nh.advertise(right_pub); 106} 107 108void loop(){ 109 left_pub.publish(&left_ticks); 110 right_pub.publish(&right_ticks); 111 nh.spinOnce(); 112 delay(10); 113}