collab/igvc2018

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

Joseph RyanMerge updated motor codec9fbf70

imu
2.5 KiB113 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 <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}