roomba 0.0.1

Sign up to get free protection for your applications and to get access to all the features.
@@ -0,0 +1,5 @@
1
+ README.rdoc
2
+ lib/**/*.rb
3
+ bin/*
4
+ features/**/*.feature
5
+ LICENSE
@@ -0,0 +1,5 @@
1
+ *.sw?
2
+ .DS_Store
3
+ coverage
4
+ rdoc
5
+ pkg
data/LICENSE ADDED
@@ -0,0 +1,20 @@
1
+ Copyright (c) 2009 Doug P.
2
+
3
+ Permission is hereby granted, free of charge, to any person obtaining
4
+ a copy of this software and associated documentation files (the
5
+ "Software"), to deal in the Software without restriction, including
6
+ without limitation the rights to use, copy, modify, merge, publish,
7
+ distribute, sublicense, and/or sell copies of the Software, and to
8
+ permit persons to whom the Software is furnished to do so, subject to
9
+ the following conditions:
10
+
11
+ The above copyright notice and this permission notice shall be
12
+ included in all copies or substantial portions of the Software.
13
+
14
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
15
+ EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
16
+ MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
17
+ NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
18
+ LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
19
+ OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
20
+ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
@@ -0,0 +1,21 @@
1
+ = roomba
2
+
3
+ Library to control a Roomba.
4
+
5
+ This code wraps the library found here:
6
+ http://roombahacking.com/roombahacks/roombacmd/
7
+
8
+ == Note on Patches/Pull Requests
9
+
10
+ * Fork the project.
11
+ * Make your feature addition or bug fix.
12
+ * Add tests for it. This is important so I don't break it in a
13
+ future version unintentionally.
14
+ * Commit, do not mess with rakefile, version, or history.
15
+ (if you want to have your own version, that is fine but
16
+ bump version in a commit by itself I can ignore when I pull)
17
+ * Send me a pull request. Bonus points for topic branches.
18
+
19
+ == Copyright
20
+
21
+ Copyright (c) 2010 Doug P. See LICENSE for details.
@@ -0,0 +1,48 @@
1
+ require 'rubygems'
2
+ require 'rake'
3
+
4
+ begin
5
+ require 'jeweler'
6
+ Jeweler::Tasks.new do |gem|
7
+ gem.name = "roomba"
8
+ gem.summary = %Q{Library to control a Roomba}
9
+ gem.description = %Q{Library to control a Roomba}
10
+ gem.email = "dougtko@gmail.com"
11
+ gem.homepage = "http://github.com/dougsko/roomba"
12
+ gem.authors = ["Doug P."]
13
+ gem.add_development_dependency "rspec"
14
+ # gem is a Gem::Specification... see http://www.rubygems.org/read/chapter/20 for additional settings
15
+ end
16
+ rescue LoadError
17
+ puts "Jeweler (or a dependency) not available. Install it with: sudo gem install jeweler"
18
+ end
19
+
20
+ require 'spec/rake/spectask'
21
+ Spec::Rake::SpecTask.new(:spec) do |spec|
22
+ spec.libs << 'lib' << 'spec'
23
+ spec.spec_files = FileList['spec/**/*_spec.rb']
24
+ end
25
+
26
+ Spec::Rake::SpecTask.new(:rcov) do |spec|
27
+ spec.libs << 'lib' << 'spec'
28
+ spec.pattern = 'spec/**/*_spec.rb'
29
+ spec.rcov = true
30
+ end
31
+
32
+ task :spec => :check_dependencies
33
+
34
+ task :default => :spec
35
+
36
+ require 'rake/rdoctask'
37
+ Rake::RDocTask.new do |rdoc|
38
+ if File.exist?('VERSION')
39
+ version = File.read('VERSION')
40
+ else
41
+ version = ""
42
+ end
43
+
44
+ rdoc.rdoc_dir = 'rdoc'
45
+ rdoc.title = "roomba #{version}"
46
+ rdoc.rdoc_files.include('README*')
47
+ rdoc.rdoc_files.include('lib/**/*.rb', 'ext/**/*.c')
48
+ end
data/VERSION ADDED
@@ -0,0 +1 @@
1
+ 0.0.1
@@ -0,0 +1,7 @@
1
+ #!/usr/bin/env ruby
2
+ #
3
+ #
4
+ #
5
+ require 'mkmf'
6
+
7
+ create_makefile("roomba")
@@ -0,0 +1,325 @@
1
+ #include "ruby.h"
2
+ #include "roombalib.h"
3
+
4
+ static VALUE cRoomba;
5
+
6
+ static void
7
+ roomba_destroy(Roomba *roomba)
8
+ {
9
+ roomba_free(roomba);
10
+ }
11
+
12
+ static VALUE
13
+ roomba_new(VALUE klass, VALUE rb_portname)
14
+ {
15
+ const char *portname;
16
+ struct Roomba *roomba;
17
+
18
+ portname = STR2CSTR(rb_portname);
19
+ roomba = roomba_init(portname);
20
+ return Data_Wrap_Struct(klass, 0, roomba_destroy, roomba);
21
+ }
22
+
23
+ static VALUE
24
+ close(VALUE self)
25
+ {
26
+ struct Roomba *roomba;
27
+
28
+ Data_Get_Struct(self, struct Roomba, roomba);
29
+ roomba_close(roomba);
30
+ return Qnil;
31
+ }
32
+
33
+ static VALUE
34
+ valid(VALUE self)
35
+ {
36
+ struct Roomba *roomba;
37
+
38
+ Data_Get_Struct(self, struct Roomba, roomba);
39
+ return INT2NUM(roomba_valid(roomba));
40
+ }
41
+
42
+ static VALUE get_portpath(VALUE self){
43
+ struct Roomba *roomba;
44
+
45
+ Data_Get_Struct(self, struct Roomba, roomba);
46
+ return rb_str_new2(roomba_get_portpath(roomba));
47
+ }
48
+
49
+ static VALUE
50
+ send(VALUE self, VALUE rb_cmd, VALUE rb_len)
51
+ {
52
+ struct Roomba *roomba;
53
+ const uint8_t *cmd;
54
+ int len;
55
+
56
+ cmd = STR2CSTR(rb_cmd);
57
+ len = NUM2INT(rb_len);
58
+ Data_Get_Struct(self, struct Roomba, roomba);
59
+
60
+ return INT2NUM(roomba_send(roomba, cmd, len));
61
+ }
62
+
63
+ static VALUE
64
+ drive(VALUE self, VALUE rb_vel, VALUE rb_rad)
65
+ {
66
+ struct Roomba *roomba;
67
+ int vel, rad;
68
+
69
+ vel = NUM2INT(rb_vel);
70
+ rad = NUM2INT(rb_rad);
71
+ Data_Get_Struct(self, struct Roomba, roomba);
72
+
73
+ roomba_drive(roomba, vel, rad);
74
+ return Qnil;
75
+ }
76
+
77
+ static VALUE
78
+ stop(VALUE self)
79
+ {
80
+ struct Roomba *roomba;
81
+
82
+ Data_Get_Struct(self, struct Roomba, roomba);
83
+ roomba_stop(roomba);
84
+ return Qnil;
85
+ }
86
+
87
+ static VALUE
88
+ forward(VALUE self)
89
+ {
90
+ struct Roomba *roomba;
91
+
92
+ Data_Get_Struct(self, struct Roomba, roomba);
93
+ roomba_forward(roomba);
94
+ return Qnil;
95
+ }
96
+
97
+ static VALUE
98
+ forward_at(VALUE self, VALUE rb_vel)
99
+ {
100
+ struct Roomba *roomba;
101
+ int vel;
102
+
103
+ vel = NUM2INT(rb_vel);
104
+ Data_Get_Struct(self, struct Roomba, roomba);
105
+ roomba_forward_at(roomba, vel);
106
+ return Qnil;
107
+ }
108
+
109
+ static VALUE
110
+ backward(VALUE self)
111
+ {
112
+ struct Roomba *roomba;
113
+
114
+ Data_Get_Struct(self, struct Roomba, roomba);
115
+ roomba_backward(roomba);
116
+ return Qnil;
117
+ }
118
+
119
+ static VALUE
120
+ backward_at(VALUE self, VALUE rb_vel)
121
+ {
122
+ struct Roomba *roomba;
123
+ int vel;
124
+
125
+ vel = NUM2INT(rb_vel);
126
+ Data_Get_Struct(self, struct Roomba, roomba);
127
+ roomba_backward_at(roomba, vel);
128
+ return Qnil;
129
+ }
130
+
131
+ static VALUE
132
+ spinleft(VALUE self)
133
+ {
134
+ struct Roomba *roomba;
135
+
136
+ Data_Get_Struct(self, struct Roomba, roomba);
137
+ roomba_spinleft(roomba);
138
+ return Qnil;
139
+ }
140
+
141
+ static VALUE
142
+ spinleft_at(VALUE self, VALUE rb_vel)
143
+ {
144
+ struct Roomba *roomba;
145
+ int vel;
146
+
147
+ vel = NUM2INT(rb_vel);
148
+ Data_Get_Struct(self, struct Roomba, roomba);
149
+ roomba_spinleft_at(roomba, vel);
150
+ return Qnil;
151
+ }
152
+
153
+ static VALUE
154
+ spinright(VALUE self)
155
+ {
156
+ struct Roomba *roomba;
157
+
158
+ Data_Get_Struct(self, struct Roomba, roomba);
159
+ roomba_spinright(roomba);
160
+ return Qnil;
161
+ }
162
+
163
+ static VALUE
164
+ spinright_at(VALUE self, VALUE rb_vel)
165
+ {
166
+ struct Roomba *roomba;
167
+ int vel;
168
+
169
+ vel = NUM2INT(rb_vel);
170
+ Data_Get_Struct(self, struct Roomba, roomba);
171
+ roomba_spinright_at(roomba, vel);
172
+ return Qnil;
173
+ }
174
+
175
+ static VALUE
176
+ set_velocity(VALUE self, VALUE rb_vel)
177
+ {
178
+ struct Roomba *roomba;
179
+ int vel;
180
+
181
+ vel = NUM2INT(rb_vel);
182
+ Data_Get_Struct(self, struct Roomba, roomba);
183
+ roomba_set_velocity(roomba, vel);
184
+ return Qnil;
185
+ }
186
+
187
+ static VALUE
188
+ get_velocity(VALUE self)
189
+ {
190
+ struct Roomba *roomba;
191
+
192
+ Data_Get_Struct(self, struct Roomba, roomba);
193
+ return INT2NUM(roomba_get_velocity(roomba));
194
+ }
195
+
196
+ static VALUE
197
+ play_note(VALUE self, VALUE rb_note, VALUE rb_duration)
198
+ {
199
+ struct Roomba *roomba;
200
+ uint8_t note, duration;
201
+
202
+ note = NUM2INT(rb_note);
203
+ duration = NUM2INT(rb_duration);
204
+ Data_Get_Struct(self, struct Roomba, roomba);
205
+ roomba_play_note(roomba, note, duration);
206
+ return Qnil;
207
+ }
208
+
209
+ static VALUE
210
+ set_motors(VALUE self, VALUE rb_mainbrush, VALUE rb_vacuum,
211
+ VALUE rb_sidebrush)
212
+ {
213
+ struct Roomba *roomba;
214
+ uint8_t mainbrush, vacuum, sidebrush;
215
+
216
+ mainbrush = NUM2INT(rb_mainbrush);
217
+ vacuum = NUM2INT(rb_vacuum);
218
+ sidebrush = NUM2INT(rb_sidebrush);
219
+ Data_Get_Struct(self, struct Roomba, roomba);
220
+ roomba_set_motors(roomba, mainbrush, vacuum, sidebrush);
221
+ return Qnil;
222
+ }
223
+
224
+ static VALUE
225
+ set_leds(VALUE self, VALUE rb_status_green, VALUE rb_status_red,
226
+ VALUE rb_spot, VALUE rb_clean, VALUE rb_max, VALUE rb_dirt,
227
+ VALUE rb_power_color, VALUE rb_power_intensity)
228
+ {
229
+ struct Roomba *roomba;
230
+ uint8_t status_green, status_red, spot, clean, max, dirt;
231
+ uint8_t power_color, power_intensity;
232
+
233
+ status_green = NUM2INT(rb_status_green);
234
+ status_red = NUM2INT(rb_status_red);
235
+ spot = NUM2INT(rb_spot);
236
+ clean = NUM2INT(rb_clean);
237
+ max = NUM2INT(rb_max);
238
+ dirt = NUM2INT(rb_dirt);
239
+ power_color = NUM2INT(rb_power_color);
240
+ power_intensity = NUM2INT(rb_power_intensity);
241
+ Data_Get_Struct(self, struct Roomba, roomba);
242
+ roomba_set_leds(roomba, status_green, status_red, spot, clean,
243
+ max, dirt, power_color, power_intensity);
244
+ return Qnil;
245
+ }
246
+
247
+ static VALUE
248
+ vacuum(VALUE self, VALUE rb_state)
249
+ {
250
+ struct Roomba *roomba;
251
+ uint8_t state;
252
+
253
+ state = NUM2INT(rb_state);
254
+ Data_Get_Struct(self, struct Roomba, roomba);
255
+ roomba_vacuum(roomba, state);
256
+ return Qnil;
257
+ }
258
+
259
+ static VALUE
260
+ read_sensors(VALUE self)
261
+ {
262
+ struct Roomba *roomba;
263
+
264
+ Data_Get_Struct(self, struct Roomba, roomba);
265
+ return INT2NUM(roomba_read_sensors(roomba));
266
+ }
267
+
268
+ static VALUE
269
+ print_sensors(VALUE self)
270
+ {
271
+ struct Roomba *roomba;
272
+
273
+ Data_Get_Struct(self, struct Roomba, roomba);
274
+ roomba_print_sensors(roomba);
275
+ return Qnil;
276
+ }
277
+
278
+ static VALUE
279
+ print_raw_sensors(VALUE self)
280
+ {
281
+ struct Roomba *roomba;
282
+
283
+ Data_Get_Struct(self, struct Roomba, roomba);
284
+ roomba_print_raw_sensors(roomba);
285
+ return Qnil;
286
+ }
287
+
288
+ static VALUE
289
+ delay(VALUE self, VALUE rb_millisecs)
290
+ {
291
+ int millisecs;
292
+
293
+ millisecs = NUM2INT(rb_millisecs);
294
+ roomba_delay(millisecs);
295
+ return Qnil;
296
+ }
297
+
298
+ void Init_roomba(){
299
+ cRoomba = rb_define_class("Roomba", rb_cObject);
300
+ rb_define_singleton_method(cRoomba, "new", roomba_new, 1);
301
+ rb_define_method(cRoomba, "close", close, 0);
302
+ rb_define_method(cRoomba, "valid", valid, 0);
303
+ rb_define_method(cRoomba, "get_portpath", get_portpath, 0);
304
+ rb_define_method(cRoomba, "send", send, 2);
305
+ rb_define_method(cRoomba, "drive", drive, 2);
306
+ rb_define_method(cRoomba, "stop", stop, 0);
307
+ rb_define_method(cRoomba, "forward", forward, 0);
308
+ rb_define_method(cRoomba, "forward_at", forward_at, 1);
309
+ rb_define_method(cRoomba, "backward", backward, 0);
310
+ rb_define_method(cRoomba, "backward_at", backward_at, 1);
311
+ rb_define_method(cRoomba, "spinleft", spinleft, 0);
312
+ rb_define_method(cRoomba, "spinleft_at", spinleft_at, 1);
313
+ rb_define_method(cRoomba, "spinright", spinright, 0);
314
+ rb_define_method(cRoomba, "spinright_at", spinright_at, 1);
315
+ rb_define_method(cRoomba, "set_velocity", set_velocity, 1);
316
+ rb_define_method(cRoomba, "get_velocity", get_velocity, 0);
317
+ rb_define_method(cRoomba, "play_note", play_note, 2);
318
+ rb_define_method(cRoomba, "set_motors", set_motors, 3);
319
+ rb_define_method(cRoomba, "set_leds", set_leds, 8);
320
+ rb_define_method(cRoomba, "vacuum", vacuum, 1);
321
+ rb_define_method(cRoomba, "read_sensors", read_sensors, 0);
322
+ rb_define_method(cRoomba, "print_sensors", print_sensors, 0);
323
+ rb_define_method(cRoomba, "print_raw_sensors", print_raw_sensors, 0);
324
+ rb_define_method(cRoomba, "delay", delay, 0);
325
+ }
@@ -0,0 +1,302 @@
1
+ /*
2
+ * roombalib -- Roomba C API
3
+ *
4
+ * http://hackingroomba.com/
5
+ *
6
+ * Copyright (C) 2006, Tod E. Kurt, tod@todbot.com
7
+ *
8
+ * Updates:
9
+ * 14 Dec 2006 - added more functions to roombalib
10
+ */
11
+
12
+
13
+ #include <stdio.h> /* Standard input/output definitions */
14
+ #include <stdint.h> /* Standard types */
15
+ #include <stdlib.h> /* calloc, strtol */
16
+ #include <string.h> /* strcpy */
17
+ #include <unistd.h> /* UNIX standard function definitions */
18
+ #include <fcntl.h> /* File control definitions */
19
+ #include <errno.h> /* Error number definitions */
20
+ #include <termios.h> /* POSIX terminal control definitions */
21
+ #include <sys/ioctl.h>
22
+
23
+ #include "roombalib.h"
24
+
25
+ int roombadebug = 0;
26
+
27
+ // internal use only
28
+ int roomba_init_serialport( const char* serialport, speed_t baud );
29
+
30
+ Roomba* roomba_init( const char* portpath )
31
+ {
32
+ int fd = roomba_init_serialport( portpath, B57600 );
33
+ if( fd == -1 ) return NULL;
34
+ uint8_t cmd[1];
35
+
36
+ cmd[0] = 128; // START
37
+ int n = write(fd, cmd, 1);
38
+ if( n!=1 ) {
39
+ perror("open_port: Unable to write to port ");
40
+ return NULL;
41
+ }
42
+ roomba_delay(COMMANDPAUSE_MILLIS);
43
+
44
+ cmd[0] = 130; // CONTROL
45
+ n = write(fd, cmd, 1);
46
+ if( n!=1 ) {
47
+ perror("open_port: Unable to write to port ");
48
+ return NULL;
49
+ }
50
+ roomba_delay(COMMANDPAUSE_MILLIS);
51
+
52
+ Roomba* roomba = calloc( 1, sizeof(Roomba) );
53
+ roomba->fd = fd;
54
+ strcpy(roomba->portpath, portpath);
55
+ roomba->velocity = DEFAULT_VELOCITY;
56
+
57
+ return roomba;
58
+ }
59
+
60
+ void roomba_free( Roomba* roomba )
61
+ {
62
+ if( roomba!= NULL ) {
63
+ if( roomba->fd ) roomba_close( roomba );
64
+ free( roomba );
65
+ }
66
+ }
67
+
68
+ const char* roomba_get_portpath( Roomba* roomba )
69
+ {
70
+ return roomba->portpath;
71
+ }
72
+
73
+ void roomba_close( Roomba* roomba )
74
+ {
75
+ close( roomba->fd );
76
+ roomba->fd = 0;
77
+ }
78
+
79
+ // is this Roomba pointer valid (but not necc connected)
80
+ int roomba_valid( Roomba* roomba )
81
+ {
82
+ return (roomba!=NULL && roomba->fd != 0);
83
+ }
84
+
85
+ void roomba_set_velocity( Roomba* roomba, int vel )
86
+ {
87
+ roomba->velocity = vel;
88
+ }
89
+
90
+ int roomba_get_velocity( Roomba* roomba )
91
+ {
92
+ return roomba->velocity;
93
+ }
94
+
95
+ // send an arbitrary length roomba command
96
+ int roomba_send( Roomba* roomba, const uint8_t* cmd, int len )
97
+ {
98
+ int n = write( roomba->fd, cmd, len);
99
+ if( n!=len )
100
+ perror("roomba_send: couldn't write to roomba");
101
+ return (n!=len); // indicate error, can usually ignore
102
+ }
103
+
104
+ // Move Roomba with low-level DRIVE command
105
+ void roomba_drive( Roomba* roomba, int velocity, int radius )
106
+ {
107
+ uint8_t vhi = velocity >> 8;
108
+ uint8_t vlo = velocity & 0xff;
109
+ uint8_t rhi = radius >> 8;
110
+ uint8_t rlo = radius & 0xff;
111
+ if(roombadebug)
112
+ fprintf(stderr,"roomba_drive: %.2hhx %.2hhx %.2hhx %.2hhx\n",
113
+ vhi,vlo,rhi,rlo);
114
+ uint8_t cmd[] = { 137, vhi,vlo, rhi,rlo }; // DRIVE
115
+ int n = write(roomba->fd, cmd, 5);
116
+ if( n!=5 )
117
+ perror("roomba_drive: couldn't write to roomba");
118
+ }
119
+
120
+ void roomba_stop( Roomba* roomba )
121
+ {
122
+ roomba_drive( roomba, 0, 0 );
123
+ }
124
+
125
+ void roomba_forward( Roomba* roomba )
126
+ {
127
+ roomba_drive( roomba, roomba->velocity, 0x8000 ); // 0x8000 = straight
128
+ }
129
+ void roomba_forward_at( Roomba* roomba, int velocity )
130
+ {
131
+ roomba_drive( roomba, velocity, 0x8000 );
132
+ }
133
+
134
+ void roomba_backward( Roomba* roomba )
135
+ {
136
+ roomba_drive( roomba, -roomba->velocity, 0x8000 );
137
+ }
138
+ void roomba_backward_at( Roomba* roomba, int velocity )
139
+ {
140
+ roomba_drive( roomba, -velocity, 0x8000 );
141
+ }
142
+
143
+ void roomba_spinleft( Roomba* roomba )
144
+ {
145
+ roomba_drive( roomba, roomba->velocity, 1 );
146
+ }
147
+ void roomba_spinleft_at( Roomba* roomba, int velocity )
148
+ {
149
+ roomba_drive( roomba, velocity, 1 );
150
+ }
151
+
152
+ void roomba_spinright( Roomba* roomba )
153
+ {
154
+ roomba_drive( roomba, roomba->velocity, -1 );
155
+ }
156
+ void roomba_spinright_at( Roomba* roomba, int velocity )
157
+ {
158
+ roomba_drive( roomba, velocity, -1 );
159
+ }
160
+
161
+ void roomba_play_note( Roomba* roomba, uint8_t note, uint8_t duration )
162
+ {
163
+ uint8_t cmd[] = { 140, 15, 1, note, duration, // SONG, then
164
+ 141, 15 }; // PLAY
165
+ int n = write( roomba->fd, cmd, 7);
166
+ if( n!=7 )
167
+ perror("roomba_play_note: couldn't write to roomba");
168
+ }
169
+
170
+ // Turns on/off the non-drive motors (main brush, vacuum, sidebrush).
171
+ void roomba_set_motors( Roomba* roomba, uint8_t mainbrush, uint8_t vacuum, uint8_t sidebrush)
172
+ {
173
+ uint8_t cmd[] = { 138, // MOTORS
174
+ ((mainbrush?0x04:0)|(vacuum?0x02:0)|(sidebrush?0x01:0))};
175
+ int n = write( roomba->fd, cmd, 2);
176
+ if( n!=2 )
177
+ perror("roomba_set_motors: couldn't write to roomba");
178
+ }
179
+
180
+ // Turns on/off the various LEDs.
181
+ void roomba_set_leds( Roomba* roomba, uint8_t status_green, uint8_t status_red,
182
+ uint8_t spot, uint8_t clean, uint8_t max, uint8_t dirt,
183
+ uint8_t power_color, uint8_t power_intensity )
184
+ {
185
+ uint8_t v = (status_green?0x20:0) | (status_red?0x10:0) |
186
+ (spot?0x08:0) | (clean?0x04:0) | (max?0x02:0) | (dirt?0x01:0);
187
+ uint8_t cmd[] = { 139, v, power_color, power_intensity }; // LEDS
188
+ int n = write( roomba->fd, cmd, 4);
189
+ if( n!=4 )
190
+ perror("roomba_set_leds: couldn't write to roomba");
191
+ }
192
+
193
+ // Turn all vacuum motors on or off according to state
194
+ void roomba_vacuum( Roomba* roomba, uint8_t state ) {
195
+ roomba_set_motors( roomba, state,state,state);
196
+ }
197
+
198
+ int roomba_read_sensors( Roomba* roomba )
199
+ {
200
+ uint8_t cmd[] = { 142, 0 }; // SENSOR, get all sensor data
201
+ int n = write( roomba->fd, cmd, 2);
202
+ roomba_delay(COMMANDPAUSE_MILLIS); //hmm, why isn't VMIN & VTIME working?
203
+ n = read( roomba->fd, roomba->sensor_bytes, 26);
204
+ if( n!=26 ) {
205
+ if(roombadebug)
206
+ fprintf(stderr,"roomba_read_sensors: not enough read (n=%d)\n",n);
207
+ return -1;
208
+ }
209
+ return 0;
210
+ }
211
+
212
+ void roomba_print_raw_sensors( Roomba* roomba )
213
+ {
214
+ uint8_t* sb = roomba->sensor_bytes;
215
+ int i;
216
+ for(i=0;i<26;i++) {
217
+ printf("%.2hhx ",sb[i]);
218
+ }
219
+ printf("\n");
220
+ }
221
+
222
+ void roomba_print_sensors( Roomba* roomba )
223
+ {
224
+ uint8_t* sb = roomba->sensor_bytes;
225
+ printf("bump: %x %x\n", bump_left(sb[0]), bump_right(sb[0]));
226
+ printf("wheeldrop: %x %x %x\n", wheeldrop_left(sb[0]),
227
+ wheeldrop_caster(sb[0]), wheeldrop_right(sb[0]));
228
+ printf("wall: %x\n", sb[1]);
229
+ printf("cliff: %x %x %x %x\n", sb[2],sb[3],sb[4],sb[5] );
230
+ printf("virtual_wall: %x\n", sb[6]);
231
+ printf("motor_overcurrents: %x %x %x %x %x\n", motorover_driveleft(sb[7]),
232
+ motorover_driveright(sb[7]), motorover_mainbrush(sb[7]),
233
+ motorover_sidebrush(sb[7]), motorover_vacuum(sb[7]));
234
+ printf("dirt: %x %x\n", sb[8],sb[9]);
235
+ printf("remote_opcode: %.2hhx\n", sb[10]);
236
+ printf("buttons: %.2hhx\n", sb[11]);
237
+ printf("distance: %.4x\n", (sb[12]<<8) | sb[13] );
238
+ printf("angle: %.4x\n", (sb[14]<<8) | sb[15] );
239
+ printf("charging_state: %.2hhx\n", sb[16]);
240
+ printf("voltage: %d\n", (sb[17]<<8) | sb[18] );
241
+ printf("current: %d\n", ((int8_t)sb[19]<<8) | sb[20] );
242
+ printf("temperature: %d\n", sb[21]);
243
+ printf("charge: %d\n", (sb[22]<<8) | sb[23] );
244
+ printf("capacity: %d\n", (sb[24]<<8) | sb[25] );
245
+ }
246
+
247
+ // 100,000 us == 100 ms == 0.1s
248
+ void roomba_delay( int millisecs )
249
+ {
250
+ usleep( millisecs * 1000 );
251
+ }
252
+
253
+
254
+
255
+ // private
256
+ // returns valid fd, or -1 on error
257
+ int roomba_init_serialport( const char* serialport, speed_t baud )
258
+ {
259
+ struct termios toptions;
260
+ int fd;
261
+
262
+ if(roombadebug)
263
+ fprintf(stderr,"roomba_init_serialport: opening port %s\n",serialport);
264
+
265
+ fd = open( serialport, O_RDWR | O_NOCTTY | O_NDELAY );
266
+ if (fd == -1) { // Could not open the port.
267
+ perror("roomba_init_serialport: Unable to open port ");
268
+ return -1;
269
+ }
270
+
271
+ if (tcgetattr(fd, &toptions) < 0) {
272
+ perror("roomba_init_serialport: Couldn't get term attributes");
273
+ return -1;
274
+ }
275
+
276
+ cfsetispeed(&toptions, baud);
277
+ cfsetospeed(&toptions, baud);
278
+
279
+ // 8N1
280
+ toptions.c_cflag &= ~PARENB;
281
+ toptions.c_cflag &= ~CSTOPB;
282
+ toptions.c_cflag &= ~CSIZE;
283
+ toptions.c_cflag |= CS8;
284
+ // no flow control
285
+ toptions.c_cflag &= ~CRTSCTS;
286
+
287
+ toptions.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines
288
+ toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl
289
+
290
+ toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
291
+ toptions.c_oflag &= ~OPOST; // make raw
292
+
293
+ toptions.c_cc[VMIN] = 26;
294
+ toptions.c_cc[VTIME] = 2; // FIXME: not sure about this
295
+
296
+ if( tcsetattr(fd, TCSANOW, &toptions) < 0) {
297
+ perror("roomba_init_serialport: Couldn't set term attributes");
298
+ return -1;
299
+ }
300
+
301
+ return fd;
302
+ }
@@ -0,0 +1,118 @@
1
+ /*
2
+ * roombalib -- Roomba C API
3
+ *
4
+ * http://hackingroomba.com/
5
+ *
6
+ * Copyright (C) 2006, Tod E. Kurt, tod@todbot.com
7
+ *
8
+ * Updates:
9
+ * 14 Dec 2006 - added more functions to roombalib
10
+ */
11
+
12
+
13
+ #include <stdint.h> /* Standard types */
14
+
15
+ #define DEFAULT_VELOCITY 200
16
+ #define COMMANDPAUSE_MILLIS 100
17
+
18
+ // holds all the per-roomba info
19
+ // consider it an opaque blob, please
20
+ typedef struct Roomba_struct {
21
+ int fd;
22
+ char portpath[80];
23
+ uint8_t sensor_bytes[26];
24
+ int velocity;
25
+ } Roomba;
26
+
27
+ // set to non-zero to see debugging output
28
+ extern int roombadebug;
29
+
30
+ // given a serial port name, create a Roomba object and return it
31
+ // or return NULL on error
32
+ Roomba* roomba_init( const char* portname );
33
+
34
+ // frees the memory of the Roomba object created with roomba_init
35
+ // will close the serial port if it's open
36
+ void roomba_free( Roomba* roomba );
37
+
38
+ // close the serial port connected to the Roomba
39
+ void roomba_close( Roomba* roomba );
40
+
41
+ // is this Roomba pointer valid (but not necc connected)
42
+ int roomba_valid( Roomba* roomba );
43
+
44
+ // return the serial port path for the given roomba
45
+ const char* roomba_get_portpath( Roomba* roomba );
46
+
47
+ // send an arbitrary length roomba command
48
+ int roomba_send( Roomba* roomba, const uint8_t* cmd, int len );
49
+
50
+ // Move Roomba with low-level DRIVE command
51
+ void roomba_drive( Roomba* roomba, int velocity, int radius );
52
+
53
+ // stop the Roomba
54
+ void roomba_stop( Roomba* roomba );
55
+
56
+ // Move Roomba forward at current velocity
57
+ void roomba_forward( Roomba* roomba );
58
+ void roomba_forward_at( Roomba* roomba, int velocity );
59
+
60
+ // Move Roomba backward at current velocity
61
+ void roomba_backward( Roomba* roomba );
62
+ void roomba_backward_at( Roomba* roomba, int velocity );
63
+
64
+ // Spin Roomba left at current velocity
65
+ void roomba_spinleft( Roomba* roomba );
66
+ void roomba_spinleft_at( Roomba* roomba, int velocity );
67
+
68
+ // Spin Roomba right at current velocity
69
+ void roomba_spinright( Roomba* roomba );
70
+ void roomba_spinright_at( Roomba* roomba, int velocity );
71
+
72
+ // Set current velocity for higher-level movement commands
73
+ void roomba_set_velocity( Roomba* roomba, int velocity );
74
+
75
+ // Get current velocity for higher-level movement commands
76
+ int roomba_get_velocity( Roomba* roomba );
77
+
78
+ // play a musical note
79
+ void roomba_play_note( Roomba* roomba, uint8_t note, uint8_t duration );
80
+
81
+ // Turns on/off the non-drive motors (main brush, vacuum, sidebrush).
82
+ void roomba_set_motors( Roomba* roomba, uint8_t mainbrush, uint8_t vacuum, uint8_t sidebrush);
83
+
84
+ // Turns on/off the various LEDs.
85
+ void roomba_set_leds( Roomba* roomba, uint8_t status_green, uint8_t status_red,
86
+ uint8_t spot, uint8_t clean, uint8_t max, uint8_t dirt,
87
+ uint8_t power_color, uint8_t power_intensity );
88
+
89
+ // Turn all vacuum motors on or off according to state
90
+ void roomba_vacuum( Roomba* roomba, uint8_t state );
91
+
92
+ // Get the sensor data from the Roomba
93
+ // returns -1 on failure
94
+ int roomba_read_sensors( Roomba* roomba );
95
+
96
+ // print existing sensor data nicely
97
+ void roomba_print_sensors( Roomba* roomba );
98
+
99
+ // print existing sensor data as string of hex chars
100
+ void roomba_print_raw_sensors( Roomba* roomba );
101
+
102
+ // utility function
103
+ void roomba_delay( int millisecs );
104
+ #define roomba_wait roomba_delay
105
+
106
+ // some simple macros of bit manipulations
107
+ #define bump_right(b) ((b & 0x01)!=0)
108
+ #define bump_left(b) ((b & 0x02)!=0)
109
+ #define wheeldrop_right(b) ((b & 0x04)!=0)
110
+ #define wheeldrop_left(b) ((b & 0x08)!=0)
111
+ #define wheeldrop_caster(b) ((b & 0x10)!=0)
112
+
113
+ #define motorover_sidebrush(b) ((b & 0x01)!=0)
114
+ #define motorover_vacuum(b) ((b & 0x02)!=0)
115
+ #define motorover_mainbrush(b) ((b & 0x04)!=0)
116
+ #define motorover_driveright(b) ((b & 0x08)!=0)
117
+ #define motorover_driveleft(b) ((b & 0x10)!=0)
118
+
File without changes
@@ -0,0 +1,7 @@
1
+ require File.expand_path(File.dirname(__FILE__) + '/spec_helper')
2
+
3
+ describe "Roomba" do
4
+ it "fails" do
5
+ fail "hey buddy, you should probably rename this file and start specing for real"
6
+ end
7
+ end
@@ -0,0 +1,9 @@
1
+ $LOAD_PATH.unshift(File.dirname(__FILE__))
2
+ $LOAD_PATH.unshift(File.join(File.dirname(__FILE__), '..', 'lib'))
3
+ require 'roomba'
4
+ require 'spec'
5
+ require 'spec/autorun'
6
+
7
+ Spec::Runner.configure do |config|
8
+
9
+ end
metadata ADDED
@@ -0,0 +1,94 @@
1
+ --- !ruby/object:Gem::Specification
2
+ name: roomba
3
+ version: !ruby/object:Gem::Version
4
+ hash: 29
5
+ prerelease: false
6
+ segments:
7
+ - 0
8
+ - 0
9
+ - 1
10
+ version: 0.0.1
11
+ platform: ruby
12
+ authors:
13
+ - Doug P.
14
+ autorequire:
15
+ bindir: bin
16
+ cert_chain: []
17
+
18
+ date: 2010-08-28 00:00:00 -04:00
19
+ default_executable:
20
+ dependencies:
21
+ - !ruby/object:Gem::Dependency
22
+ name: rspec
23
+ prerelease: false
24
+ requirement: &id001 !ruby/object:Gem::Requirement
25
+ none: false
26
+ requirements:
27
+ - - ">="
28
+ - !ruby/object:Gem::Version
29
+ hash: 3
30
+ segments:
31
+ - 0
32
+ version: "0"
33
+ type: :development
34
+ version_requirements: *id001
35
+ description: Library to control a Roomba
36
+ email: dougtko@gmail.com
37
+ executables: []
38
+
39
+ extensions:
40
+ - ext/extconf.rb
41
+ extra_rdoc_files:
42
+ - LICENSE
43
+ - README.rdoc
44
+ files:
45
+ - .document
46
+ - .gitignore
47
+ - LICENSE
48
+ - README.rdoc
49
+ - Rakefile
50
+ - VERSION
51
+ - ext/extconf.rb
52
+ - ext/roomba.c
53
+ - ext/roombalib.c
54
+ - ext/roombalib.h
55
+ - lib/roomba.rb
56
+ - spec/roomba_spec.rb
57
+ - spec/spec_helper.rb
58
+ has_rdoc: true
59
+ homepage: http://github.com/dougsko/roomba
60
+ licenses: []
61
+
62
+ post_install_message:
63
+ rdoc_options:
64
+ - --charset=UTF-8
65
+ require_paths:
66
+ - lib
67
+ required_ruby_version: !ruby/object:Gem::Requirement
68
+ none: false
69
+ requirements:
70
+ - - ">="
71
+ - !ruby/object:Gem::Version
72
+ hash: 3
73
+ segments:
74
+ - 0
75
+ version: "0"
76
+ required_rubygems_version: !ruby/object:Gem::Requirement
77
+ none: false
78
+ requirements:
79
+ - - ">="
80
+ - !ruby/object:Gem::Version
81
+ hash: 3
82
+ segments:
83
+ - 0
84
+ version: "0"
85
+ requirements: []
86
+
87
+ rubyforge_project:
88
+ rubygems_version: 1.3.7
89
+ signing_key:
90
+ specification_version: 3
91
+ summary: Library to control a Roomba
92
+ test_files:
93
+ - spec/roomba_spec.rb
94
+ - spec/spec_helper.rb