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

scoutos / scout_avr / src / main.cpp @ 6e7f0a98

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

Project

General

Profile

Colony » Colony Scout

scoutos / scout_avr / src / main.cpp @ 6e7f0a98