/scout_avr/src/main.cpp - Annotate - Colony Scout - Robotics Club

scoutos / scout_avr / src / main.cpp @ 47e26dee

History | View | Annotate | Download (2.34 KB)

1	6e7f0a98	Tom Mullins	#if 1 ///////////////////////////////////////////////
2	807483bf	Tom Mullins
3	49090532	Tom Mullins	#include "ros.h"
4	6e7f0a98	Tom Mullins	#include "bom.h"
5	47e26dee	Tom Mullins	#include "range.h"
6	cf115e3d	Tom Mullins	#include <std_msgs/Int16.h>
7	47e26dee	Tom Mullins	#include <util/delay.h>
8	88fb3a79	Tom Mullins
9	6e7f0a98	Tom Mullins	ros::Publisher *pbom_pub;
10	49090532	Tom Mullins
11	812788aa	Tom Mullins	void debug(const char *str)
12			{
13	31f4a032	Tom Mullins	}
14
15	cf115e3d	Tom Mullins	void callback(const std_msgs::Int16& msg)
16	49090532	Tom Mullins	{
17			}
18
19			int main()
20	88fb3a79	Tom Mullins	{
21	6e7f0a98	Tom Mullins	char i, id;
22	88fb3a79	Tom Mullins	ros::NodeHandle nh;
23	6e7f0a98	Tom Mullins	bom::bom bom_msg;
24	cf115e3d	Tom Mullins	ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
25	6e7f0a98	Tom Mullins	ros::Publisher bom_pub("bom", &bom_msg);
26	49090532	Tom Mullins
27	6e7f0a98	Tom Mullins	pbom_pub = &bom_pub;
28	88fb3a79	Tom Mullins
29			nh.initNode();
30	47e26dee	Tom Mullins	range_init();
31			bom_init();
32	49090532	Tom Mullins	nh.subscribe(test_in);
33	6e7f0a98	Tom Mullins	nh.advertise(bom_pub);
34	88fb3a79	Tom Mullins
35	6e7f0a98	Tom Mullins	id = 0;
36	88fb3a79	Tom Mullins	while (1)
37			{
38	6e7f0a98	Tom Mullins	id++;
39			if (id == 0x40) {
40			id = 0;
41			}
42			set_robot_id(id);
43	47e26dee	Tom Mullins
44			bom_send(id & 1);
45	6e7f0a98	Tom Mullins	for (i = 0; i < 4; i++) {
46			int msg = bom_get(i);
47			if (msg != BOM_NO_DATA) {
48			bom_msg.sender = bom_msg_get_robot_id(msg);
49			bom_msg.send_dir = bom_msg_get_dir(msg);
50			bom_msg.recv_dir = i;
51			bom_pub.publish(&bom_msg);
52			}
53			}
54	47e26dee	Tom Mullins
55	88fb3a79	Tom Mullins	nh.spinOnce();
56	47e26dee	Tom Mullins	_delay_ms(500);
57	88fb3a79	Tom Mullins	}
58
59			return 0;
60	49090532	Tom Mullins	}
61	88fb3a79	Tom Mullins
62	807483bf	Tom Mullins	#else ///////////////////////////////////////////////
63
64			extern "C"
65	88fb3a79	Tom Mullins	{
66			#include <stdlib.h>
67			#include <string.h>
68	fd73d758	Tom Mullins	#include <avr/io.h>
69			#include <util/delay.h>
70	88fb3a79	Tom Mullins	}
71
72	49090532	Tom Mullins	#include "Atmega128rfa1.h"
73	1c3c96ce	Tom Mullins	#include "range.h"
74	f115416e	Tom Mullins	#include "bom.h"
75	49090532	Tom Mullins
76	31f4a032	Tom Mullins	Atmega128rfa1 avr;
77
78	812788aa	Tom Mullins	void debug(const char *str)
79			{
80	31f4a032	Tom Mullins	avr.puts(str);
81			}
82
83	88fb3a79	Tom Mullins	int main()
84			{
85	1c3c96ce	Tom Mullins	char buf[20];
86	f115416e	Tom Mullins	int i;
87	31f4a032	Tom Mullins	char id = 0;
88	cf115e3d	Tom Mullins	unsigned long now, next = 0;
89	88fb3a79	Tom Mullins	avr.init();
90	1c3c96ce	Tom Mullins	range_init();
91	f115416e	Tom Mullins	bom_init();
92	31f4a032	Tom Mullins	avr.puts("Hello!\r\n");
93	88fb3a79	Tom Mullins	while (1)
94			{
95	49090532	Tom Mullins	now = avr.time();
96			if (now > next) {
97	6e7f0a98	Tom Mullins	next = now + 500;
98	fd73d758	Tom Mullins	/*ultoa(now, buf, 10);
99	812788aa	Tom Mullins	avr.puts(buf);
100	fd73d758	Tom Mullins	avr.puts("\r\n");*/
101	31f4a032	Tom Mullins	id++;
102			if (id == 0x40) {
103			id = 0;
104			}
105			set_robot_id(id);
106	f115416e	Tom Mullins	/*utoa(range_get(0), buf, 10);
107			avr.puts("Range: ");
108			avr.puts(buf);
109			avr.puts(", ");
110	807483bf	Tom Mullins	utoa(range_get(1), buf, 10);
111	f115416e	Tom Mullins	avr.puts(buf);*/
112	fd73d758	Tom Mullins	for (i = 0; i < 4; i++) {
113	f115416e	Tom Mullins	bom_send(i);
114			int msg = bom_get(i);
115			if (msg != BOM_NO_DATA) {
116			avr.puts("BOM ");
117			itoa(i, buf, 10);
118			avr.puts(buf);
119			avr.puts(": ");
120	31f4a032	Tom Mullins	itoa(bom_msg_get_robot_id(msg), buf, 10);
121			avr.puts(buf);
122			avr.puts(" (");
123			itoa(bom_msg_get_dir(msg), buf, 10);
124	f115416e	Tom Mullins	avr.puts(buf);
125	31f4a032	Tom Mullins	avr.puts(")\r\n");
126	f115416e	Tom Mullins	}
127	fd73d758	Tom Mullins	}
128	49090532	Tom Mullins	}
129	88fb3a79	Tom Mullins	}
130			return 0;
131	cf115e3d	Tom Mullins	}
132	807483bf	Tom Mullins
133			#endif //////////////////////////////////////////////

Project

General

Profile

Colony » Colony Scout

scoutos / scout_avr / src / main.cpp @ 47e26dee