Robotics-IRobot

 view release on metacpan or  search on metacpan

lib/Robotics/IRobot.pm  view on Meta::CPAN

	$self->writeBytes(144,CORE::reverse(@{$self->{pwmState}}));
	print "Setting pwm loads: " . join(", ",@{$self->{pwmState}}) . "\n";
}

=item $robot->setPWMOnOff($lsd0, $lsd1, $lsd2)

Turns on and off low side drivers.  Values are boolean.

=cut

sub setPWMOnOff($$$$) {
	my $self=shift;
	
	my $lsd0=shift;
	my $lsd1=shift;
	my $lsd2=shift;
	
	my $byte=($lsd0 & 1) + ($lsd1 & 1) * 2 + ($lsd2 & 1) * 4;
	
	$self->writeBytes(138,$byte);
}

=item $robot->sendIR($byte)

Sends IR byte through LED hooked up to LSD1.

See Open Interface doc for details.

=cut

sub sendIR($$) {
	my $self=shift;
	
	my $irByte=shift;
	$self->writeBytes(151,$irByte);
}

=item $robot->setSongRaw($songId, @songBytes)

Sets song $songId in robot's memory to @songBytes.  
@songBytes can contain up to 16 notes.

See Open Interface doc for details.

=cut

sub setSongRaw($$@) {
	my $self=shift;
	my $songId=shift;
	
	print "setting song: " . $songId . ": ". join(", ",@_) , "\n";
	
	$self->writeBytes(140,$songId,(@_/2),@_);
}

=item $robot->playABCNotation($file, $callback)

Loads song in ABC format (see abcnotation.com) from $file and begins playing on Create.
If passed, calls &$callback($robot) when done.

I<NOTE:  You must either poll sensor values frequently or use the sensor data streaming methods
for this method to work properly.  Calling this method will overwrite any data contained in song banks 14 and 15>

=cut

sub playABCNotation($$$) {
	my $self=shift;
	my $file=shift;
	my $callback=shift;
	
	$self->playLongSongRaw($callback,loadABCNotation($file));
 
}

=item $robot->playLongSongRaw($callback, @songBytes)

Plays song contained in @songBytes (may be longer than 16 notes).  If passed, calls &$callback($robot) when done.

I<NOTE:  You must either poll sensor values frequently or use the sensor data streaming methods
for this method to work properly.  Calling this method will overwrite any data contained in song banks 14 and 15>

=cut

sub playLongSongRaw($$@) {
	my $self=shift;
	my $callback=shift;
 
	my @song=@_;
	
	#print "playing: " . join(", ",@song) . "\n";
	
	$self->setSongRaw(15,splice(@song,0,32));
	
	$self->addSensorEvent(300,sub{
			my $self=shift;
			my $listener=shift;
			
			if ($self->{sensorState}{songPlaying}) {
				$listener->{param}=$listener->{param}==15?-14:-15 if ($listener->{param}>0);
			} else {
				if ($listener->{param}<0) {
					$listener->{param}=-$listener->{param};
					return 1;
				}
			}
			
			return 0;
		},
		sub {
			my $self=shift;
			my $listener=shift;
			
			my $last=(@song==0);
			
			$self->playSong($listener->{param});
			print "Song length: " . (@song+0) . "\n";
			if ($last) {
				$self->removeSensorListener($listener->{id});
				&$callback($self) if ($callback);
			} else {
				$self->setSongRaw(($listener->{param}==15?14:15),splice(@song,0,32));
			}
			
			return 1;
		},-15,0);

}

=item IRobot::loadABCNotation($file)

Loads song in ABCNotation format from $file (see abcnotation.com).
Returns song in format defined in OI Interface.  If smaller than 16 notes (32 bytes)
can be passed to setSongRaw.  Otherwise resulting bytes can be passed to playLongSongRaw.

=cut
	

