-
Notifications
You must be signed in to change notification settings - Fork 0
/
armDuino.ino
118 lines (92 loc) · 2.98 KB
/
armDuino.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#include <pac-man-arm.h>
#include <ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <behaviors/polar_coordinate.h>
/*
* This will control the arm on the pac-man-bot
*/
// CONSTANTS
/**********************************************************************/
#define ARM_BASE_HEIGHT 5
#define SENSOR_RADIUS 5
// initializer variables:
/*********************************************************************/
ros::NodeHandle nh;
std_msgs::Bool done;
std_msgs::Float64 distance;
float heading;
// init arm
/*********************************************************************/
pac_man_arm arm(SENSOR_RADIUS, ARM_BASE_HEIGHT);
// pubs
/*********************************************************************/
ros::Publisher pub_done ("/arm/done", &done);
ros::Publisher pub_distance("/arm/distance", &distance);
// angle funcs
/*********************************************************************/
float global_to_rel(float theta) {
float returner = theta;
if (theta > heading) {
returner = theta - heading;
} else if (theta < heading) {
returner = 360 - heading + theta;
}
return returner;
}
// callbacks
/*********************************************************************/
void heading_cb(const std_msgs::Float64 &val) {
heading = int(val.data) % 360;
}
/*
* All theta's will come in as an angle in relate to
* the body of the bot, have to convert from Global
* theta to relative theta
*/
void grabBlock_cb(const std_msgs::Float64& theta) {
float ang = global_to_rel(theta.data);
arm.grabBlock(ang);
done.data = true;
pub_done.publish(&done);
}
void scan_cb(const behaviors::polar_coordinate& point) {
float ang = global_to_rel(point.theta);
distance.data = arm.point(ang, point.r);
done.data =true;
pub_distance.publish(&distance);
pub_done.publish(&done);
}
void armReset_cb(const std_msgs::Bool& yes) {
if (yes.data) {
arm.setBaseAngle(0);
arm.setArmAngle(0);
}
done.data = true;
pub_done.publish(&done);
}
// subscribers
/*********************************************************************/
ros::Subscriber <std_msgs::Float64> sub_heading("heading", &heading_cb);
ros::Subscriber <std_msgs::Float64> sub_grabBlock("/arm/grabBlock", &grabBlock_cb);
ros::Subscriber <behaviors::polar_coordinate> sub_scan("/arm/distance", &scan_cb);
ros::Subscriber <std_msgs::Bool> sub_reset("/arm/reset", &armReset_cb);
// normal stuff
/*********************************************************************/
void setup() {
nh.initNode();
while (!nh.connected()) {
nh.spinOnce();
}
// create publishers and subscribers
nh.advertise( pub_done );
nh.advertise(pub_distance);
nh.subscribe(sub_heading);
nh.subscribe(sub_grabBlock);
nh.subscribe(sub_scan);
nh.subscribe(sub_reset);
}
void loop() {
// this one is all reactive, so everything happens in callbacks, leaving loop empty
// on purpose
}