-
Notifications
You must be signed in to change notification settings - Fork 0
/
driverDuino.ino
149 lines (124 loc) · 3.8 KB
/
driverDuino.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
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
#include <psuedo-tank.h>
#include <ros.h>
#include <behaviors/coordinate.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64.h>
/*
* driverDuino.ino
* This file drives the wheels.....duh
*
* It will accept inputs in the form of distances to move,
* and turn commands, as well as read in current pose
* and heading
*/
// CONSTANTS
/**********************************************************************/
// initializer variables:
/*********************************************************************/
float heading, x, y;
std_msgs::Bool doneMove;
bool overUnder; // if True, checking coords until greater than target,
// else it will check coords until less than target
bool check_x; // determines whether or not to check x or y. If true
// check x values, else y
float target_x, target_y;
ros::NodeHandle nh;
// init bot
/*********************************************************************/
psuedoTank bot (A1, A2, 13, A3, A4, 12, A2);
// pubs
/*********************************************************************/
ros::Publisher pub_done("/bot/doneMove", &doneMove);
// callbacks
/*********************************************************************/
void move_cb(const std_msgs::Float32& val) {
// moving in x or y
if (int(heading) == 0) {
// moving in y
check_x = false;
overUnder = true;
target_y = y + val.data;
} else if (int(heading) == 180) {
check_x = false;
overUnder = false;
target_y = y - val.data;
} else if (int(heading) == 90) {
check_x = true;
overUnder = true;
target_x = x + val.data;
} else {
check_x = true;
overUnder = false;
target_x = x - val.data;
}
// send move command
bot.forward();
doneMove.data = false;
}
void turnLeft_cb(const std_msgs::Bool & val) {
// send left command
bot.left();
doneMove.data = true;
pub_done.publish( &doneMove);
}
void turnRight_cb(const std_msgs::Bool & val) {
bot.right();
doneMove.data = true;
pub_done.publish( &doneMove);
}
void turn180_cb(const std_msgs::Bool & val) {
bot.left();
bot.left();
doneMove.data = true;
pub_done.publish( &doneMove);
}
void pose_cb(const behaviors::coordinate &loc) {
x = loc.X;
y = loc.Y;
}
void head_cb(const std_msgs::Float64 &head) {
heading = int(head.data) % 360;
}
// subscribers
/*********************************************************************/
ros::Subscriber <std_msgs::Float32> sub_move("/bot/move", &move_cb);
ros::Subscriber <std_msgs::Bool> sub_turnLeft("/bot/turnLeft", &turnLeft_cb);
ros::Subscriber <std_msgs::Bool> sub_trunRight("/bot/turnRight", &turnRight_cb);
ros::Subscriber <std_msgs::Bool> sub_turn180("/bot/turn180", &turn180_cb);
ros::Subscriber <behaviors::coordinate> sub_pose("position", &pose_cb);
ros::Subscriber <std_msgs::Float64> sub_heading("heading", &head_cb);
// normal stuff
/*********************************************************************/
void setup() {
nh.initNode();
while (!nh.connected()) {
nh.spinOnce();
}
// setup pubs and subs
nh.advertise(pub_done);
nh.subscribe(sub_move);
nh.subscribe(sub_turnLeft);
nh.subscribe(sub_trunRight);
nh.subscribe(sub_turn180);
nh.subscribe(sub_pose);
nh.subscribe(sub_heading);
}
void loop() {
// don't do anything unless set
if(!doneMove.data) {
if (overUnder ) {
if (check_x)doneMove.data = (x >= target_x);
else doneMove.data = (y >= target_y);
} else {
if(check_x) doneMove.data = (x <= target_x);
else doneMove.data = (y <= target_y);
}
if (doneMove.data) {
// if passed, then send stop signals
bot.brake();
pub_done.publish( &doneMove );
}
nh.spinOnce();
}
}