sub loadABCNotation($) {
	#my $self=shift;

lib/Robotics/IRobot.pm  view on Meta::CPAN

sub setSong($$$) {
	my $self=shift;
	
	my $songId=shift;
	my $song=shift;
	
	my $lastOctave=4;
	
	my @song;
	my $note;
	my $totalDuration=0;
	foreach $note (split(/\s/,lc($song))) {
		$note=~/(\d?)([a-gr]#?)(\d{1,3})/;
		my $octave=$1 || $lastOctave;
		my $letter=$2;
		my $duration=$3;
		
		my $num;
		if ($letter eq 'r') {
			$num=0;
		} else {
			$num=$notes->{$letter}+($octave+1)*12;
		}
		
		push @song,$num,$duration;
		
		$lastOctave=$octave;
		$totalDuration+=$duration;
	}
	
	$self->setSongRaw($songId,@song);
	
	return $totalDuration/64;
}

=item $robot->playSong($songId)

Plays song from bank $songId.

=cut

sub playSong($$) {
	my $self=shift;
	my $songId=shift;
	
	#print "playing song: $songId\n";
	
	$self->writeBytes(141,$songId);
}

=item $robot->turnTo($direction, $speed, $callback)

Attempts to turn the robot to face $direction relative to the direction it
was facing when $robot->init() was called or last $robot->markOrigin call.
Robot will make the turn at $speed.  Robot will stop once complete and call
&$callback($robot) if $callback is passed.

See section DEAD RECKONING for more information.

I<NOTE:  You must either poll sensor values frequently or use the sensor data streaming methods
for this method to work properly.>

=cut

sub turnTo($$$) {
	my $self=shift;
	my $angle=shift;
	my $speed=shift;
	my $callback=shift;
	
	$angle=_normalizeAngle($angle);
	my $direction=$self->{sensorState}{direction};
	my $delta=_normalizeAngle($angle-$direction);

	#print join(", ",$angle,$delta,$speed) . "\n";
	
	$self->waitAngle(200,$delta-8*$EPSILON*$speed/$WHEEL_WIDTH, sub {
		$self->stop();
		&$callback($self,$delta) if ($callback);
	});
	$self->rotateLeft(($delta<=>0)*$speed);
	
}

=item $robot->goTo($x, $y, $speed, $callback)

Attempts to drive the robot to position ($x,$y) relative to its location
when $robot->init() was called or last $robot->markOrigin call.
Robot will make the proceed at $speed.  Robot will stop once complete
and call &$callback($robot) if $callback is passed.

See section DEAD RECKONING for more information.

I<NOTE:  You must either poll sensor values frequently or use the sensor data streaming methods
for this method to work properly.>

=cut

sub goTo($$$$$) {
	my ($self,$destX,$destY,$speed,$callback)=@_;
	
	my $x=$self->{sensorState}{x};
	my $y=$self->{sensorState}{y};
	
	my $deltaX=$destX-$x;
	my $deltaY=$destY-$y;
	
	my ($distance,$angle)=cartesian_to_cylindrical($x,$y);
	$angle+=pip2; #so 0 is along +y axis
	
	$self->turnTo($angle,$speed,
		sub {
			$self->forward($speed);
			$self->waitDistance(200,$distance,
				sub {
					$self->stop();
					&$callback($self,$distance) if ($callback);
				}
			);
		}
	);
}

######Sensor Commands##########
=back

=head2 Sensors

The robot's sensor data can be retrieved in several different ways.  The easiest is 
to simply call $robot->refreshSensors on a regular basis.  This will retrieve all sensor
data from the robot, which can then be accessed from the hash returned by
$robot->getSensorState().  If you do not want all sensor data to be retrieved, then
you can use the $robot->getSensor($id) method.  This will only retrieve data for
one sensor (or sensor set) but, it is not recommended.

Consult the OI Interface document for more details on sensor ids.

Another method is to use the iRobot's sensor streaming functionality.  When the
robot is put in streaming mode it will send back sensor data once every 15ms.  Use the
$robot->startSteam, $robot->pauseStream. $robot->resumeStream method to start and
stop the stream.  The $robot->getStreamFrame method should be called at least every
15ms to read in the sensor data and update the sensor state hash.  As with the polling
method, you can pass a sensor ids to $robot->startStream to have the robot stream data
for only particular sensors, but again, this is not recommeded.

The third method is to use the event-driven approach.  Your program can register sensor listeners
or events to listen for using the $robot->addSensorListener, $robot->addSensorEvent,
$robot->runEvery, $robot->waitTime, $robot->waitDistance, and $robot->waitAngle methods.  Once these
have been registered the $robot->runSensorLoop and $robot->exitSensorLoop methods will put the robot in
streaming mode then read sensor data as it comes in while updating the sensor state hash and calling any
sensor listeners or events.

=over 4

=item $robot->getSensorState()

Returns a hash reference containing last read values from robot sensors.

=cut

sub getSensorState() {
	my $self=shift;
	
	return $self->{sensorState};
}

=item $robot->getDockSignal()

Returns an array indicating the presense of a "home base" docking station and any docking beacons seen.

Example:

	my ($dockPresent,$greenBeacon,$forceField,$redBeacon)=$robot->getDockSignal();

=cut

sub getDockSignal($) {
	my $self=shift;
	my $irByte=$self->{sensorState}{irByte};
	
	my $dock=(($irByte & 241)==240);
	
	#print join(", ",$irByte,($irByte & 241)) . "\r\n";
	
	if ($dock) {
		my $green=($irByte & 4) >> 2;
		my $red=($irByte & 8) >> 3;
		my $force=($irByte & 2) >> 1;
		
		return (1,$green,$force,$red);
	} else {
		return (0,0,0,0);
	}
}


=item $robot->getSensorLocation($sensor)

Gets the current location of a sensor relative to the origin.  Possible sensors:

=over 4

=item cliffLeft

=item cliffFrontLeft

=item cliffFrontRight

=item cliffRight

lib/Robotics/IRobot.pm  view on Meta::CPAN

	sleep($EPSILON - $sinceLastRefresh) if ($sinceLastRefresh < $EPSILON);
	
	$self->getSensor(6);
}

=item $robot->getSensor($sensorId)

Retreives data from a single sensor, refreshes sensor state hash, and triggers any sensor listeners or events.
This method is generally not recommedended.  $robot->refreshSensors() should be used instead.

If you are not polling the distance and angle sensors more than once every few seconds.  You may wish to switch the dead reckoning
mode to 'robot' or 'raw', as these may be more accurate in this situation.  See setMovementCorrectionMode method.

See OI Documentation for sensor ids.

=cut

sub getSensor($$) {
	my $self=shift;
	my $sensorId=shift;

	$self->writeBytes(142,$sensorId);
	my @data=$self->_readSensorData($sensorId);
	
	$self->_triggerSensorEvents([$sensorId]);
	
	return wantarray ? @data : $data[0];
}

=item $robot->getSensors($sensorId1, $sensorId2, ... )

Retrieves data for a particular sensor, refreshes sensor state hash, and triggers any sensor listeners or events.  This method is
generally not recommended.  $robot->refreshSensors() should be used instead.

If you are not polling the distance and angle sensors more than once every few seconds.  You may wish to switch the dead reckoning
mode to 'robot' or 'raw', as these may be more accurate in this situation.  See setMovementCorrectionMode method.

See OI Documentation for sensor ids.

=cut

sub getSensors($@) {
	my $self=shift;
	
	$self->writeBytes(149,(@_+0),@_);
	
	my @retArr;
	
	my $sensorId;
	foreach $sensorId (@_) {
		push @retArr,$self->_readSensorData($sensorId);
	}
	
	$self->_triggerSensorEvents(\@_);
	
	return @retArr;
}

=item $robot->runSensorLoop()

Begins streaming sensor data from the robot.  Updates sensor state hash every 15ms and triggers any
sensor listeners or events.  This method will block until $robot->exitSensorLoop() is called.

=cut

sub runSensorLoop($) {
	my $self=shift;
	
	$self->{exitLoop}=0;

	$self->startStream(6);

	while(!$self->{exitLoop}) {
		$self->getStreamFrame();
	}
	
	$self->pauseStream();
}

=item $robot->exitSensorLoop()

Stops streaming data from robot.  Causes any previous call to runSensorLoop to return.

=cut

sub exitSensorLoop($) {
	my $self=shift;

	$self->{exitLoop}=1;
}

=item $robot->startStream()

=item $robot->startStream($sensorId)

Puts robot into streaming mode.  If a $sensorId is passed only streams that sensor (not recommended).  Otherwises streams data from
all sensors.

See OI Documentation for more details

=cut

sub startStream($@) {
	my $self=shift;
	
	push @_,6 unless (@_ > 0);
	
	$self->writeBytes(148,(@_+0),@_);
	$self->{isStreaming}=1;
	
	$self->_syncStream();
}

=item $robot->pauseStream()

Pauses the sensor data stream.

=cut

sub pauseStream($) {
	my $self=shift;
	
	$self->writeBytes(150,0);
	$self->{isStreaming}=0;
}

=item $robot->resumeStream()

Resumes a previously paused sensor stream.

=cut

sub resumeStream($) {
	my $self=shift;
	
	$self->writeBytes(150,1);
	$self->{isStreaming}=1;
}

=item $robot->getStreamFrame()

Gets one frame of sensor data, updates sensor data hash, and triggers any sensor listeners or events.
Should be called at least once every 15ms.  Method will block
until one frame of sensor data has been read.

See OI Documentation for more details.

=cut

sub getStreamFrame($) {
	my $self=shift;
		
	my (@data,$readBytes);
	
	if ($self->{isStreaming}) {
		while ($data[0]!=19) {

lib/Robotics/IRobot.pm  view on Meta::CPAN

	
}

=item $robot->setMovementCorrectionMode($mode)

Sets the movementCorrection method used by the module.  Can be one of the following:

=over 5

=item calibration:
(default) Uses result of calibration to correct reported sensor values. This works best when robot is limited to straight movement and rotation in place.  See Sensor Calibration.

=item time:
Ignores angle sensor values and relies solely on requested movement and time.

=item robot:
Trusts value reported by robot angle sensor is accurate.  Assumes
sensor value is difference between distance traveled by left wheel
and distance travel by right wheel.  (This seems to actually be the case.)

=item raw:
Trusts value reported by robot angle sensor is accurate.  Assumes
sensor value is degrees rotated.  (This is the value reported by the OI
doc, but does not actually seem to be the case.)

=back

I<<Or>> you can pass your own sub to perform movement correction.  When called it will be passed $robot, $listener, and $sensorIds.
$sensorIds is a array ref containing the read sensorIds. $listener is a hash containing the details of the sensor listener used to calculate
indirect sensor values.  See addSensorListener for more details.  The sub must return a list containing actual distance traveled in mm
followed by actual angle rotated in radians.

=back

=cut

sub setMovementCorrectionMode($$) {
	my $self=shift;
	my $deadReckoner=shift;

	if ($deadReckoner eq 'calibration') {
		$deadReckoner=\&_correctiveDeadReckoning;
	} elsif ($deadReckoner eq 'time') {
		$deadReckoner=\&_timeDeadReckoning;
	} elsif ($deadReckoner eq 'robot') {
		$deadReckoner=\&_robotDeadReckoning;
	} elsif ($deadReckoner eq 'raw') {
		$deadReckoner=\&_rawDeadReckoning;
	} 
	
	$self->{deadReckoning}=$deadReckoner;
	
}

=head2 Closing Connection

=over 4

=item $robot->close()

Stops the robot motion and sensor streaming and closes communication port.

=back

=cut


sub close($) {
	my $self=shift;
	
	$self->stop();
	
	$self->stopTelemetry if ($self->{telem});
	
	if ($self->{replay}) {
                close $self->{replay};
        } else {
                $self->{port}->close();
        }

}

=head1 SENSORS

The sensor state hash can be retrieved from the $robot->getSensorState() method.  This only need be retrieved once
as subsequent updates will be made to the same hash.  Each direct and indirect sensor reading can be retrieved from this
hash using the keys below.

=head2 Direct Sensors

These are sensor values that are read directly from the robot.

=head3 Keys

=over 5

=item wheeldropCaster --
wheeldrop sensor on front caster (boolean)

=item wheeldropLeft --
wheeldrop sensor on left wheel (boolean)

=item wheeldropRight --
wheeldrop sensor on right wheel (boolean)

=item bumpLeft --
left bump sensor (boolean)

=item bumpRight --
right bump sensor (boolean)

=item wall --
physical wall sensor (boolean)

=item cliffLeft --
left cliff sensor (boolean)

=item cliffFrontLeft --
front-left cliff sensor (boolean)

=item cliffFrontRight --